🤖 柔顺着陆

import math class SoftLandingController: def __init__(self, mass=6.2, leg_stiffness=2000, leg_damping=50): self.mass = mass self.g = 9.81 self.k = leg_stiffness self.b = leg_damping def simulate_landing(self, drop_height=0.05, dt=0.0001, duration=0.5): z = drop_height z_dot = 0 leg_length = 0.3 # natural leg length leg_compression = 0 t = 0 history = [] peak_force = 0 while t < duration: if z <= 0: # Ground contact leg_compression = -z F_spring = self.k * leg_compression F_damp = self.b * (-z_dot) F_ground = F_spring + F_damp if F_ground < 0: F_ground = 0 # Can't pull ground z_ddot = F_ground / self.mass - self.g peak_force = max(peak_force, F_ground) else: z_ddot = -self.g F_ground = 0 z_dot += z_ddot * dt z += z_dot * dt if int(t*10000) % 500 == 0: history.append((t, z, z_dot, F_ground, leg_compression)) t += dt return history, peak_force def simulate_active_landing(self, drop_height=0.05, dt=0.0001, duration=0.5, target_force=None): if target_force is None: target_force = self.mass * self.g * 1.5 z = drop_height z_dot = 0 t = 0 history = [] peak_force = 0 active_k = self.k active_b = self.b while t < duration: if z <= 0: leg_compression = -z # Active damping: increase damping when force too high F_spring = active_k * leg_compression F_damp = active_b * (-z_dot) F_current = F_spring + F_damp if F_current > target_force: active_b = self.b * 3 # triple damping else: active_b = self.b F_ground = F_spring + active_b * (-z_dot) if F_ground < 0: F_ground = 0 z_ddot = F_ground / self.mass - self.g peak_force = max(peak_force, F_ground) else: z_ddot = -self.g F_ground = 0 active_b = self.b z_dot += z_ddot * dt z += z_dot * dt if int(t*10000) % 500 == 0: history.append((t, z, z_dot, F_ground)) t += dt return history, peak_force slc = SoftLandingController() print("=" * 55) print(" Soft Landing Simulation") print("=" * 55) # Passive landing from different heights print("\n [Passive Landing - varying drop height]") for h in [0.02, 0.05, 0.10, 0.15, 0.20]: hist, peak = slc.simulate_landing(drop_height=h) impact_speed = math.sqrt(2 * 9.81 * h) print(f" h={h*100:.0f}cm: impact={impact_speed:.2f}m/s, peak_F={peak:.1f}N, " f"ratio={peak/(6.2*9.81):.1f}x body_weight") # Active vs Passive print("\n [Active vs Passive Landing (h=10cm)]") hist_p, peak_p = slc.simulate_landing(drop_height=0.10) hist_a, peak_a = slc.simulate_active_landing(drop_height=0.10, target_force=6.2*9.81*2.0) print(f" Passive: peak_F={peak_p:.1f}N ({peak_p/(6.2*9.81):.1f}x)") print(f" Active: peak_F={peak_a:.1f}N ({peak_a/(6.2*9.81):.1f}x)") print(f" Reduction: {(1-peak_a/peak_p)*100:.1f}%") # Damping optimization print("\n [Damping Optimization (h=10cm)]") for b in [20, 50, 100, 200, 500]: slc2 = SoftLandingController(leg_damping=b) _, peak = slc2.simulate_landing(drop_height=0.10) print(f" b={b:3d}: peak_F={peak:.1f}N ({peak/(6.2*9.81):.1f}x)") # Stiffness effect print("\n [Stiffness Effect (h=10cm)]") for k in [500, 1000, 2000, 5000, 10000]: slc3 = SoftLandingController(leg_stiffness=k) hist, peak = slc3.simulate_landing(drop_height=0.10) settling = max(t for t, z, _, _, _ in hist if abs(z) > 0.001) print(f" k={k:5d}: peak_F={peak:.1f}N, settling~{settling:.3f}s") print() print(" OK - Soft landing simulation complete")