Matlab Inverse Kinematics 6 DOF

Question

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

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

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