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

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

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

Velocity forward kinematics

Angular velocity of link {i} in frame {j} coordinates

ω[dh_,  0,  0] :=  {0, 0, 0};

ω[dh_,  i_,  i_] :=  
      R[dh,  i,  i - 1] . ω[dh,  i - 1, i - 1] + 
        If[dh[[i, 5]]  ==  0,  
          Dt[dh[[i, 4]], t] {0, 0, 1}, {0, 0, 0}]

\(ω[dh_, \ j_, \ i_] := 
	
      R[dh, \ j, \ i] . ω[dh, \ i, i]\)

Linear velocity of link {i} in frame {j} coordinates

v[dh_,  0,  0] :=  {0, 0, 0};

\(v[dh_, \ i_, \ i_] := 
	
      R[dh, \ i, \ 
            i - 1] . \((v[dh, \ i - 1, \ i - 1]\  + \ 
              Cross[ω[dh, \ i - 1, i - 1], \ \ P[dh, i - 1, i]])\) + 
        If[dh[\([i, 5]\)] == 1, 
          Dt[dh[\([i, 3]\)], t] {0, 0, 1}, {0, 0, 0}]\)

\(\(v[dh_, \ j_, \ i_] := 
	
        R[dh, \ j, \ i] . v[dh, \ i, i];\)\)

Jacobian (from recursive equations)

Determine variables (as opposed to constants) in DH parameter list

