起初对于灯哥已经开源的代码中正运动学函数ik()的角度求解存在疑问
x1=-x1
shank1=pi-acos((x1*x1+y1*y1-l1*l1-l2*l2)/(-2*l1*l2))
fai1=acos((l1*l1+x1*x1+y1*y1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1)))
if x1>0:
ham1=abs(atan(y1/x1))-fai1
elif x1<0:
ham1=pi-abs(atan(y1/x1))-fai1
else:
ham1=pi-1.5707-fai1
shank1=180*shank1/pi
ham1=180*ham1/pi
因此自己推导其角度求解
此时应注意在使用仿真软件时,使电机旋转后大腿与地面平行时对应的位置为角度值为0的点,因此需要知道腿抬平时对应的角度值,在θ1和θ2的初始数值上做一些补偿,再将角度发送给电机。