🤖 台阶攀爬

import math class StairClimber: def __init__(self, mass=6.2, body_L=0.4, body_W=0.2, L_thigh=0.15, L_calf=0.15): self.mass = mass self.g = 9.81 self.body_L = body_L self.body_W = body_W self.L_thigh = L_thigh self.L_calf = L_calf self.max_leg = L_thigh + L_calf def check_step_feasibility(self, step_height, step_depth): max_h = self.max_leg * 0.85 min_d = 0.05 return step_height <= max_h and step_depth >= min_d def plan_climbing_gait(self, n_steps, step_height, step_depth): plan = [] foot_positions = { 'LF': [0, self.body_W/2, 0], 'RF': [0, -self.body_W/2, 0], 'LB': [-self.body_L/2, self.body_W/2, 0], 'RB': [-self.body_L/2, -self.body_W/2, 0], } for step in range(n_steps): phase = [] swing_order = ['LF', 'RF', 'LB', 'RB'] for leg in swing_order: old_pos = foot_positions[leg][:] if leg in ['LF', 'RF']: new_z = (step + 1) * step_height new_x = (step + 1) * step_depth else: new_z = (step + 1) * step_height new_x = step * step_depth + step_depth/2 new_pos = [new_x, foot_positions[leg][1], new_z] phase.append((leg, old_pos, new_pos)) foot_positions[leg] = new_pos plan.append(phase) return plan def compute_required_torque(self, step_height): # Knee torque when lifting body up one step h = step_height d = self.body_L / 2 # horizontal distance from hip to CoM angle = math.atan2(h, d) F_vertical = self.mass * self.g * 0.75 # 3-leg support tau_knee = F_vertical * self.L_calf * math.sin(angle) tau_hip = F_vertical * self.L_thigh * math.cos(angle) return tau_hip, tau_knee sc = StairClimber() print("=" * 55) print(" Stair Climbing Simulation") print("=" * 55) # Feasibility check print("\n [Step Feasibility Check]") test_steps = [(0.10, 0.25), (0.15, 0.25), (0.20, 0.30), (0.25, 0.25), (0.30, 0.20)] for h, d in test_steps: feasible = sc.check_step_feasibility(h, d) print(f" h={h*100:.0f}cm, d={d*100:.0f}cm: {'FEASIBLE' if feasible else 'INFEASIBLE'}") # Climbing plan for 3 steps print("\n [3-Step Climbing Plan (h=15cm, d=25cm)]") plan = sc.plan_climbing_gait(3, 0.15, 0.25) for step_i, phase in enumerate(plan): print(f"\n Step {step_i+1}:") for leg, old, new in phase: dz = new[2] - old[2] dx = new[0] - old[0] print(f" {leg}: ({old[0]:.3f},{old[2]:.3f}) -> ({new[0]:.3f},{new[2]:.3f}) " f"dz={dz*100:.0f}cm dx={dx*100:.0f}cm") # Torque requirements print("\n [Torque Requirements]") for h in [0.05, 0.10, 0.15, 0.20, 0.25]: tau_h, tau_k = sc.compute_required_torque(h) print(f" h={h*100:.0f}cm: hip_tau={tau_h:.1f}Nm, knee_tau={tau_k:.1f}Nm") # Stability analysis during climbing print("\n [Stability During Climbing]") for step_i in range(3): z_front = (step_i + 1) * 0.15 z_back = step_i * 0.15 com_z = (z_front * 2 + z_back * 2) / 4 tilt = math.atan2(z_front - z_back, sc.body_L) * 180 / math.pi print(f" Step {step_i+1}: front_z={z_front*100:.0f}cm back_z={z_back*100:.0f}cm " f"tilt={tilt:.1f}deg") print() print(" OK - Stair climbing simulation complete")