Why the second output doesn't appear?

Question

0 个投票

Hi guys,

I have a simple code here. What am I trying to achieve is to get the values of theta1 and theta2 (basically, this is an inverse kinematics). the code gives the output, the problem now is, it is just give me the first output.

Here is the coding

   function[theta1_deg,theta2_deg]= inverse_two_links(x2,y2)
r2 = 0.1404; 
l1 = 0.3132;
cos_theta2 = (x2^2+y2^2-l1^2-r2^2)/(2*l1*r2);
sin_theta2 = sqrt(1-cos_theta2*cos_theta2);
theta2 = atan2(sin_theta2,cos_theta2);
k1 = l1 + r2 * cos(theta2);
k2 = r2 * sin(theta2);
theta1 = atan2(y2,x2) - atan2(k2,k1);
theta2_deg = theta2 * 180 /pi;
theta1_deg = theta1 * 180 /pi;

when i execute the code for instance, x=-0.1404,y=0.3132, it just give me only 90 degree which is for theta1. Could you help me to figure out, what happened actually in my code?

Thanks in advance..