Manipulator specific functions

Static forward kinematic definitions

In this notebook, DH parameters should be speficied as table, with one entry per link in the form {α_ (i - 1), a_ (i - 1), d_i, θ_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___}] := <br />     {If ... ha], 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___}] := <br />    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_] := <br />        Which[<br /> &nbs ... nbsp;         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]] ;

Velocity forward kinematics definitions

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_] := <br />    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_] := <br />    R[dh, i, i - 1] . (v[dh, i - 1, i - 1] + Cross ... - 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_] := <br />    R[dh, j, i] . v[dh, i, i] ;

Jacobian (from recursive equations)

Variable extraction and creation routines (from DH parameters)

ExtractVariables[dh_] := <br />    Module[{vars}, <br />    ... nbsp;    Take[vars, #] & /@ Position[NumberQ /@ vars, False] // Flatten] ;

Compute linear velocity Jacobian from recursive equations

LinearJacobian1[dh_, j_, i_] := <br />    Module[{vars, vel, j1}, <br />&n ... sp;      j1<br />        ] ;

Compute angular velocity Jacobian from recursive equations

AngularJacobian1[dh_, j_, i_] := <br />    Module[{vars, vel, j1}, <br />& ... sp;      j1<br />        ] ;

Compute full Jacobian from recursive equations

Jacobian1[dh_, j_, i_] := <br />    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_] := <br />    Module[{vars, p, j2}, <br />&nbs ... sp;      j2<br />        ] ;

Compute angular velocity Jacobian from forward kinematic homogeneous transform

AngularJacobian2[dh_, j_, i_] := <br />    Module[{vars, r, r2, Ω, j2 ... sp;      j2<br />        ] ;

Compute full Jacobian from forward kinematic homogeneous transform

Jacobian2[dh_, j_, i_] := <br />    Join[LinearJacobian2[dh, j, i], AngularJacobian2[dh, j, i]] ;

Manipulator-specific animation functions

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

ExtractMaximalPlotRange[ranges_, l_:2] := <br />    Module[{r2}, <br />&nb ... ]}, {Min[r2[[3]]], Max[r2[[4]]]}, {Min[r2[[5]]], Max[r2[[6]]]}}] <br />    ] ;

AnimateGraphicsList[traj_, l_:2] := <br />    Module[{f, g, plrng}, <br /> ... bsp;Show[f[#], PlotRange->plrng, FrameTrue, AspectRatio->Automatic] & /@ traj] ;

AnimateTrajectory[dh_, traj_, l_:2, n_:20, tfin_:False] := <br />    Modul ...    AnimateGraphicsList[Table[ConvertTrajToGraphics[traj2], {t, 0, tf, tf/n}], l]] ;

Created by Mathematica  (October 1, 2003)