🤖 爬楼梯

import math class StairClimbingSim: def __init__(self, step_h=0.15, step_d=0.25, n_steps=5): self.step_h = step_h self.step_d = step_d self.n_steps = n_steps def get_terrain_height(self, x): if x < 0: return 0 step_idx = int(x / self.step_d) if step_idx >= self.n_steps: return self.n_steps * self.step_h return step_idx * self.step_h def plan_foot_sequence(self): plan = [] body_x = -0.1 body_z = 0.2 for stair in range(self.n_steps): for leg in ['LF', 'RF', 'LB', 'RB']: if leg in ['LF', 'RF']: target_x = (stair + 1) * self.step_d - 0.05 target_z = self.get_terrain_height(target_x) else: target_x = stair * self.step_d + 0.05 target_z = self.get_terrain_height(target_x) plan.append((stair, leg, target_x, target_z)) return plan def simulate_climb(self): plan = self.plan_foot_sequence() body_x = -0.1 body_z = 0.2 body_roll = 0 body_pitch = 0 results = [] for stair, leg, tx, tz in plan: body_x = (tx + body_x) / 2 body_z = tz + 0.2 # Pitch adjustment front_z = self.get_terrain_height(body_x + 0.2) back_z = self.get_terrain_height(body_x - 0.2) body_pitch = math.atan2(front_z - back_z, 0.4) results.append((stair, leg, tx, tz, body_x, body_z, body_pitch)) return results scs = StairClimbingSim(step_h=0.15, step_d=0.25, n_steps=5) print("=" * 55) print(" Stair Climbing Simulation") print("=" * 55) print(f" Step: h={scs.step_h*100:.0f}cm, d={scs.step_d*100:.0f}cm, n={scs.n_steps}") plan = scs.plan_foot_sequence() print(f"\n [Foot Placement Plan ({len(plan)} moves)]") for stair, leg, tx, tz in plan: print(f" Stair {stair+1}, {leg}: target=({tx:.3f}, {tz:.3f})") results = scs.simulate_climb() print(f"\n [Climbing Simulation Results]") for stair, leg, tx, tz, bx, bz, bp in results: print(f" Stair {stair+1} {leg}: foot=({tx:.3f},{tz:.3f}) body=({bx:.3f},{bz:.3f}) pitch={bp*180/math.pi:.1f}deg") print() print(" OK - Stair climbing simulation complete")