import math
class LoadWalkingSim:
def __init__(self, robot_mass=6.2, body_L=0.4, body_W=0.2):
self.robot_mass = robot_mass
self.g = 9.81
self.body_L = body_L
self.body_W = body_W
def compute_com_shift(self, load_mass, load_offset_x, load_offset_y):
total_mass = self.robot_mass + load_mass
com_x = load_mass * load_offset_x / total_mass
com_y = load_mass * load_offset_y / total_mass
return com_x, com_y
def required_force_per_leg(self, load_mass, n_support=4):
total = (self.robot_mass + load_mass) * self.g
return total / n_support
def stability_with_load(self, load_mass, load_offset_x):
total = self.robot_mass + load_mass
com_x = load_mass * load_offset_x / total
margin_fwd = self.body_L/2 - com_x
margin_back = self.body_L/2 + com_x
return margin_fwd, margin_back
def simulate_loaded_walk(self, load_mass=2.0, load_offset=(0.1, 0),
duration=3.0, dt=0.01):
total_mass = self.robot_mass + load_mass
com_shift_x = load_mass * load_offset[0] / total_mass
com_shift_y = load_mass * load_offset[1] / total_mass
x = 0
z = 0.2
speed = 0.3 * (1 - load_mass / 20) # slower with load
t = 0
history = []
step_phase = 0
step_freq = 1.5 * (1 - load_mass / 30)
while t < duration:
step_phase = (t * step_freq) % 1.0
x += speed * dt
z = 0.2 + 0.003 * math.sin(2*math.pi*step_freq*t)
# Ground reaction forces (4 legs)
f_per_leg = total_mass * self.g / 4
if int(t*100) % 30 == 0:
history.append((t, x, speed, f_per_leg, com_shift_x, com_shift_y))
t += dt
return history
lws = LoadWalkingSim()
print("=" * 55)
print(" Load Walking Simulation")
print("=" * 55)
# CoM shift analysis
print(f"\n [CoM Shift with Load]")
for load, ox in [(0, 0), (1, 0.1), (2, 0.1), (3, 0.15), (5, 0.2)]:
cx, cy = lws.compute_com_shift(load, ox, 0)
mf, mb = lws.stability_with_load(load, ox)
print(f" load={load}kg @ {ox}m: CoM_shift={cx*1000:.1f}mm, "
f"fwd_margin={mf*1000:.1f}mm back_margin={mb*1000:.1f}mm "
f"{'OK' if mf>0 and mb>0 else 'UNSTABLE'}")
# Force per leg
print(f"\n [Force per Leg]")
for load in [0, 1, 2, 3, 5]:
f4 = lws.required_force_per_leg(load, 4)
f3 = lws.required_force_per_leg(load, 3)
print(f" load={load}kg: 4-leg={f4:.1f}N, 3-leg={f3:.1f}N, "
f"4-leg/3-leg ratio={f4/lws.required_force_per_leg(0,4):.2f}x")
# Simulation
print(f"\n [Loaded Walk: 2kg at (0.1, 0)]")
hist = lws.simulate_loaded_walk(load_mass=2.0, load_offset=(0.1, 0))
for t, x, speed, fpl, cx, cy in hist:
print(f" t={t:.2f}s x={x:.3f}m speed={speed:.3f}m/s F/leg={fpl:.1f}N CoM_off={cx*1000:.1f}mm")
print()
print(" OK - Load walking simulation complete")
仿真结果：
=======================================================
Load Walking Simulation
=======================================================
[CoM Shift with Load]
load=0kg @ 0m: CoM_shift=0.0mm, fwd_margin=200.0mm back_margin=200.0mm OK
load=1kg @ 0.1m: CoM_shift=13.9mm, fwd_margin=186.1mm back_margin=213.9mm OK
load=2kg @ 0.1m: CoM_shift=24.4mm, fwd_margin=175.6mm back_margin=224.4mm OK
load=3kg @ 0.15m: CoM_shift=48.9mm, fwd_margin=151.1mm back_margin=248.9mm OK
load=5kg @ 0.2m: CoM_shift=89.3mm, fwd_margin=110.7mm back_margin=289.3mm OK
[Force per Leg]
load=0kg: 4-leg=15.2N, 3-leg=20.3N, 4-leg/3-leg ratio=1.00x
load=1kg: 4-leg=17.7N, 3-leg=23.5N, 4-leg/3-leg ratio=1.16x
load=2kg: 4-leg=20.1N, 3-leg=26.8N, 4-leg/3-leg ratio=1.32x
load=3kg: 4-leg=22.6N, 3-leg=30.1N, 4-leg/3-leg ratio=1.48x
load=5kg: 4-leg=27.5N, 3-leg=36.6N, 4-leg/3-leg ratio=1.81x
[Loaded Walk: 2kg at (0.1, 0)]
t=0.00s x=0.003m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=0.30s x=0.084m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=0.60s x=0.165m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=0.90s x=0.246m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=1.20s x=0.327m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=1.50s x=0.408m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=1.80s x=0.489m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=2.11s x=0.572m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=2.41s x=0.653m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
t=2.71s x=0.734m speed=0.270m/s F/leg=20.1N CoM_off=24.4mm
OK - Load walking simulation complete