Puma 560 inverse kinematic solution

General definitions

Load packages and turn off specific messages

Turn off annoying warnings messages

Off[General::spell1];
Off[General::spell];
Off[ParametricPlot::ppcom];
Off[ParametricPlot3D::ppcom];
Off[Solve::ifun];

Load in necessary packages

<< LinearAlgebra`MatrixManipulation`;
<< Graphics`Shapes`;

Elementary rotation and transform definitions

Definition of elementary rotation matrices

rX[
          a_] := {{1, 0, 0}, {0,  Cos[a],  -Sin[a]}, {0, Sin[a],  
            Cos[a]}};
rY[
          a_] := {{Cos[a], 0, Sin[a]}, {0, 1, 0},  {-Sin[a], 0, 
            Cos[a]}};
rZ[
          a_] := {{Cos[a],  -Sin[a], 0}, {Sin[a], Cos[a], 0}, {0, 0, 
            1}};

Convert a 3x3 rotation matrix and a 3x1 position vector to a 4x4 homogeneous operator

\(\(RAndPToT[r_, \ p_] := 
	
        Join[Transpose[
            Join[Transpose[r], \ {p}]], {{0, 0, 0, 1}}];\)\)

Convert a 3x3 rotation matrix to 4x4 homogeneous rotational operator

RToT[r_] := RAndPToT[r,  {0, 0, 0}];

Elementary homogeneous rotational operators

RX[a_] := RToT[rX[a]];
RY[a_] := RToT[rY[a]];
RZ[a_] := RToT[rZ[a]];

Angle-axis rotation

\(\(RK[θ_, \ k_] := 
	
        Module[{kx, \ ky, kz, \ v, c, s}, 
		{kx, \ ky, \ kz}\  = \ 
            k/Sqrt[k . k]; 
		{{kx^2\ v\  + \ c, kx\ ky\ v\  - \ kz\ s, \ 
                kx\ kz\ v\  + \ ky\ s}, 
			{kx\ ky\ v\  + \ kz\ s, \ 
                ky^2\ v + \ c, \ 
                ky\ kz\ v\  - \ kx\ s}, 
			{kx\ kz\ v\  - \ ky\ s, \ 
                ky\ kz\ v\  + \ kx\ s, \ 
                kz^2\ v\  + \ c}}\  /. \ {v -> \((1 - Cos[θ])\), \ 
              c -> Cos[θ], \ s -> Sin[θ]}];\)\)

Convert a 3x1 position vector to a 4x4 homogeneous translational operator

PToT[p_] := RAndPToT[IdentityMatrix[3],  p];

Elementary homogeneous translational operators

DX[x_] := PToT[{x, 0, 0}];
DY[y_] := PToT[{0, y, 0}];
DZ[z_] := PToT[{0, 0, z}];

Extract 3x3 rotation matrix from 4x4 homogeneous transform

RFromT[t_] := TakeMatrix[t, {1, 1}, {3, 3}];

Extract 3x1 position vector from 4x4 homogeneous transform

PFromT[t_] := 
        TakeMatrix[t, {1, 4}, {3, 4}]  //  Flatten;

Compute inverse of a 4x4 homogeneous transform

\(\(InverseT[t_] := 
	
        Module[{r, \ p}, 
		r\  = \ RFromT[t]; 
		p\  = \ 
            PFromT[t]; 
		RAndPToT[
            Transpose[r], \ \(-Transpose[r] . p\)]
	];\)\)

Static forward kinematic definitions

In this notebook, DH parameters should be speficied as table, with one entry per link in the form {(Subscript[α, i - 1]), (Subscript[a, i - 1]), (Subscript[d, i]), (Subscript[θ, i]), type}, where "type" is 0 for revolute joints and 1 for prismatic joints.

Degree to radian conversion (for DH parameter list)

