🤖 足端力控制

import math class FootForceController: def __init__(self, mass=6.2, body_L=0.4, body_W=0.2, z_com=0.2): self.mass = mass self.g = 9.81 self.body_L = body_L self.body_W = body_W self.z_com = z_com self.legs = { 'LF': ( body_L/2, body_W/2), 'RF': ( body_L/2, -body_W/2), 'LB': (-body_L/2, body_W/2), 'RB': (-body_L/2, -body_W/2), } def compute_desired_forces(self, com_acc, com_torque, contact_legs): n = len(contact_legs) if n == 0: return {} # Total force needed F_total = [self.mass * com_acc[0], self.mass * com_acc[1], self.mass * (com_acc[2] + self.g)] # Distribute equally f_per_leg = [F_total[i]/n for i in range(3)] forces = {} for leg in contact_legs: lx, ly = self.legs[leg] # Add torque compensation fx = f_per_leg[0] + com_torque[1] / (n * self.z_com) fy = f_per_leg[1] - com_torque[0] / (n * self.z_com) fz = f_per_leg[2] - com_torque[2] / (n * max(abs(lx), abs(ly), 0.01)) forces[leg] = (fx, fy, fz) return forces def friction_cone_check(self, forces, mu=0.6): valid = {} for leg, (fx, fy, fz) in forces.items(): ft = math.sqrt(fx**2 + fy**2) fn = fz if fn > 0: ratio = ft / fn valid[leg] = ratio <= mu else: valid[leg] = False return valid def simulate_stance_force_control(self, duration=1.0, dt=0.001, kp=200, kd=40): contact = ['LF', 'RF', 'LB', 'RB'] z_target = 0.2 z = z_target z_dot = 0 roll = 0 roll_dot = 0 pitch = 0 pitch_dot = 0 history = [] t = 0 while t < duration: # PD force control fz_err = kp * (z_target - z) - kd * z_dot roll_tau = 100 * (0 - roll) - 20 * roll_dot pitch_tau = 100 * (0 - pitch) - 20 * pitch_dot forces = self.compute_desired_forces( (0, 0, fz_err/self.mass), (roll_tau, pitch_tau, 0), contact ) # Simplified dynamics Fz_total = sum(f[2] for f in forces.values()) z_ddot = Fz_total / self.mass - self.g z_dot += z_ddot * dt z += z_dot * dt roll_ddot = roll_tau / 0.05 roll_dot += roll_ddot * dt roll += roll_dot * dt pitch_ddot = pitch_tau / 0.1 pitch_dot += pitch_ddot * dt pitch += pitch_dot * dt if int(t*1000) % 200 == 0: history.append((t, z, roll, pitch, forces)) t += dt return history ffc = FootForceController() print("=" * 55) print(" Foot Force Control Simulation") print("=" * 55) # Force distribution test print("\n [Force Distribution - 4 legs standing]") forces = ffc.compute_desired_forces((0, 0, 0), (0, 0, 0), ['LF','RF','LB','RB']) for leg, f in forces.items(): print(f" {leg}: F=({f[0]:.2f}, {f[1]:.2f}, {f[2]:.2f}) N") friction = ffc.friction_cone_check(forces) print(f" Friction cone valid: {friction}") # With acceleration print("\n [Force Distribution - Forward accel 1m/s2]") forces = ffc.compute_desired_forces((1, 0, 0), (0, 0, 0), ['LF','RF','LB','RB']) for leg, f in forces.items(): print(f" {leg}: F=({f[0]:.2f}, {f[1]:.2f}, {f[2]:.2f}) N") friction = ffc.friction_cone_check(forces) print(f" Friction cone valid: {friction}") # Force control simulation print("\n [Force Control Balancing]") history = ffc.simulate_stance_force_control() for t, z, roll, pitch, forces in history: fz_avg = sum(f[2] for f in forces.values()) / 4 print(f" t={t:.3f}s z={z*1000:.1f}mm roll={roll*180/math.pi:.2f}deg pitch={pitch*180/math.pi:.2f}deg Fz_avg={fz_avg:.1f}N") # 3-leg support print("\n [3-leg Force Distribution (lift LF)]") forces3 = ffc.compute_desired_forces((0, 0, 0), (0, 0, 0), ['RF','LB','RB']) for leg, f in forces3.items(): fn = f[2] ft = math.sqrt(f[0]**2 + f[1]**2) print(f" {leg}: F=({f[0]:.2f}, {f[1]:.2f}, {f[2]:.2f}) N, Ft/Fn={ft/fn:.3f}" if fn > 0 else f" {leg}: lift-off!") print() print(" OK - Foot force control simulation complete")