c++ - 逆运动学 ACos 误差 QNAN、INAN

Question

嘿，我从以下函数中得到“不是数字”的返回值:

void Spider::setAngles(double x,double y, double l1, double l2){
double theta1, theta2;
theta2=acos((pow(x,2)+pow(y,2)-pow(l1,2)-pow(l2,2))/(2*l1*l2));
cout<<theta2* 180/PI<<endl;
theta1=(-((l2*sin(theta2)*x)+(l1+l2*cos(theta2)*y))/((l2*sin(theta2)*y)+    (l1+l2*cos(theta2)*x)))* 180/PI;
cout<<theta1;
}

我知道 ACos 需要一个介于 -1 和 1 之间的参数值，但我不知道该怎么做，如果说末端执行器点位于 (15,15) 且长度都等于 2...

我需要规范化一切吗？包括关节之间的距离，以及从(0,0)->(15,15)的方向 vector

如有任何帮助，我们将不胜感激!

Answer 1

回想一下 right angled triangle :

cos(angle) = Adjacent/Hypotenuse

这意味着在您的 theta2 代码中您有:

Adjacent = x*x + y*y - l1*l1 - l2*l2
Hypotenuse = 2*l1*l2

充其量这是倒退的，并且更可能是错误的，具体取决于您尝试做什么。如果您要确定斜边从 (0,0) 到 (x+l1, y+l2) 的直角三角形的角度，您可以使用:

Adjacent = x + l1
Hypotenuse = sqrt((x+l1)*(x+l1) + (y+l2)*(y+l2))

或者对于三角形 (0,0) 到 (x-l1, y-l2):

Adjacent = x - l1
Hypotenuse = sqrt((x-l1)*(x-l1) + (y-l1)*(y-l1))

还要确保您正在尝试计算直角三角形而不是任意三角形的角度。