以下代码实现了腿部正/逆运动学、雅可比矩阵和工作空间分析：
import math
class LegMechanism:
def __init__(self, L1=0.15, L2=0.15, hip_offset=0.04):
self.L1 = L1
self.L2 = L2
self.hip_offset = hip_offset
def forward_kinematics(self, theta1, theta2):
x = self.L1 * math.sin(theta1) + self.L2 * math.sin(theta1 + theta2)
z = -(self.L1 * math.cos(theta1) + self.L2 * math.cos(theta1 + theta2))
return x, z
def inverse_kinematics(self, x, z):
d = math.sqrt(x**2 + z**2)
if d > self.L1 + self.L2:
d = self.L1 + self.L2 - 0.001
if d < abs(self.L1 - self.L2):
d = abs(self.L1 - self.L2) + 0.001
cos_theta2 = (d**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
cos_theta2 = max(-1, min(1, cos_theta2))
theta2 = math.acos(cos_theta2)
alpha = math.atan2(x, -z)
beta = math.atan2(self.L2 * math.sin(theta2), self.L1 + self.L2 * math.cos(theta2))
theta1 = alpha - beta
return theta1, theta2
def jacobian(self, theta1, theta2):
j11 = self.L1 * math.cos(theta1) + self.L2 * math.cos(theta1 + theta2)
j12 = self.L2 * math.cos(theta1 + theta2)
j21 = self.L1 * math.sin(theta1) + self.L2 * math.sin(theta1 + theta2)
j22 = self.L2 * math.sin(theta1 + theta2)
return [[j11, j12], [j21, j22]]
def workspace_analysis(self, n_samples=360):
points = []
for i in range(n_samples):
theta1 = math.radians(-90 + 180 * i / n_samples)
for j in range(n_samples):
theta2 = math.radians(180 * j / n_samples)
x, z = self.forward_kinematics(theta1, theta2)
if z < 0:
points.append((x, z))
xs = [p[0] for p in points]
zs = [p[1] for p in points]
return {
'x_range': (min(xs), max(xs)),
'z_range': (min(zs), max(zs)),
'area_approx': (max(xs)-min(xs)) * abs(min(zs)-max(zs)),
'n_points': len(points)
}
leg = LegMechanism(L1=0.15, L2=0.15)
print("=" * 55)
print(" 腿机构学仿真")
print("=" * 55)
print(f" 大腿长度 L1 = {leg.L1} m")
print(f" 小腿长度 L2 = {leg.L2} m")
print()
print(" 【正运动学测试】")
test_angles = [
(0, 0),
(0.3, 0.5),
(-0.3, 0.8),
(0.5, 1.2),
(math.pi/4, math.pi/3),
]
for t1, t2 in test_angles:
x, z = leg.forward_kinematics(t1, t2)
print(f" th1={t1:.2f}, th2={t2:.2f} -> foot({x:.4f}, {z:.4f}) m")
print()
print(" 【逆运动学验证(FK->IK->FK闭环)】")
for t1, t2 in test_angles:
x, z = leg.forward_kinematics(t1, t2)
t1_r, t2_r = leg.inverse_kinematics(x, z)
x_r, z_r = leg.forward_kinematics(t1_r, t2_r)
err = math.sqrt((x-x_r)**2 + (z-z_r)**2)
ok = "OK" if err < 1e-6 else "FAIL"
print(f" ({t1:.2f},{t2:.2f}) -> IK -> ({t1_r:.4f},{t2_r:.4f}) err={err:.6f}m [{ok}]")
print()
print(" 【雅可比矩阵分析】")
t1, t2 = 0.0, 0.5
J = leg.jacobian(t1, t2)
det_J = J[0][0]*J[1][1] - J[0][1]*J[1][0]
print(f" th1={t1:.2f}, th2={t2:.2f}")
print(f" J = [[{J[0][0]:.4f}, {J[0][1]:.4f}],")
print(f" [{J[1][0]:.4f}, {J[1][1]:.4f}]]")
print(f" det(J) = {det_J:.4f}")
sing = "non-singular" if abs(det_J) > 0.001 else "NEAR SINGULAR"
print(f" status: {sing}")
print()
print(" 【工作空间分析】")
ws = leg.workspace_analysis()
print(f" X range: [{ws['x_range'][0]:.3f}, {ws['x_range'][1]:.3f}] m")
print(f" Z range: [{ws['z_range'][0]:.3f}, {ws['z_range'][1]:.3f}] m")
print(f" Approx area: {ws['area_approx']*1e4:.1f} cm^2")
print()
print(" OK - Leg mechanism simulation complete, FK/IK loopback verified")
仿真结果：
=======================================================
腿机构学仿真
=======================================================
大腿长度 L1 = 0.15 m
小腿长度 L2 = 0.15 m
【正运动学测试】
th1=0.00, th2=0.00 -> foot(0.0000, -0.3000) m
th1=0.30, th2=0.50 -> foot(0.1519, -0.2478) m
th1=-0.30, th2=0.80 -> foot(0.0276, -0.2749) m
th1=0.50, th2=1.20 -> foot(0.2207, -0.1123) m
th1=0.79, th2=1.05 -> foot(0.2510, -0.0672) m
【逆运动学验证(FK->IK->FK闭环)】
(0.00,0.00) -> IK -> (0.0000,0.0000) err=0.000000m [OK]
(0.30,0.50) -> IK -> (0.3000,0.5000) err=0.000000m [OK]
(-0.30,0.80) -> IK -> (-0.3000,0.8000) err=0.000000m [OK]
(0.50,1.20) -> IK -> (0.5000,1.2000) err=0.000000m [OK]
(0.79,1.05) -> IK -> (0.7854,1.0472) err=0.000000m [OK]
【雅可比矩阵分析】
th1=0.00, th2=0.50
J = [[0.2816, 0.1316],
[0.0719, 0.0719]]
det(J) = 0.0108
status: non-singular
【工作空间分析】
X range: [-0.300, 0.300] m
Z range: [-0.300, -0.000] m
Approx area: 1800.0 cm^2
OK - Leg mechanism simulation complete, FK/IK loopback verified