\(\(ConvertFromDegreesOne[{alpha_, \ a_, \ d_, \ theta_, \ 
            type___}] := 
	{If[NumericQ[alpha], \ alpha\ Degree, \ 
              alpha], \ a, \ d, \ 
            If[NumericQ[theta], \ theta\ Degree, \ theta], \ type}\  // \ 
          FullSimplify;\)\)

ConvertFromDegrees[l_] := 
        ConvertFromDegreesOne  /@  l;

Basic one link homogeneous transform (based on DH parameters)

\(\(TLink[{α_, \ a_, \ \ d_, θ_, type___}] := 
	
        RX[α] . DX[a] . RZ[θ] . DZ[d];\)\)

Compound transform for multiple links (for DH parameter list)

TAll[l_] :=  Dot  @@ (TLink  /@  l);

Homogeneous transform for robot  from frame {j} to frame {i}

\(\(T[dh_, \ i_, \ 
          j_] := 
		Which[
			i\  == \ j, 
				IdentityMatrix[
            4], 
			i\  < \ j, 
				TAll[
            Take[dh, \ {i + 1, \ j}]], 
			i\  > \ j, 
				InverseT[
            TAll[Take[dh, {j + 1, i}]]]];\)\)

Rotation matrix for robot from frame {j} to frame {i}

R[dh_, i_, j_] := RFromT[T[dh,  i,  j]];

Position vector for robot from frame {j} to frame {i}

P[dh_, i_, j_] := PFromT[T[dh,  i,  j]];

Expression simplification functions

