import math
class JumpController:
def __init__(self, mass=6.2, max_leg_force=150, leg_length=0.3):
self.mass = mass
self.g = 9.81
self.max_force = max_leg_force
self.leg_length = leg_length
def compute_jump_params(self, target_height):
v_needed = math.sqrt(2 * self.g * target_height)
impulse = self.mass * v_needed
push_time = impulse / self.max_force
push_distance = 0.5 * (self.max_force/self.mass - self.g) * push_time**2
return v_needed, impulse, push_time, push_distance
def simulate_jump(self, target_height=0.1, dt=0.0001):
v_needed, impulse, push_time, push_dist = self.compute_jump_params(target_height)
z = 0
z_dot = 0
phase = 'push'
t = 0
history = []
peak_height = 0
while t < 2.0:
if phase == 'push':
F = self.max_force
z_ddot = F / self.mass - self.g
z_dot += z_ddot * dt
z += z_dot * dt
if z_dot >= v_needed:
phase = 'flight'
elif phase == 'flight':
z_ddot = -self.g
z_dot += z_ddot * dt
z += z_dot * dt
peak_height = max(peak_height, z)
if z <= 0 and z_dot < 0:
phase = 'landing'
z = 0
elif phase == 'landing':
# Spring-damper landing
F = 2000 * (-z) + 100 * (-z_dot)
if F < 0: F = 0
z_ddot = F / self.mass - self.g
z_dot += z_ddot * dt
z += z_dot * dt
if abs(z) < 0.001 and abs(z_dot) < 0.01:
break
if int(t*10000) % 200 == 0:
history.append((t, z, z_dot, phase))
t += dt
return history, peak_height
jc = JumpController()
print("=" * 55)
print(" Jump Control Simulation")
print("=" * 55)
# Jump parameters
print(f"\n [Jump Parameters]")
for h in [0.02, 0.05, 0.10, 0.15, 0.20, 0.30]:
v, imp, pt, pd = jc.compute_jump_params(h)
feasible = pd < jc.leg_length * 0.8
print(f" h={h*100:.0f}cm: v={v:.2f}m/s impulse={imp:.1f}Ns "
f"push_time={pt*1000:.1f}ms push_dist={pd*1000:.1f}mm {'OK' if feasible else 'INFEASIBLE'}")
# Jump simulation
print(f"\n [Jump Simulation: 10cm target]")
hist, peak = jc.simulate_jump(target_height=0.10)
for t, z, v, phase in hist:
print(f" t={t:.4f}s z={z*1000:.1f}mm v={v*1000:.1f}mm/s [{phase}]")
print(f" Peak height: {peak*100:.2f}cm")
# Max jump height
print(f"\n [Maximum Jump Height]")
max_h = 0
for h_cm in range(1, 50):
h = h_cm / 100.0
v, imp, pt, pd = jc.compute_jump_params(h)
if pd < jc.leg_length * 0.8 and pt > 0.001:
max_h = h
print(f" Max feasible jump: {max_h*100:.0f}cm (with max_force={jc.max_force}N)")
print()
print(" OK - Jump control simulation complete")
仿真结果：
=======================================================
Jump Control Simulation
=======================================================
[Jump Parameters]
h=2cm: v=0.63m/s impulse=3.9Ns push_time=25.9ms push_dist=4.8mm OK
h=5cm: v=0.99m/s impulse=6.1Ns push_time=40.9ms push_dist=12.1mm OK
h=10cm: v=1.40m/s impulse=8.7Ns push_time=57.9ms push_dist=24.1mm OK
h=15cm: v=1.72m/s impulse=10.6Ns push_time=70.9ms push_dist=36.2mm OK
h=20cm: v=1.98m/s impulse=12.3Ns push_time=81.9ms push_dist=48.2mm OK
h=30cm: v=2.43m/s impulse=15.0Ns push_time=100.3ms push_dist=72.3mm OK
[Jump Simulation: 10cm target]
t=0.0000s z=0.0mm v=1.4mm/s [push]
t=0.0201s z=2.9mm v=290.5mm/s [push]
t=0.0400s z=11.6mm v=576.8mm/s [push]
t=0.0600s z=26.0mm v=864.5mm/s [push]
t=0.0800s z=46.2mm v=1152.1mm/s [push]
t=0.1000s z=72.0mm v=1374.5mm/s [flight]
t=0.1200s z=97.6mm v=1178.3mm/s [flight]
t=0.1400s z=119.2mm v=982.1mm/s [flight]
t=0.1601s z=136.9mm v=784.9mm/s [flight]
t=0.1801s z=150.6mm v=588.7mm/s [flight]
t=0.2001s z=160.4mm v=392.5mm/s [flight]
t=0.2201s z=166.3mm v=196.3mm/s [flight]
t=0.2401s z=168.3mm v=0.1mm/s [flight]
t=0.2601s z=166.3mm v=-196.1mm/s [flight]
t=0.2801s z=160.4mm v=-392.3mm/s [flight]
t=0.3001s z=150.6mm v=-588.5mm/s [flight]
t=0.3201s z=136.8mm v=-784.7mm/s [flight]
t=0.3401s z=119.2mm v=-980.9mm/s [flight]
t=0.3601s z=97.6mm v=-1177.1mm/s [flight]
t=0.3801s z=72.1mm v=-1373.3mm/s [flight]
t=0.4001s z=42.6mm v=-1569.5mm/s [flight]
t=0.4201s z=9.3mm v=-1765.7mm/s [flight]
t=0.4401s z=-24.6mm v=-1504.2mm/s [landing]
t=0.4601s z=-50.1mm v=-1043.2mm/s [landing]
t=0.4801s z=-66.4mm v=-595.5mm/s [landing]
t=0.5001s z=-74.3mm v=-206.3mm/s [landing]
t=0.5201s z=-75.2mm v=98.0mm/s [landing]
t=0.5401s z=-71.0mm v=307.7mm/s [landing]
t=0.5601s z=-63.5mm v=425.9mm/s [landing]
t=0.5801s z=-54.4mm v=465.0mm/s [landing]
t=0.6001s z=-45.3mm v=442.6mm/s [landing]
t=0.6201s z=-37.0mm v=378.0mm/s [landing]
t=0.6401s z=-30.3mm v=289.9mm/s [landing]
t=0.6601s z=-25.5mm v=194.5mm/s [landing]
t=0.6801s z=-22.5mm v=104.2mm/s [landing]
t=0.7001s z=-21.2mm v=27.4mm/s [landing]
t=0.7201s z=-21.3mm v=-31.1mm/s [landing]
t=0.7401s z=-22.3mm v=-70.2mm/s [landing]
t=0.7601s z=-24.0mm v=-90.8mm/s [landing]
t=0.7801s z=-25.9mm v=-95.8mm/s [landing]
t=0.8001s z=-27.7mm v=-88.9mm/s [landing]
t=0.8201s z=-29.4mm v=-74.3mm/s [landing]
t=0.8401s z=-30.7mm v=-55.6mm/s [landing]
t=0.8601s z=-31.6mm v=-35.9mm/s [landing]
t=0.8801s z=-32.1mm v=-17.8mm/s [landing]
t=0.9001s z=-32.3mm v=-2.7mm/s [landing]
t=0.9201s z=-32.3mm v=8.5mm/s [landing]
t=0.9401s z=-32.0mm v=15.7mm/s [landing]
t=0.9601s z=-31.7mm v=19.2mm/s [landing]
t=0.9801s z=-31.3mm v=19.6mm/s [landing]
t=1.0001s z=-30.9mm v=17.8mm/s [landing]
t=1.0201s z=-30.6mm v=14.5mm/s [landing]
t=1.0401s z=-30.3mm v=10.6mm/s [landing]
t=1.0601s z=-30.1mm v=6.6mm/s [landing]
t=1.0801s z=-30.0mm v=2.9mm/s [landing]
t=1.1001s z=-30.0mm v=-0.0mm/s [landing]
t=1.1201s z=-30.0mm v=-2.1mm/s [landing]
t=1.1401s z=-30.1mm v=-3.4mm/s [landing]
t=1.1601s z=-30.2mm v=-4.0mm/s [landing]
t=1.1801s z=-30.3mm v=-4.0mm/s [landing]
t=1.2001s z=-30.3mm v=-3.5mm/s [landing]
t=1.2201s z=-30.4mm v=-2.8mm/s [landing]
t=1.2401s z=-30.4mm v=-2.0mm/s [landing]
t=1.2601s z=-30.5mm v=-1.2mm/s [landing]
t=1.2801s z=-30.5mm v=-0.5mm/s [landing]
t=1.3001s z=-30.5mm v=0.1mm/s [landing]
t=1.3201s z=-30.5mm v=0.5mm/s [landing]
t=1.3401s z=-30.5mm v=0.7mm/s [landing]
t=1.3601s z=-30.5mm v=0.8mm/s [landing]
t=1.3801s z=-30.4mm v=0.8mm/s [landing]
t=1.4001s z=-30.4mm v=0.7mm/s [landing]
t=1.4201s z=-30.4mm v=0.5mm/s [landing]
t=1.4401s z=-30.4mm v=0.4mm/s [landing]
t=1.4601s z=-30.4mm v=0.2mm/s [landing]
t=1.4801s z=-30.4mm v=0.1mm/s [landing]
t=1.5001s z=-30.4mm v=-0.0mm/s [landing]
t=1.5201s z=-30.4mm v=-0.1mm/s [landing]
t=1.5401s z=-30.4mm v=-0.2mm/s [landing]
t=1.5601s z=-30.4mm v=-0.2mm/s [landing]
t=1.5801s z=-30.4mm v=-0.2mm/s [landing]
t=1.6001s z=-30.4mm v=-0.1mm/s [landing]
t=1.6201s z=-30.4mm v=-0.1mm/s [landing]
t=1.6401s z=-30.4mm v=-0.1mm/s [landing]
t=1.6601s z=-30.4mm v=-0.0mm/s [landing]
t=1.6801s z=-30.4mm v=-0.0mm/s [landing]
t=1.7001s z=-30.4mm v=0.0mm/s [landing]
t=1.7201s z=-30.4mm v=0.0mm/s [landing]
t=1.7401s z=-30.4mm v=0.0mm/s [landing]
t=1.7601s z=-30.4mm v=0.0mm/s [landing]
t=1.7801s z=-30.4mm v=0.0mm/s [landing]
t=1.8001s z=-30.4mm v=0.0mm/s [landing]
t=1.8201s z=-30.4mm v=0.0mm/s [landing]
t=1.8401s z=-30.4mm v=0.0mm/s [landing]
t=1.8601s z=-30.4mm v=0.0mm/s [landing]
t=1.8801s z=-30.4mm v=0.0mm/s [landing]
t=1.9001s z=-30.4mm v=-0.0mm/s [landing]
t=1.9201s z=-30.4mm v=-0.0mm/s [landing]
t=1.9401s z=-30.4mm v=-0.0mm/s [landing]
t=1.9601s z=-30.4mm v=-0.0mm/s [landing]
t=1.9801s z=-30.4mm v=-0.0mm/s [landing]
Peak height: 16.83cm
[Maximum Jump Height]
Max feasible jump: 49cm (with max_force=150N)
OK - Jump control simulation complete
通过改变推力方向控制跳跃方向：
Fdirection = Ftotal · [sin(α), cos(α)]
α = atan2(vx,target, vz,target)
前跳：推力略向前倾；侧跳：左右腿不对称推力；旋转跳：加偏航力矩。