Elementary rotation and transform definitions

Load in necessary packages

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

Turn off annoying warnings messages

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

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

Tlink  =  TLink;

Compound transform for multiple links (for DH parameter list)

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

Tall  =  TAll;

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

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

Drawing stuff

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

Pneuman specific definitions

lengths  =  {d3 -> 12. };

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

\(\(AnimatePneuman[l_, \ coord_, \ 
          scale_, {start_, \ stop_, \ n_}] := 
	
        Module[{glist, \ plrng, p}, 
		glist\  = \ 
            Table[DrawManipulator[
                l\  /. \ MakePneumanRules[
                    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
];\)\)

Pneuman animation experiments

\(\(dh\  = \ {{0, 0, 0, θ1}, {Pi/2, \ 0, 
            0, θ2}, 
		{\(-Pi\)/2, 0, 
            d3, \ θ3}, 
		{Pi/2, \ 0, \ 
            0, \ θ4}, 
		{\(-Pi\)/2, 0, 0, θ5}};\)\)
dh  //  TraditionalForm

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

Pneuman Cartesian workspace analysis

Random number shortcuts for each angle

rr1  :=  Random[Real,  {0,  Pi}];
rr2  :=  Random[Real,  {0,  Pi}];
rr3  :=  Random[Real,  {0,  Pi}];
rr4  :=  Random[Real,  {0,  Pi}];
rr5  :=  Random[Real,  {0,  Pi}];

Table of positions

n  =  1000;
p  =  P[dh,  0,  5];

ptbl  =  
        Table[p  /.  
            MakePneumanRules[{rr1, rr2, rr3, rr4, rr5},  
              lengths],  {n}];

Plotting

Show[Graphics3D[{PointSize[ .02],  Point[#]} &  /@  ptbl],  
        AspectRatio -> Automatic,  Axes -> True];

Pneuman orientation workspace analysis

Random number shortcuts for each angle

rr1  :=  Random[Real,  {0,  Pi}];
rr2  :=  Random[Real,  {0,  Pi}];
rr3  :=  Random[Real,  {0,  Pi}];
rr4  :=  Random[Real,  {0,  Pi}];
rr5  :=  Random[Real,  {0,  Pi}];

Table of axes orientations

\(\(kv[rm_, θ_] := 
	
        Module[{vars, rn, \ 
            kdir}, 
		vars\  = {θ1, \ θ2, \ θ3, θ4, θ5}; 
		rn\  = \ 
            N[rm]\  /. \ \((\(\((#[\([1]\)] -> #[\([2]\)])\) &\)\  /@ \ 
                  Transpose[{vars, θ}])\); 
		kdir = {rn[\([3, 
                    2]\)]\  - \ rn[\([2, 3]\)], 
              rn[\([1, 3]\)] - rn[\([3, 1]\)], 
              rn[\([2, 1]\)] - rn[\([1, 2]\)]}; 
		kdir/
            Sqrt[kdir . kdir]];\)\)

n  =  1000;
\(\(rm\  = \ R[dh, \ 0, \ 5];\)
\)
rtbl  =  
        Table[kv[rm,  {rr1, rr2, rr3, rr4, rr5}], {n}];

Plotting

Show[Graphics3D[{PointSize[ .02],  Point[#]} &  /@  rtbl],  
        AspectRatio -> Automatic,  Axes -> True];

Pneuman waist analysis

dhD = {{0, 0, 0, θ1},  {-90,  0,  
            0, θ2}};
dhD // TraditionalForm

dh = ConvertFromDegrees[dhD]; dh // TraditionalForm

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

T12 = T[dh,  1,  2]; T12 // TraditionalForm

T02 = T[dh,  0,  2]; T02 // TF // FS

Trotpt = {{R11,  R12,  R13,  PX}, {R21,  R22,  R23,  PY}, {R31,  
          R32,  R33,  PZ}, {0, 0, 0, 1}}; Trotpt // TF