Forward manipulator kinematic examples

This notebook  gives some basic rotation, homogeneous transformation, and  forward manipulator kinematic definitions. Two examples of forward kinematics are illustrated.

Generic definitions

Turn off annoying warnings messages

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

Load in necessary packages

<< LinearAlgebra`MatrixManipulation`;

Nice view of matrices and vectors

Nice[m_] := m // MatrixForm  //  TraditionalForm

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 [(Subscript[R, k])(θ)]

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

Forward kinematic definitions

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

Drawing routines

\(\(DrawCoordFrameOne[t_, \ which_, \ scale_] := 
	
        Module[{ttemp, p0, \ px, \ py, \ pz}, 
		ttemp\  = \ 
            Dot\  @@ \ Take[t, \ {1, \ which}]; 
		p0\  = \ 
            Drop[ttemp . {0, 0, 0, 1}, \(-1\)]; 
		px\  = \ 
            Drop[ttemp . {scale, 0, 0, 1}, \(-1\)]; 
		py\  = \ 
            Drop[ttemp . {0, scale, 0, 1}, \(-1\)]; 
		pz\  = \ 
            Drop[ttemp . {0, 0, scale, 
                  1}, \(-1\)]; 
		{Thickness[ .015], {RGBColor[1, 0, 0], \ 
              Line[{p0, px}]}, {RGBColor[0, 1, 0], \ 
              Line[{p0, py}]}, 
			{RGBColor[0, 0, 1], 
              Line[{p0, \ pz}]}}];\)\)

\(ConvertToRectilinear[{{a_, b_, c_}, {d_, e_, f_}}, \ 
        do_:  False] := 
	
      If[do\  == \ True, 
	{{a, b, c}, \ {d, \ b, \ c}, \ {d, \ b, \ 
            f}, \ {d, e, f}}, \ {{a, b, c}, {d, e, f}}]\)

