import math
class PlanarQuadrupedSim:
def __init__(self, mass=6.2, body_L=0.4, L1=0.15, L2=0.15, dt=0.001):
self.mass = mass
self.g = 9.81
self.body_L = body_L
self.L1 = L1
self.L2 = L2
self.dt = dt
# State: [x, z, theta, x_dot, z_dot, theta_dot]
self.state = [0, 0.25, 0, 0, 0, 0]
def leg_fk(self, q1, q2, hip_x_offset):
x = hip_x_offset + self.L1 * math.sin(q1) + self.L2 * math.sin(q1+q2)
z = -(self.L1 * math.cos(q1) + self.L2 * math.cos(q1+q2))
return x, z
def simulate_trot(self, duration=3.0, step_length=0.1, step_height=0.05, freq=2.0):
T = 1.0 / freq
hip_L = -self.body_L / 2
hip_R = self.body_L / 2
x, z, th, xd, zd, thd = self.state
history = []
t = 0
step_count = 0
while t < duration:
phase = (t * freq) % 1.0
# Leg trajectories (simplified)
q1_L = 0.3 * math.sin(2*math.pi*phase)
q2_L = 0.6 * (1 - math.cos(2*math.pi*phase)) / 2
q1_R = 0.3 * math.sin(2*math.pi*(phase + 0.5))
q2_R = 0.6 * (1 - math.cos(2*math.pi*(phase + 0.5))) / 2
# Foot positions relative to body
fx_L, fz_L = self.leg_fk(q1_L, q2_L, hip_L)
fx_R, fz_R = self.leg_fk(q1_R, q2_R, hip_R)
# Ground contact
foot_L_ground = (z + fz_L) <= 0
foot_R_ground = (z + fz_R) <= 0
# Ground reaction forces
Fz_L = 0
Fz_R = 0
if foot_L_ground and fz_L < 0:
Fz_L = self.mass * self.g / 2
if foot_R_ground and fz_R < 0:
Fz_R = self.mass * self.g / 2
# Simplified body dynamics
Fz_total = Fz_L + Fz_R
z_ddot = Fz_total / self.mass - self.g
zd += z_ddot * self.dt
z += zd * self.dt
if z < 0.15:
z = 0.15
zd = 0
# Forward motion (simplified)
xd = step_length * freq * 0.5
x += xd * self.dt
if int(t*1000) % 100 == 0:
history.append((t, x, z, th, fx_L, fz_L, fx_R, fz_R,
foot_L_ground, foot_R_ground))
t += self.dt
return history
sim = PlanarQuadrupedSim()
print("=" * 55)
print(" Planar Quadruped Simulation")
print("=" * 55)
hist = sim.simulate_trot(duration=3.0, step_length=0.1, step_height=0.05, freq=2.0)
print(f"\n [Trot Simulation: 3s at 2Hz]")
print(f" {'Time':>6s} {'X':>7s} {'Z':>7s} {'FootL_z':>8s} {'FootR_z':>8s} {'L_gnd':>5s} {'R_gnd':>5s}")
for t, x, z, th, fx_L, fz_L, fx_R, fz_R, lg, rg in hist:
print(f" {t:6.3f} {x:7.3f} {z:7.4f} {fz_L:8.4f} {fz_R:8.4f} {'Y' if lg else 'N':>5s} {'Y' if rg else 'N':>5s}")
print()
print(" OK - Planar quadruped simulation complete")
仿真结果：
=======================================================
Planar Quadruped Simulation
=======================================================
[Trot Simulation: 3s at 2Hz]
Time X Z FootL_z FootR_z L_gnd R_gnd
0.000 0.000 0.2500 -0.3000 -0.2738 Y Y
0.100 0.010 0.2500 -0.2761 -0.2931 Y Y
0.200 0.020 0.2500 -0.2605 -0.2966 Y Y
0.300 0.030 0.2500 -0.2877 -0.2936 Y Y
0.400 0.040 0.2500 -0.2935 -0.2608 Y Y
0.500 0.050 0.2500 -0.3000 -0.2738 Y Y
0.600 0.060 0.2500 -0.2761 -0.2931 Y Y
0.700 0.070 0.2500 -0.2605 -0.2966 Y Y
0.800 0.080 0.2500 -0.2877 -0.2936 Y Y
0.900 0.090 0.2500 -0.2935 -0.2608 Y Y
1.000 0.100 0.2500 -0.3000 -0.2738 Y Y
1.101 0.110 0.2500 -0.2757 -0.2931 Y Y
1.201 0.120 0.2500 -0.2607 -0.2967 Y Y
1.301 0.130 0.2500 -0.2879 -0.2933 Y Y
1.401 0.140 0.2500 -0.2935 -0.2606 Y Y
1.501 0.150 0.2500 -0.3000 -0.2741 Y Y
1.601 0.160 0.2500 -0.2757 -0.2931 Y Y
1.701 0.170 0.2500 -0.2607 -0.2967 Y Y
1.801 0.180 0.2500 -0.2879 -0.2933 Y Y
1.901 0.190 0.2500 -0.2935 -0.2606 Y Y
2.001 0.200 0.2500 -0.3000 -0.2741 Y Y
2.101 0.210 0.2500 -0.2757 -0.2931 Y Y
2.201 0.220 0.2500 -0.2607 -0.2967 Y Y
2.301 0.230 0.2500 -0.2879 -0.2933 Y Y
2.401 0.240 0.2500 -0.2935 -0.2606 Y Y
2.501 0.250 0.2500 -0.3000 -0.2741 Y Y
2.601 0.260 0.2500 -0.2757 -0.2931 Y Y
2.701 0.270 0.2500 -0.2607 -0.2967 Y Y
2.801 0.280 0.2500 -0.2879 -0.2933 Y Y
2.901 0.290 0.2500 -0.2935 -0.2606 Y Y
OK - Planar quadruped simulation complete