79 lines
2.2 KiB
Python
79 lines
2.2 KiB
Python
# 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()
|