Обратная Кинематическая ошибка 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

Напомним, что для прямоугольного треугольника :

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

Также убедитесь, что вы пытаетесь вычислить угол в прямоугольном треугольнике, а не в произвольном.