\(\(ExtractVariables[dh_] := 
	
        Module[{vars}, 
			vars\  = \ \(If[#[\([5]\)]\  == \ 
                    0, \ #[\([4]\)], \ #[\([3]\)]] &\)\  /@ \ 
              dh; 
			\(Take[vars, \ #] &\)\  /@ \ 
              Position[NumberQ\  /@ \ vars, \ False]\  // \ 
            Flatten];\)\)

Compute linear velocity Jacobian from recursive equations

\(\(LinearJacobian1[dh_, \ j_, \ i_] := 
	
        Module[{vars, \ vel, \ j1}, 
		vars\  = \ 
            ExtractVariables[dh]; 
		vel\  = \ 
            v[dh, \ j, \ i]; 
		j1\  = \ 
            Transpose[\(Coefficient[vel, \ Dt[#, \ t]] &\)\  /@ \ 
                vars]; 
		j1
		];\)\)

Compute angular velocity Jacobian from recursive equations

\(\(AngularJacobian1[dh_, \ j_, \ i_] := 
	
        Module[{vars, \ vel, \ j1}, 
		vars\  = \ 
            ExtractVariables[dh]; 
		vel\  = \ ω[dh, \ j, \ 
              i]; 
		j1\  = \ 
            Transpose[\(Coefficient[vel, \ Dt[#, \ t]] &\)\  /@ \ 
                vars]; 
		j1
		];\)\)

Compute full Jacobian from recursive equations

\(\(Jacobian1[dh_, \ j_, \ i_] := 
	
        Join[LinearJacobian1[dh, \ j, \ i], \ 
          AngularJacobian1[dh, \ j, \ i]];\)\)

Jacobian (from forward kinematic homogeneous transform)

Compute linear velocity Jacobian from forward kinematic homogeneous transform

\(\(LinearJacobian2[dh_, \ j_, \ i_] := 
	
        Module[{vars, \ p, \ j2}, 
		vars\  = \ 
            ExtractVariables[dh]; 
		p\  = \ 
            P[dh, \ j, \ i]; 
		j2\  = \ 
            Outer[D, \ p, \ vars]; 
		j2
		];\)\)

Compute angular velocity Jacobian from forward kinematic homogeneous transform

\(\(AngularJacobian2[dh_, \ j_, \ i_] := 
	
        Module[{vars, r, \ r2, Ω, \ j2}, 
		vars\  = \ 
            ExtractVariables[dh]; 
		r\  = \ 
            R[dh, \ j, \ i]; 
		r2\  = \ 
            Dt[r, \ t] . 
              Transpose[r]; 
		Ω\  = \ {r2[\([3, 2]\)], \ 
              r2[\([1, 3]\)], \ r2[\([2, 1]\)]}; 
		j2\  = \ 
            Transpose[\(Coefficient[Ω, \ Dt[#, \ t]] &\)\  /@ \ 
                vars]; 
		j2
		];\)\)

Compute full Jacobian from forward kinematic homogeneous transform

\(\(Jacobian2[dh_, \ j_, \ i_] := 
	
        Join[LinearJacobian2[dh, \ j, \ i], \ 
          AngularJacobian2[dh, \ j, \ i]];\)\)

Animation of simulated velocity trajectories

\(\(ConvertTrajToGraphics[l_] := 
	
        Join[{{Thickness[ .015], \ RGBColor[0, 0, 1], \ 
              Line[l]}}, \({PointSize[ .02], \ RGBColor[1, 0, 0], \ 
                Point[#]} &\)\  /@ \ l];\)\)

\(\(ExtractMaximalPlotRange[ranges_, \ l_:  2] := 
	
        Module[{r2}, 
		If[
            l\  == \ 2, 
				r2\  = \ 
              Transpose[
                Partition[Flatten[ranges], 4]]; 
				{{Min[
                  r2[\([1]\)]], \ Max[r2[\([2]\)]]}, \ {Min[r2[\([3]\)]], \ 
                Max[r2[\([4]\)]]}}, 
				r2\  = \ 
              Transpose[
                Partition[Flatten[ranges], 6]]; 
				{{Min[
                  r2[\([1]\)]], \ Max[r2[\([2]\)]]}, {Min[r2[\([3]\)]], \ 
                Max[r2[\([4]\)]]}, \ {Min[r2[\([5]\)]], \ 
                Max[r2[\([6]\)]]}}]
	];\)\)

\(\(AnimateGraphicsList[traj_, \ l_:  2] := 
	
        Module[{f, \ g, \ plrng}, 
	
          If[l\  == \ 2, \ f\  = \ Graphics, \ f = Graphics3D\ ]; 
	
          g\  = \ \(Show[f[#], \ DisplayFunction -> Identity, \ 
                  PlotRange -> All] &\)\  /@ \ traj; 
	
          plrng\  = \ 
            ExtractMaximalPlotRange[\(FullOptions[#, \ PlotRange] &\)\  /@ \ 
                g, l]; 
	\(Show[f[#], \ PlotRange -> plrng, \ 
                AspectRatio -> Automatic] &\)\  /@ \ traj];\)\)

\(\(PlotVelocityTrajectory[dh_, \ traj_, \ l_:  2, \ n_:  20, \ 
          tfin_:  False] := 
	
        Module[{nF, \ joints, \ p0, \ ptake, \ vars, \ rul, \ traj2, \ 
            tf}, 
		If[l\  == \ 2, \ p0\  = \ {0, 0}; \ ptake\  = \ 2, \ 
            p0\  = \ {0, 0, 0}; \ ptake\  = \ 3]; 
		vars\  = \ 
            ExtractVariables[dh]; 
		nF\  = \ 
            Length[dh]; 
		joints\  = \ 
            Join[{p0}, 
              Table[Take[P[dh, \ 0, \ i], \ {1, ptake}], {i, 1, \ 
                  nF}]]; 
		rul\  = \(\((#[\([1]\)]\  -> \ \(#[\([2]\)]\)[
                      t])\) &\)\  /@ \ \(({vars, \ traj}\  // \ 
                  Transpose)\); 
		traj2\  = \ joints\  /. \ rul; 
		If[
            tfin\  == \ False, tf\  = \ traj[\([1, 1, 1, 2]\)], \ 
            tf\  = \ tfin, \ tf = tfin]; 
		AnimateGraphicsList[
            Table[ConvertTrajToGraphics[traj2], \ {t, \ 0, \ tf, \ tf/n}], 
            l]];\)\)

Velocity kinematic simulation routines

Mathematica conversion routine to use numerical differential equations solver

\(\(MakeDiffEq[p_, \ dh_] := 
	
        Module[{r, \ vars, d1, d2, 
            eq}, 
		vars\  = \ \ ExtractVariables[
              dh]; 
		r = \ \(\((# -> #[t])\) &\)\  /@ \ 
              vars; 
		d1\  = \ D[vars\  /. \ r\ , t]; 
		d2\  = \ 
            p\  /. \ 
              r; 
		eqs\  = \(\((#[\([1]\)]\  == \ #[\([2]\)])\) &\)\  /@ \ \(({d1, d2}\  // \ Transpose)\)];\)\)

General simulation of a specified Cartesian velocity trajectory

This function simulates a trajectory specified by the velocity relationship \(TraditionalForm\`\(θ\& . \) = \(J\^g\) \(x\& . \)\), where (J^g) (jg) specifies some user defined inverse relationship (matrix), and the other function arguments are: dh = list of DH parameters, traj = desired end-effector velocity trajectory (as a function of t), initial = a list of initial joint angles for θ, and time = the simulation length (0, time).

\(\(SimulateVelocityTrajectory[jg_, \ dh_, \ traj_, \ initial_, \ 
          time_:  1] := 
	
        Module[{\ vθ, \ vars, \ eq1, \ eq2, \ eq, \ 
            sol}, 
			vθ\  = \ \ jg . traj; 
			eq1\  = \ 
            MakeDiffEq[vθ, dh]; 
			vars\  = \ 
            ExtractVariables[
              dh]; 
			eq2\  = \ \(\((\(#[\([1]\)]\)[
                      0] == #[\([2]\)])\) &\)\  /@ \ 
              Transpose[{vars, \ initial}]; 
			eq\  = \ 
            Join[eq1, \ eq2]; 
			sol\  = \ 
            NDSolve[eq, \ vars, \ {t, \ 0, \ time}]\  // \ 
              Flatten; 
			vars\  /. \ sol];\)\)

Jacobian velocity trajectories

Generalized Inverse Jacobian velocity trajectories

This function simulates a trajectory specified by the velocity relationship \(TraditionalForm\`\(θ\& . \) = \(J\^*\) \(x\& . \)\), where \(TraditionalForm\`\(J\^*\)\) is the pseudo-inverse of the Jacobian (inverse in the case of square matrices). The function arguments are: dh = list of DH parameters, pick = a list of length six of 0s and 1s, which specifies which rows of the Jacobian to use, traj = desired end-effetor velocity trajectory (as a function of t), initial = a list of initial joint angles for θ, and time = the simulation length (0, time).

\(\(SimulateInverseJacobianVelocityTrajectory[dh_, \ pick_, \ traj_, \ 
          initial_, \ time_:  1] := 
	
        Module[{\ nF, \ j, jg}, 
		nF\  = \ Length[dh]; 
		j = \ 
            Jacobian1[dh, \ 0, \ nF]; 
		j = 
            Flatten[\(Take[j, \ #] &\)\  /@ \ Position[pick, 1], 1]; 
		If[
            MatchQ[Dimensions[j], \ {tmp_, \ tmp_}] == 
              True, 
			Print["Square Jacobian: Using Inverse[ ]"]; 
				jg\  = \ 
              Inverse[j]\  // \ 
                Chop, 	
			Print["Nonsquare Jacobian: Using Pseudoinverse"]; 
				jg\  = \ 
              Transpose[j] . Inverse[j . Transpose[j]]\  // \ 
                Chop]; 
		SimulateVelocityTrajectory[jg, \ dh, \ traj, \ 
            initial, \ time]
		];\)\)

Short-hand for above function

SIJVT[a__] := 
        SimulateInverseJacobianVelocityTrajectory[a];

Jacobian transpose velocity trajectories

This function simulates a trajectory specified by the velocity relationship \(TraditionalForm\`θ\& .  = \(J\^T\) x\& . \), where (J^T) is the transpose of the Jacobian. The function arguments are: dh = list of DH parameters, pick = a list of length six of 0s and 1s, which specifies which rows of the Jacobian to use, traj = desired end-effetor velocity trajectory (as a function of t), initial = a list of initial joint angles for θ, and time = the simulation length (0, time).

\(\(SimulateTransposeJacobianVelocityTrajectory[dh_, \ pick_, \ traj_, \ 
          initial_, \ time_:  1] := 
	
        Module[{\ nF, \ j, jt}, 
		nF\  = \ Length[dh]; 
		j = \ 
            Jacobian1[dh, \ 0, \ nF]; 
		j = 
            Flatten[\(Take[j, \ #] &\)\  /@ \ Position[pick, 1], 
              1]; 
		jt\  = \ 
            Transpose[j]; 
		SimulateVelocityTrajectory[jt, \ dh, \ 
            traj, \ initial, \ time]
	];\)\)

Short-hand for above function

STJVT[a__] := 
        SimulateTransposeJacobianVelocityTrajectory[a];

Inverse kinematics through Jacobian transpose

\(\(IKTJOneStep[j_, \ p_, \ dh_, \ theta_, xf_, \ dt_:  0.02\ ] := 
	
        Module[{\ rul, nF, \ jn, \ pn, 
            xc}, 
		rul\  = \(\((#[\([1]\)]\  -> \ #[\([2]\)])\) &\)\  /@ \ \(({ExtractVariables[dh], \ theta}\  // \ Transpose)\); 
		jn = \ 
            j /. \ rul\  // \ N; 
		xc\  = \ 
            p /. \ rul\  // \ N; 
		theta\  + \ 
            dt*Transpose[jn] . \((xf - xc)\)
	];\)\)

These functions provide an iterative solution to the inverse kinematics (position only, for now).

\(\(InverseKinematicsTransposeJacobian[dh_, \ pick_, \ initial_, \ xf_, \ 
          dt_] := 
	
        Module[{nF, j, p, \ res}, 
		nF\  = \ Length[dh]; 
		j = \ 
            Jacobian1[dh, \ 0, \ nF]; 
		j = 
            Flatten[\(Take[j, \ #] &\)\  /@ \ Position[pick, 1], 
              1]; 
		p\  = \ P[dh, \ 0, \ nF]; 
		p\  = 
            Flatten[\(Take[p, \ #] &\)\  /@ \ Position[pick, 1], 
              1]; 
		res\  = \ 
            FixedPointList[IKTJOneStep[j, p, dh, \ #, \ xf, dt] &, \ 
              initial, 
			SameTest -> \((Sqrt[\((#1 - #2)\) . \((#1 - #2)\)] < 0.0001 &)\)]; 
		\(ListInterpolation[#, \ {{0, \ 
                    dt*\((Length[res] - 1)\)}}] &\)\  /@ \ 
            Transpose[res]
	];\)\)

Short-hand for above function

IKTJ[a__] := InverseKinematicsTransposeJacobian[a];