🤖 正逆运动学

import math class QuadrupedKinematics3D: def __init__(self, body_L=0.4, body_W=0.2, L_hip=0.04, L_thigh=0.15, L_calf=0.15): self.body_L = body_L self.body_W = body_W self.L_hip = L_hip self.L_thigh = L_thigh self.L_calf = L_calf self.hip_offsets = { 'LF': ( body_L/2, body_W/2, 0), 'RF': ( body_L/2, -body_W/2, 0), 'LB': (-body_L/2, body_W/2, 0), 'RB': (-body_L/2, -body_W/2, 0), } self.hip_signs = {'LF': 1, 'RF': -1, 'LB': 1, 'RB': -1} def forward_kinematics(self, leg_name, q_hip, q_thigh, q_calf): hx, hy, hz = self.hip_offsets[leg_name] sign = self.hip_signs[leg_name] knee_y = hy + sign * self.L_hip * math.cos(q_hip) knee_z = hz - self.L_hip * math.sin(q_hip) knee_x = hx ankle_x = knee_x + self.L_thigh * math.sin(q_thigh) ankle_z = knee_z - self.L_thigh * math.cos(q_thigh) ankle_y = knee_y foot_x = ankle_x + self.L_calf * math.sin(q_thigh + q_calf) foot_z = ankle_z - self.L_calf * math.cos(q_thigh + q_calf) foot_y = ankle_y return (foot_x, foot_y, foot_z) def inverse_kinematics(self, leg_name, foot_pos): fx, fy, fz = foot_pos hx, hy, hz = self.hip_offsets[leg_name] sign = self.hip_signs[leg_name] dy = fy - hy q_hip = math.asin(max(-1, min(1, -sign * (fz - hz) / self.L_hip if abs(fz-hz) < self.L_hip else 0))) knee_y = hy + sign * self.L_hip * math.cos(q_hip) knee_z = hz - self.L_hip * math.sin(q_hip) dx = fx - hx dz = -(fz - knee_z) d = math.sqrt(dx**2 + dz**2) cos_q_calf = (d**2 - self.L_thigh**2 - self.L_calf**2) / (2*self.L_thigh*self.L_calf) cos_q_calf = max(-1, min(1, cos_q_calf)) q_calf = math.acos(cos_q_calf) alpha = math.atan2(dx, dz) beta = math.atan2(self.L_calf * math.sin(q_calf), self.L_thigh + self.L_calf * math.cos(q_calf)) q_thigh = alpha - beta return q_hip, q_thigh, q_calf def standing_pose(self, height=0.2): poses = {} for leg in self.hip_offsets: hx, hy, _ = self.hip_offsets[leg] foot_pos = (hx, hy, -height) try: q = self.inverse_kinematics(leg, foot_pos) poses[leg] = q except: poses[leg] = (0, 0, 0) return poses robot = QuadrupedKinematics3D() print("=" * 55) print(" 3D Forward/Inverse Kinematics Simulation") print("=" * 55) print(f" Body: {robot.body_L}m x {robot.body_W}m") print(f" Hip rod: {robot.L_hip}m, Thigh: {robot.L_thigh}m, Calf: {robot.L_calf}m") print() print(" [3D Forward Kinematics Test]") test_poses = [ ('LF', 0, 0, 0.5), ('LF', 0, 0.3, 0.8), ('RF', 0.2, 0, 0.5), ('LB', -0.2, -0.3, 1.0), ('RB', 0.1, 0.2, 0.6), ] for leg, q1, q2, q3 in test_poses: pos = robot.forward_kinematics(leg, q1, q2, q3) print(f" {leg}: q=({q1:.1f},{q2:.1f},{q3:.1f}) -> foot({pos[0]:.4f},{pos[1]:.4f},{pos[2]:.4f})") print() print(" [Standing Pose] height=0.2m") standing = robot.standing_pose(height=0.2) for leg, angles in standing.items(): q1, q2, q3 = angles pos = robot.forward_kinematics(leg, q1, q2, q3) print(f" {leg}: q=({q1:.3f},{q2:.3f},{q3:.3f}) -> foot({pos[0]:.4f},{pos[1]:.4f},{pos[2]:.4f})") print() print(" [FK->IK Loopback Verification]") random_angles = [ ('LF', 0.1, 0.2, 0.6), ('RF', -0.15, 0.3, 0.8), ('LB', 0.05, -0.1, 0.5), ('RB', -0.1, 0.15, 0.7), ] for leg, q1, q2, q3 in random_angles: pos = robot.forward_kinematics(leg, q1, q2, q3) q1r, q2r, q3r = robot.inverse_kinematics(leg, pos) pos_r = robot.forward_kinematics(leg, q1r, q2r, q3r) err = math.sqrt(sum((a-b)**2 for a,b in zip(pos, pos_r))) ok = "OK" if err < 0.001 else "FAIL" print(f" {leg}: err={err:.6f}m [{ok}]") print() max_reach = robot.L_thigh + robot.L_calf min_reach = abs(robot.L_thigh - robot.L_calf) print(f" [Reach Analysis]") print(f" Max reach: {max_reach:.3f} m") print(f" Min reach: {min_reach:.3f} m") if min_reach > 0: print(f" Ratio: {max_reach/min_reach:.1f}x") print() print(" OK - 3D kinematics simulation complete")