\(\(AngleSumRules[l_, \ t_:  θ] := 
	
        Plus\  @@ \ \((\(ToExpression[ToString[t] <> ToString[#]] &\)\  /@ \ 
              l)\);\)\)

\(\(MakeRulesList[l_, add___] := 
	
        Module[{t, \ rl, \ tmp}, 
		If[
            Length[{add}\  // \ Flatten]\  > \ 
              0, 
		tmp\  = \({Cos[AngleSumRules[#]] -> 
                      ToExpression["c" <> 
                          StringJoin[ToString\  /@ \ #]], 
			Sin[
                        AngleSumRules[#]]\  -> 
                      ToExpression["s" <> 
                          StringJoin[ToString\  /@ \ #]]} &\)\  /@ \ 
                add, 
			tmp\  = \ {}]; 
		t\  = \ 
            Transpose[l]\  // \ 
              Last; 
		rl\  = \(If[\(! NumericQ[#]\), \ \ {Cos[#] -> 
                      ToExpression["c" <> 
                          ToString[\((Position[t, \ #]\  // \ 
                                  Flatten)\)[\([1]\)]]], \ 
                    Sin[#] -> 
                      ToExpression["s" <> 
                          ToString[\((Position[t, \ #]\  // \ 
                                  Flatten)\)[\([1]\)]]]}] &\)\  /@ \ 
              t; 
		Join[tmp, DeleteCases[rl, \ Null]]\  // \ 
            Flatten
	];\)\)

Inverse kinematic setup

Puma DH parameters (degrees)

dhDegrees  =  {{0, 0, 0, θ1}, {-90,  0, 
          0, θ2}, {0, a2,  d3,  θ3}, {-90, a3,  
          d4,  θ4}, {90, 0, 0, θ5}, {-90, 0, 
          0, θ6}};  dhDegrees  //  TraditionalForm

Puma DH parameters (radians)

dh =  ConvertFromDegrees[dhDegrees];  
    dh  //  TraditionalForm

Short-hand display notation

short  =  MakeRulesList[dh, {{2, 3}}];
TF  :=  TraditionalForm;
d[m_] :=  m  /.  short  //  TraditionalForm;

What we know:

In the inverse kinematics problem, we assume that the matrix below is known; that is, we assume that we have a desired position/orientation of the end-effector with respect to the base, and we want to solve for the joint angles.

Trp  =  
        RAndPToT[{{r11, r12, r13}, {r21, r22, r23}, {r31, r32, r33}}, {px, 
            py, pz}];
Trp  //  TF

Link transforms

T01  =  T[dh,  0,  1];
T01  //  d

T13  =  T[dh, 1, 3]  //  FullSimplify; 
T13  //  d

T34  =  T[dh, 3, 4];
T34  //  d

T45  =  T[dh, 4, 5];
T45  //  d

T56  =  T[dh, 5, 6];
T56  //  d

Overall transform

Below, we now have 12 possible equations in terms of the joint angles; namely Trp = T06...

T06  =  T01 . T13 . T34 . T45 . T56;
T06  // Transpose  //  d

Trp  //  Transpose  //  d

Inverse kinematic solution: θ1: Premultiply by T10

One side of the equation

T16a  = T13 . T34 . T45 . T56;
T16a  //  d

The other side of the equation

T10  =  T[dh, 1, 0];
T10  //  d

T16b  =  T10 . Trp  //  FullSimplify;
T16b  //  d

Both sides of the equation

T16a  //  d

T16b  //  d

Note element (2, 4) is just in terms of θ1

eq1  =  T16a[[2, 4]]  ==  T16b[[2, 4]];  
    eq1  //  TF

Mathematica solution...

sol1  =  
      Solve[eq1,  θ1];  (θ1  /.  sol1)   //  
      TraditionalForm

How can we solve this by hand?

Make the following substitutions

subs  =  {px -> ρ Cos[ϕ],  
          py -> ρ Sin[ϕ]}; 
({#} &  /@  subs)  // MatrixForm  //  TF

eq1a  =  eq1  /.  subs;
eq1a  //  TF

Can solve for θ1

eq1b  =  
        eq1a[[1]]/ρ  ==  eq1a[[2]]/ρ; 
eq1b   // TF

eq1c  =  eq1b  //  Simplify; 
eq1c  //  TraditionalForm

Interim solution for θ1

sin (ϕ  -  θ1)  =  (d3/ρ)cos (ϕ  -  θ1) = ±(Sqrt[1-((((d3/ρ)))^2)])

ϕ  -  θ1 = atan2((d3/ρ), ±(Sqrt[1-((((d3/ρ)))^2)]))

What is ρ and ϕ in terms of px and py (by hand)

subs  =  {px -> ρ Cos[ϕ],  
          py -> ρ Sin[ϕ]}; 
({#} &  /@  subs)  // MatrixForm  //  TF

sols  =  {ρ  ->  (Sqrt[(px^2) + (py^2)]),  ϕ -> 
            ArcTan[px,  py]};
({#} &  /@  sols)  // MatrixForm  //  TF

Complete solution for θ1

ϕ  -  θ1 = atan2((d3/ρ), ±(Sqrt[1-((((d3/ρ)))^2)]))
ϕ  -  θ1 = atan2(d3, ±(Sqrt[(ρ^2)-(d3^2)]))

θ1 = atan2 (px,  py) -   atan2(d3, ±(Sqrt[ (px^2)+  (py^2)-(d3^2)]))

Inverse kinematic solution: θ3: Some more trickery

The following matrix is now known completely (why?)

T16b  //  d

What can we do next?

Here's what we got on the other side of the equation

T16a //  d

Equate elements (1, 4), (2, 4) and (3, 4):

\(\(\(T16a[\([1, 4]\)]\  == \ T16b[\([1, 4]\)]\  // \ TF

    T16a[\([2, 4]\)]\  == \ T16b[\([2, 4]\)]\  // \ TF\ 

    T16a[\([3, 4]\)]\  == \ T16b[\([3, 4]\)]\  // \ TF\)\(\ \)\)\)

Solve for θ2 and θ3. Square and add above equations:

Square elements (1, 4) eqs:

eq1b  =  (T16a[[1, 4]])^2  ==  (T16b[[1, 
                  4]])^2; 
eq1b  //  TF

Square elements (2, 4) eqs:

eq1a  =  (T16a[[2, 4]])^2  ==  (T16b[[2, 
                  4]])^2; 
eq1a  //  TF

Square elements (3, 4) eqs:

eq1c  =  (T16a[[3, 4]])^2  ==  (T16b[[3, 
                  4]])^2; 
eq1c  //  TF

Add them all up:

eq2  =  
        eq1a[[1]]  +  eq1b[[1]]  +  eq1c[[1]]  ==  
          eq1a[[2]] + eq1b[[2]]  +  eq1c[[2]]; 
eq2  //  TF

Simplify:

eq3  =  FullSimplify [eq2]; 
eq3  //  TF

No longer dependent on θ2! Lump all constants together, and we have simple eq. in terms of θ3:

2 a3 cos (θ3) a2 - 
            2 d4 sin (θ3) a2  =  
          (px^2) + (py^2) + (pz^2) - ((a2^2) +  (a3^2) + (d3^2) + (d4^2))
 
      a3 cos (θ3)  -  d4 sin (θ3)  =  
        (((px^2) + (py^2) + (pz^2) - 
            ((a2^2) +  (a3^2) + (d3^2) + (d4^2)))/(2  a2))

a3 cos (θ3)  -  d4 sin (θ3)  =  K
K  =   
          (((px^2) + (py^2) + (pz^2) - 
              ((a2^2) +  (a3^2) + (d3^2) + (d4^2)))/(2  a2))

Very similar to equation for θ1:

py cos (θ1) - px sin (θ1)  =  d3
          
θ1 = atan2(px,  py) -   atan2(d3, ±(Sqrt[ (px^2)+  (py^2)-(d3^2)]))

So, the solution for θ3 is:

a3 cos (θ3)  -  d4 sin (θ3)  =  K
K  =   
          (((px^2) + (py^2) + (pz^2) - 
              ((a2^2) +  (a3^2) + (d3^2) + (d4^2)))/(2  a2))
θ3 = atan2 (a3,  d4) -   atan2(K, ±(Sqrt[ (a3^2)+ (d4^2)-(K^2)]))

Inverse kinematic solution: θ2: Premultiply by T30

One side of the equation

T36a  = T34 . T45 . T56;
T36a  //  d

The other side of the equation

T30  =  T[dh, 3, 0]  //  FullSimplify;
T30  //  d

T36b  =  T30 . Trp;
T36b  //  d

Both sides of the equation

T36a  //  d

T36b  //  d

Equate elements (1, 4) and (2, 4)

eq1  =  T36a[[1, 4]]  ==  T36b[[1, 4]];
eq1  //  d

eq2  =  T36a[[2, 4]]  ==  T36b[[2, 4]];
eq2  //  d

Solve for c23 and s23:

sol  = 
        Solve[{eq1  /.  short,  eq2  /.  short},  {c23, s23}]  //  
          FullSimplify;
sol  //  Flatten  //  TF

c23 = ((-d4 pz + a2 s3 pz + 
                (a3 + a2 c3) (c1 px + py s1))/((pz^2) + 
                (((c1 px + py s1))^2)))

s23 = -
              (((a3 + a2 c3) pz + 
                    (c1 px + py s1) (d4 - a2 s3))/((pz^2) + 
                    (((c1 px + py s1))^2)))

Solve for θ2:

θ23  = atan2(s23, c23)

θ2 + θ3 = atan2[-(a3+a2 c3) pz-(c1 px+py s1) (d4-a2 s3), -d4 pz+a2 s3 pz+(a3+a2 c3) (c1 px+py s1)]

θ2  = atan2[-(a3+a2 c3) pz-(c1 px+py s1) (d4-a2 s3), -d4 pz+a2 s3 pz+(a3+a2 c3) (c1 px+py s1)] - θ3

Inverse kinematic solution: θ4, θ5, θ6: The home stretch

The following matrix is now known completely:

T36b  //  d

What can we do next?

Here's what we got on the other side of the equation

T36a //  d

When s5 ≠ 0, elements (1, 3) and (3, 3) offer solution to θ4:

eq1  =  T36a[[1, 3]]  ==  T36b[[1, 3]];
eq1  //  d

eq2  =  T36a[[3, 3]]  ==  T36b[[3, 3]];
eq2  //  d

θ4  = atan2(c1 r23-r13 s1, -c1 c23 r13- c23 r23 s1+r33 s23), sin(θ5) ≠0

Next, consider T46:

The following matrix is completely known:

T46b  =  T[dh, 4, 3] . T36b;
T46b  // Transpose //d

What can we do next?

T46a  =  T45 . T56;
T46a  //  Transpose  // d

Using elements (1, 3) and (3, 3), solve for θ5:

eq1  =  T46a[[1, 3]]  ==  T46b[[1, 3]];
eq1  //  d

Solve[eq1  /.  short,  s5]  //  Flatten //  TF

eq2  =  T46a[[3, 3]]  ==  T46b[[3, 3]];
eq2  //  d

c5 =-c23 r33-c1 r13 s23-r23 s1 s23

s5 =-c1 c23 c4 r13-s1 s4 r13-c23 c4 r23 s1+c4 r33 s23+c1 r23 s4


θ5  = atan2(s5, c5)

Next, consider T56:

The following matrix is completely known:

T56b  =  T[dh, 5, 4] . T46b;
T56b  // d

What can we do next?

T56a  =  T[dh,  5,  6];
T56a  //  d

Using elements (1, 1) and (3, 1), solve for θ6:

eq1  =  T56a[[1, 1]]  ==  T56b[[1, 1]];
eq1  //  d

eq2  =  T56a[[3, 1]]  ==  T56b[[3, 1]];
eq2  //  d

Solve[eq2  /.  short,  s6]  //  Flatten //  TF

c6 = c5 (c4 (c1 c23 r11+c23 r21 s1-r31 s23)-(c1 r21-r11 s1) s4)+(-c23 r31-c1 r11 s23-r21 s1 s23) s5

s6 = -c1 c4 r21-c23 s1 s4 r21+c4 r11 s1-c1 c23 r11 s4+r31 s23 s4


θ6  = atan2(s6, c6)

Inverse kinematic summary

Solution derived thus far:

θ1 = atan2 (px,  py) -   atan2(d3, ±(Sqrt[ (px^2)+  (py^2)-(d3^2)]))

K  =   
          (((px^2) + (py^2) + (pz^2) - 
              ((a2^2) +  (a3^2) + (d3^2) + (d4^2)))/(2  a2))
θ3 = atan2 (a3,  d4) -   atan2(K, ±(Sqrt[ (a3^2)+ (d4^2)-(K^2)]))

θ2  = atan2[-(a3+a2 c3) pz-(c1 px+py s1) (d4-a2 s3), -d4 pz+a2 s3 pz+(a3+a2 c3) (c1 px+py s1)] - θ3

θ4  = atan2(c1 r23-r13 s1, -c1 c23 r13- c23 r23 s1+r33 s23), sin(θ5) ≠0

c5 =-c23 r33-c1 r13 s23-r23 s1 s23

s5 =-c1 c23 c4 r13-s1 s4 r13-c23 c4 r23 s1+c4 r33 s23+c1 r23 s4

θ5 = atan2(s5, c5)

c6= c5 (c4 (c1 c23 r11+c23 r21 s1-r31 s23)-(c1 r21-r11 s1) s4)+(-c23 r31-c1 r11 s23-r21 s1 s23) s5

s6= -c1 c4 r21-c23 s1 s4 r21+c4 r11 s1-c1 c23 r11 s4+r31 s23 s4

θ6  = atan2(s6, c6)

Alternate solution:

θ4' = θ4 + π

θ5' = -θ5

θ6' = θ6 + π