# Matlab Inverse Kinematics 6 DOF

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

• could you please give me the what these variables mean? Mar 15, 2017 at 9:58