Inverse Kinematic ACos error QNAN, INAN

Question

Hey I am getting a return value of "Not a Number" from the following function:

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;
}

I understand that ACos needs an argument value between -1 and 1, but I cannot figure out how to do this, if per say the end effector point is at (15,15) with lengths both equal to 2...

Do I need to normalise everything? Including the distances between joints, and the directional vector from (0,0)->(15,15)

Any help would be greatly appreciated!

Answer 1

Recall that for a right angled triangle:

cos(angle) = Adjacent/Hypotenuse

which means in your code for theta2 you have:

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

At best this is backwards and is more likely just wrong depending on exactly what you are trying to do. If you are trying to determine the angle of a right angled triangle with the hypotenuse from (0,0) to (x+l1, y+l2) you would use:

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

or for a triangle (0,0) to (x-l1, y-l2):

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

Also make sure you are trying to compute the angle in a right-angled triangle and not an arbitrary one.