🤖 3D四足仿真

import math class Quadruped3DSim: def __init__(self, mass=6.2, body_L=0.4, body_W=0.2, dt=0.001): self.mass = mass self.g = 9.81 self.body_L = body_L self.body_W = body_W self.dt = dt self.hips = { 'LF': ( body_L/2, body_W/2), 'RF': ( body_L/2, -body_W/2), 'LB': (-body_L/2, body_W/2), 'RB': (-body_L/2, -body_W/2), } def trot_step(self, phase, step_l=0.1, step_h=0.05): trajectories = {} group_a = ['LF', 'RB'] group_b = ['RF', 'LB'] for leg, (hx, hy) in self.hips.items(): if leg in group_a: if phase < 0.5: x = hx + step_l * (2*phase - 0.5) z = step_h * 4 * (2*phase) * (1 - 2*phase) else: x = hx + step_l * (1.5 - 2*phase) z = 0 else: if phase < 0.5: x = hx + step_l * (0.5 - 2*phase) z = 0 else: x = hx + step_l * (2*phase - 1.5) z = step_h * 4 * (2*phase-1) * (2-2*phase) trajectories[leg] = (x, hy, z) return trajectories def simulate(self, duration=2.0, freq=2.0): T = 1.0 / freq x_body = 0 z_body = 0.2 roll = 0 pitch = 0 history = [] t = 0 while t < duration: phase = (t / T) % 1.0 traj = self.trot_step(phase) # Count ground contacts n_ground = sum(1 for v in traj.values() if v[2] < 0.001) # Simplified body dynamics speed = 0.1 * freq * 0.5 x_body += speed * self.dt # Small oscillations z_body = 0.2 + 0.005 * math.sin(2*math.pi*freq*t*2) roll = 0.02 * math.sin(2*math.pi*freq*t) pitch = 0.01 * math.sin(2*math.pi*freq*t + 0.5) if int(t*1000) % 100 == 0: history.append((t, x_body, z_body, roll, pitch, n_ground, traj)) t += self.dt return history sim = Quadruped3DSim() print("=" * 55) print(" 3D Quadruped Simulation") print("=" * 55) hist = sim.simulate(duration=2.0, freq=2.0) print(f"\n [3D Trot Simulation]") print(f" {'Time':>6s} {'X':>7s} {'Z':>7s} {'Roll':>7s} {'Pitch':>7s} {'Gnd':>3s}") for t, x, z, r, p, ng, traj in hist: print(f" {t:6.3f} {x:7.3f} {z:7.4f} {r*180/math.pi:+6.2f} {p*180/math.pi:+6.2f} {ng:3d}") print() print(" OK - 3D quadruped simulation complete")