Definitions

Turn off annoying warnings

Off[General :: spell] ; Off[General :: spell1] ;

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[ ... 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_] := <br />    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_] := <br />    Module[{kx, ky, kz, v, c, s}, <br /> &nb ... y 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_] := <br />    Module[{r, p}, <br />    &nb ... nbsp;   RAndPToT[Transpose[r], -Transpose[r] . p] <br />    ] ;

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

Created by Mathematica  (October 21, 2003)