# test_kinematics.py import numpy as np def forward_kinematics(theta, link, side_sign): l1 = link['hip'] * side_sign l2 = -link['thigh'] l3 = -link['calf'] q1, q2, q3 = theta s1, s2, s3 = np.sin([q1, q2, q3]) c1, c2, c3 = np.cos([q1, q2, q3]) c23 = c2 * c3 - s2 * s3 s23 = s2 * c3 + c2 * s3 x = l3 * s23 + l2 * s2 y = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1 z = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2 return np.array([x, y, z]) def q1_ik(py, pz, l1): L = np.sqrt(py*py + pz*pz - l1*l1) return np.arctan2(pz*l1 + py*L, py*l1 - pz*L) def q3_ik(b3z, b4z, b): temp = (b3z**2 + b4z**2 - b**2) / (2.0 * np.abs(b3z * b4z)) temp = np.clip(temp, -1.0, 1.0) q3 = np.arccos(temp) return -(np.pi - q3) def q2_ik(q1, q3, px, py, pz, b3z, b4z): a1 = py * np.sin(q1) - pz * np.cos(q1) a2 = px m1 = b4z * np.sin(q3) m2 = b3z + b4z * np.cos(q3) return np.arctan2(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1) def inverse_kinematics(foot, link, side_sign): px, py, pz = foot b2y = link['hip'] * side_sign b3z = -link['thigh'] b4z = -link['calf'] a = link['hip'] c = np.sqrt(px*px + py*py + pz*pz) b = np.sqrt(c*c - a*a) if np.isnan(b) or b > abs(b3z) + abs(b4z): return None q1 = q1_ik(py, pz, b2y) q3 = q3_ik(b3z, b4z, b) q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z) return np.array([q1, q2, q3]) # ...existing code... def test(): link = {'hip': 0.05, 'thigh': 0.2, 'calf': 0.2} side_sign = 1 # 左前腿 # 输入一个目标点 foot_target = np.array([0.0, 0.06, -0.2]) print("目标足端位置:", foot_target) # 逆运动学求解关节角度 theta = inverse_kinematics(foot_target, link, side_sign) if theta is None: print("目标点不可达!") return print("逆运动学解(关节角度,单位:弧度):", theta) print("逆运动学解(关节角度,单位:度):", np.degrees(theta)) # 正运动学验算 foot_check = forward_kinematics(theta, link, side_sign) print("正运动学验算足端位置:", foot_check) print("位置误差:", np.abs(foot_target - foot_check)) if __name__ == "__main__": test()