import math
class CoMController:
def __init__(self, mass=6.2, body_L=0.4, body_W=0.2):
self.mass = mass
self.g = 9.81
self.body_L = body_L
self.body_W = body_W
self.legs = {
'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 compute_com(self, foot_positions, contact_flags):
total_force = [0, 0]
for leg, pos in foot_positions.items():
if contact_flags.get(leg, False):
total_force[0] += pos[0]
total_force[1] += pos[1]
n = sum(1 for v in contact_flags.values() if v)
if n == 0:
return (0, 0)
return (total_force[0]/n, total_force[1]/n)
def com_trajectory_planning(self, target_com, current_com, dt=0.01, kp=10.0):
trajectory = []
com = list(current_com)
vel = [0, 0]
for i in range(200):
fx = kp * (target_com[0] - com[0]) - 3.0 * vel[0]
fy = kp * (target_com[1] - com[1]) - 3.0 * vel[1]
ax = fx / self.mass
ay = fy / self.mass
vel[0] += ax * dt
vel[1] += ay * dt
com[0] += vel[0] * dt
com[1] += vel[1] * dt
trajectory.append((i*dt, com[0], com[1]))
if abs(com[0]-target_com[0]) < 0.001 and abs(com[1]-target_com[1]) < 0.001:
break
return trajectory
def swing_shift_strategy(self, swing_leg):
# Shift CoM away from swing leg to maintain stability
leg_pos = self.legs[swing_leg]
# Target: centroid of remaining support legs
support = {l: p for l, p in self.legs.items() if l != swing_leg}
cx = sum(p[0] for p in support.values()) / len(support)
cy = sum(p[1] for p in support.values()) / len(support)
return (cx, cy)
ctrl = CoMController()
print("=" * 55)
print(" Center of Mass Control Simulation")
print("=" * 55)
# CoM shift for each swing leg
print("\n [CoM Shift Strategy for Walk Gait]")
for swing in ['LF', 'RF', 'LB', 'RB']:
target = ctrl.swing_shift_strategy(swing)
current = (0, 0)
shift = (target[0]-current[0], target[1]-current[1])
print(f" Lift {swing}: target CoM=({target[0]:.4f},{target[1]:.4f}), shift=({shift[0]*1000:.1f},{shift[1]*1000:.1f})mm")
# CoM trajectory tracking
print("\n [CoM Trajectory Tracking]")
targets = [(0.05, 0), (-0.05, 0), (0, 0.03), (0, -0.03)]
for target in targets:
traj = ctrl.com_trajectory_planning(target, (0, 0))
t_final = traj[-1][0]
x_final, y_final = traj[-1][1], traj[-1][2]
overshoot = 0
for t, x, y in traj:
d = math.sqrt(x**2 + y**2)
target_d = math.sqrt(target[0]**2 + target[1]**2)
if d > target_d * 1.1:
overshoot = max(overshoot, d - target_d)
print(f" Target ({target[0]*1000:.0f},{target[1]*1000:.0f})mm: "
f"reached ({x_final*1000:.1f},{y_final*1000:.1f})mm in {t_final:.3f}s, "
f"overshoot={overshoot*1000:.1f}mm")
# Walk gait CoM trajectory
print("\n [Walk Gait Full CoM Trajectory]")
swing_order = ['LF', 'RF', 'LB', 'RB']
com_pos = [0, 0]
for phase, swing in enumerate(swing_order):
target = ctrl.swing_shift_strategy(swing)
traj = ctrl.com_trajectory_planning(target, tuple(com_pos), kp=15.0)
com_pos = [traj[-1][1], traj[-1][2]]
print(f" Phase {phase} (lift {swing}): CoM -> ({com_pos[0]*1000:.1f},{com_pos[1]*1000:.1f})mm")
print()
print(" OK - CoM control simulation complete")
仿真结果：
=======================================================
Center of Mass Control Simulation
=======================================================
[CoM Shift Strategy for Walk Gait]
Lift LF: target CoM=(-0.0667,-0.0333), shift=(-66.7,-33.3)mm
Lift RF: target CoM=(-0.0667,0.0333), shift=(-66.7,33.3)mm
Lift LB: target CoM=(0.0667,-0.0333), shift=(66.7,-33.3)mm
Lift RB: target CoM=(0.0667,0.0333), shift=(66.7,33.3)mm
[CoM Trajectory Tracking]
Target (50,0)mm: reached (49.2,0.0)mm in 1.380s, overshoot=0.0mm
Target (-50,0)mm: reached (-49.2,0.0)mm in 1.380s, overshoot=0.0mm
Target (0,30)mm: reached (0.0,29.3)mm in 1.370s, overshoot=0.0mm
Target (0,-30)mm: reached (0.0,-29.3)mm in 1.370s, overshoot=0.0mm
[Walk Gait Full CoM Trajectory]
Phase 0 (lift LF): CoM -> (-66.1,-33.0)mm
Phase 1 (lift RF): CoM -> (-66.7,32.7)mm
Phase 2 (lift LB): CoM -> (67.0,-33.5)mm
Phase 3 (lift RB): CoM -> (66.7,32.7)mm
OK - CoM control simulation complete