逆運動學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）

任何幫助將不勝感激！

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)) 

另外，還要確保你正在試圖計算一個直角三角形的角度，而不是任意一個。