\(\(DrawManipulator[l_, \ coord_, \ scale_] := 
	
        Module[{t, \ links, \ gm, gf}, 
		t\  = \ 
            Join[{IdentityMatrix[4]}, TLink\  /@ \ l]; 
		links\  = \ 
            Transpose[
              Drop[Transpose[
                  Table[Dot\  @@ \ 
                      Join[Take[t, \ {1, i}], {{0, 0, 0, 1}}], {i, \ 1, \ 
                      Length[t]}]], \(-1\)]]; 
		gm = \({RGBColor[1, 0, 
                    1], Thickness[ .02], 
                  Line[ConvertToRectilinear[#, \ False]]} &\)\  /@ \ 
              Partition[links, 2, 
                1]; 
		gm2\  = \ \({RGBColor[0, 0, 0], \ 
                  PointSize[ .025], \ Point[#]} &\)\  /@ \ 
              links; 
		gf\  = \ 
            DeleteCases[
              Table[If[coord[\([i]\)]\  > \ 0, 
                  DrawCoordFrameOne[t, \ i, \ scale]], {i, \ 1, \ 
                  Length[t]}], \ Null]; 
		Graphics3D[{gm2, gf, \ 
              gm}]
	];\)\)

Example #1: Planar manipulator

Planar specific definitions

lengths =  {L1 -> 1,  L2 ->  .75,  L3  ->   .5};

\(\(MakePlanarRules[angles_, \ 
          lengths_] := 
	{θ1 -> angles[\([1]\)], \ θ2 -> 
              angles[\([2]\)], \ θ3 -> angles[\([3]\)]}~Join~
          lengths;\)\)

\(\(AnimatePlanar[l_, \ coord_, \ scale_, {start_, \ stop_, \ n_}] := 
	
        Module[{glist, \ plrng, p}, 
		glist\  = \ 
            Table[DrawManipulator[
                l\  /. \ MakePlanarRules[
                    start*\((1 - i)\)\  + \ stop*i\  // \ N, \ lengths], \ 
                coord, \ scale], {i, 0, 1, 
                1/n}]; 
		p\  = \({Min[#], \ Max[#]} &\)\  /@ \ 
              Transpose[
                Partition[
                  Flatten[\((\(FullOptions[#, \ PlotRange] &\)\  /@ \ 
                        glist)\)], 6]]; 
		plrng\  = \ {{p[\([1, 1]\)], 
                p[\([2, 2]\)]}, {p[\([3, 1]\)], \ 
                p[\([4, 2]\)]}, {p[\([5, 1]\)], \ 
                p[\([6, 2]\)]}}; 
		\(Show[#, \ PlotRange -> plrng, \ 
                Boxed -> True] &\)\  /@ \ glist
];\)\)

Planar DH parameters

dh  =  {{0, 0, 0, θ1}, {0, L1,  0,  θ2}, {0, L2,  
          0,  θ3},  {0,  L3,  0,  0}};  dh  // Nice

Full transform (yuk!)

t  =  T[dh, 0, 3];
t  //  Nice

Simplified transform

t2  =  t  //  FullSimplify; 
t2  //  Nice

"Home" position

t2  /.  {θ1 -> 0, θ2 -> 0, θ3 -> 0}  //  
      Nice

In short-hand notation

t3  =  
        t2  /.  MakeRulesList[
            dh,  {{1, 2, 3}, {1, 2}}]; 
t3  //  Nice

Some visualization

AnimatePlanar[dh,  {1, 0, 0, 0, 1},  
        1,  {{0, 0, 0, 0},  {Pi/4, Pi/2, Pi/2, 0}, 30}];

Example #2: Puma 560

Puma specific definitions

lengths  =  {h ->   0. ,  a2 -> 431.8,  d3  ->  149.09,  
          a3  -> 20.32,  d4  ->  433.07};

\(\(MakePumaRules[angles_, \ 
          lengths_] := 
	{θ1 -> angles[\([1]\)], \ θ2 -> 
              angles[\([2]\)], \ θ3 -> angles[\([3]\)], \ θ4 -> 
              angles[\([4]\)], \ θ5 -> angles[\([5]\)], \ θ6 -> 
              angles[\([6]\)]}~Join~lengths;\)\)

\(\(AnimatePuma[l_, \ coord_, \ scale_, {start_, \ stop_, \ n_}] := 
	
        Module[{glist, \ plrng, p}, 
		glist\  = \ 
            Table[DrawManipulator[
                l\  /. \ MakePumaRules[
                    start*\((1 - i)\)\  + \ stop*i\  // \ N, \ lengths], \ 
                coord, \ scale], {i, 0, 1, 
                1/n}]; 
		p\  = \({Min[#], \ Max[#]} &\)\  /@ \ 
              Transpose[
                Partition[
                  Flatten[\((\(FullOptions[#, \ PlotRange] &\)\  /@ \ 
                        glist)\)], 6]]; 
		plrng\  = \ {{p[\([1, 1]\)], 
                p[\([2, 2]\)]}, {p[\([3, 1]\)], \ 
                p[\([4, 2]\)]}, {p[\([5, 1]\)], \ 
                p[\([6, 2]\)]}}; 
		\(Show[#, \ PlotRange -> plrng, \ 
                Boxed -> True] &\)\  /@ \ glist
];\)\)

Puma DH parameters (degrees)

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

Puma DH parameters (radians)

dh =  ConvertFromDegrees[puma];
dh  //  Nice

Full transform (yuk!)

t  =  T[dh, 0, 6]

LeafCount[t]

Slightly simpler transform (combines θ2 nd θ3 angles)

t2  =  
      T[dh, 0, 1] . (T[dh, 1, 3]  //  FullSimplify) . 
        T[dh, 3, 6]

LeafCount[t2]

In short-hand notation

t3  =  
        t2  /.  MakeRulesList[dh,  {{2, 3}}];
Nice  /@  Transpose[t3]

LeafCount[t3]

Rotation component of transform

r  =  RFromT[t3];
Nice  /@  Transpose[r]

Translational component of transform

p  =  PFromT[t3];
p  //  Nice

Some visualization

AnimatePuma[dh,  {1, 1, 1, 1, 1, 1, 1},  
        100,  {{0, 0, 0, 0, 0, 0},  {0, -Pi/2, -Pi/2, 0, 0, 0}, 
          30}];