🤖 扰动恢复

import math, random class DisturbanceRecovery: def __init__(self, mass=6.2, Ixx=0.05, Iyy=0.1, Izz=0.1, body_L=0.4, body_W=0.2, z_com=0.2): self.mass = mass self.Ixx, self.Iyy, self.Izz = Ixx, Iyy, Izz self.body_L = body_L self.body_W = body_W self.z_com = z_com self.g = 9.81 def capture_point(self, com_pos, com_vel): # Capture point: where to step to stop omega = math.sqrt(self.g / self.z_com) x_cp = com_pos[0] + com_vel[0] / omega y_cp = com_pos[1] + com_vel[1] / omega return (x_cp, y_cp) def tip_over_analysis(self, impulse_force, direction, duration=0.1): # Simplified: angular impulse = F * d * dt if direction == 'lateral': arm = self.body_W / 2 I = self.Ixx else: arm = self.body_L / 2 I = self.Iyy angular_impulse = impulse_force * arm * duration omega = angular_impulse / I # Energy to recover E_recover = 0.5 * I * omega**2 # Max tilt angle tilt_max = omega # approximate return omega, E_recover, tilt_max def simulate_recovery(self, push_force=50, push_duration=0.1, direction='lateral', kp=100, kd=20): # Initial push if direction == 'lateral': arm = self.body_W / 2 I = self.Ixx else: arm = self.body_L / 2 I = self.Iyy angular_impulse = push_force * arm * push_duration omega_init = angular_impulse / I tilt = 0 tilt_dot = omega_init dt = 0.001 trajectory = [] t = 0 recovered = False max_tilt = 0 while t < 3.0: # PD recovery tau = kp * (0 - tilt) - kd * tilt_dot # Gravity torque gravity_tau = -self.mass * self.g * self.z_com * math.sin(tilt) # Max available torque from legs max_tau = 4 * 30 * 0.1 # 4 legs * max_force * arm tau = max(-max_tau, min(max_tau, tau)) tilt_ddot = (tau + gravity_tau) / I tilt_dot += tilt_ddot * dt tilt += tilt_dot * dt max_tilt = max(max_tilt, abs(tilt)) if abs(tilt) < 0.01 and abs(tilt_dot) < 0.01 and t > 0.5: recovered = True trajectory.append((t, tilt, tau)) t += dt return trajectory, max_tilt, recovered dr = DisturbanceRecovery() print("=" * 55) print(" Disturbance Recovery Simulation") print("=" * 55) # Capture point calculation print("\n [Capture Point Analysis]") for vx in [0.5, 1.0, 1.5, 2.0]: for vy in [0, 0.5]: cp = dr.capture_point((0, 0), (vx, vy)) print(f" v=({vx:.1f},{vy:.1f})m/s -> CP=({cp[0]:.4f},{cp[1]:.4f})m") # Tip-over analysis print("\n [Tip-over Analysis]") for force in [20, 50, 100, 200]: for direction in ['lateral', 'sagittal']: omega, E, tilt = dr.tip_over_analysis(force, direction) print(f" F={force:3d}N {direction:9s}: omega={omega:.2f}rad/s, " f"E={E:.2f}J, max_tilt={tilt*180/math.pi:.1f}deg") # Recovery simulation print("\n [Recovery Simulation]") for force in [30, 50, 80, 120]: traj, max_tilt, recovered = dr.simulate_recovery(push_force=force, direction='lateral') status = "RECOVERED" if recovered else "FALLEN" print(f" Push {force:3d}N lateral: max_tilt={max_tilt*180/math.pi:.1f}deg [{status}]") # Show trajectory for i in range(0, min(len(traj), 500), max(1, len(traj)//6)): t, tilt, tau = traj[i] if i % 80 == 0 or i == len(traj)-1: print(f" t={t:.3f}s tilt={tilt*180/math.pi:+6.2f}deg tau={tau:+.2f}Nm") # Maximum push recovery print("\n [Maximum Push Recovery Capability]") for direction in ['lateral', 'sagittal']: max_force = 0 for force in range(10, 300, 10): _, _, recovered = dr.simulate_recovery(push_force=force, direction=direction, kp=100) if recovered: max_force = force else: break print(f" {direction}: max recoverable push = {max_force}N") print() print(" OK - Disturbance recovery simulation complete")