I'm trying to write an inverse kinematics Matlab code for a 6 DOF robotic arm that has the following link parameters:

Twist angle (alpha): [-90, 0, 90, -90, 90, 0]

Link length (a): [0, 0.5, 0, 0, 0, 0]

Offset distance (d): [0, 0.25, 0, 1, 0, 0.5]

and Px, Py, Pz are the following [1,1,0]

I'm using the following equations for theta 1,2 and 3 values (closed form solution):

enter image description here

As seen in the equations theta 1 and 2 have 2 two roots (2 possible solutions) thus, the robot has eight groups of inverse kinematics solutions. How do I modify my code to select the ideal solution for theta ?

%Theta 1
     theta1 = (atan2(real(py),real(px)))-atan2(real(d2),real(sign1*sqrt(px^2+py^2-d2^2)));

 c1 = cos(theta1);
 s1 = sin(theta1);

 %Theta 2
 A = (c1*px)+(s1*py);
 B = (A^2+pz^2+a2^2-d4^2)/(2*a2);

 theta2 = (atan2(real(A),real(pz)))-atan2(real(B),real(sign2*sqrt(A^2+pz^2-B^2)));

 c2 = cos(theta2);
 s2 = sin(theta2);

 %Theta 3
 A1 = (c2*px)+(s2*py);
 theta3 = (atan2(real(A1-(a2*c2)),real(pz+(a2*s2)))) - theta2;

 c3 = cos(theta3);
 s3 = sin(theta3);
  • $\begingroup$ could you please give me the what these variables mean? $\endgroup$
    – Aruny
    Commented Mar 15, 2017 at 9:58

1 Answer 1


Ideal solution can be defined in many ways.

  • The simplest way to choose one is to compare which of the 8 solutions is closest to your current pose in joint space. This is usually a good idea if you are moving along a line (or similar defined trajectores).
  • In practics some robot manufactruers have solved this using the status and turn variables. These variables are additional inputs for the inverse kinematics problem and specify exactly which inverse kinematics solution is needed.
  • You can also check is one of the poses has a collision with the environmnent to filter infeasable poses.
  • You can calculate which pose offers the larges dexterity index

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.