General definitions (not manipulator specific)

Load packages and turn off specific messages

Turn off annoying warnings messages

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

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

Created by Mathematica  (October 1, 2003)