Complete manipulator definitions

This notebook supercedes all previous definition notebooks, including all previous as well as new definitions.

General definitions (not manipulator specific)

Generic definitions

Turn off annoying warnings messages

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

Load in necessary packages

<< LinearAlgebra`MatrixManipulation`;
<< Graphics`Shapes`;
<< Graphics`Colors`;
<< Graphics`Graphics3D`;

Nice view of matrices and vectors

DisplayMatrixOrVector[m_] := m // MatrixForm  //  
        TraditionalForm

Conversion of table to rules list

ConvertTableToRules[tbl_] := (Rule  @@  #) &  /@  
          tbl;

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[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 [(Subscript[R, k])(θ)]

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

Cubic trajectory generation (continuous acceleration at via points)

Nice output of vectors

ViewSolution[
        v_] :=  v  // 
          TableForm[#, TableAlignments -> {Left}] &  //  
        TraditionalForm

Cubic polynomial definition

Cubic[
          coeff_, {t_,  ts_}] := 
Module[{a0, a1, a2, 
            a3}, 
{a0, a1, a2, a3}  =  
            coeff; 
a0  +  a1 (t - ts)  +  
            a2 (((t - ts))^2) + 
            a3 (((t - ts))^3)
];

Cubic short-cut functions

cubicVia[a_, n_, teval_] := 
Cubic[
          Table[a[n, i], {i, 0, 3}], {teval,  0}];

DcubicVia[a_,  n_,  teval_] := 
        D[cubicVia[a, n, t], t]  /.  t -> teval;

DDcubicVia[a_,  n_,  teval_] := 
        D[cubicVia[a, n, t], {t, 2}]  /.  t -> teval;

eqFormat[lhs_,  rhs_, flag_:  False] := 
If[
          flag  ==  False, 
lhs  -  
            rhs, 
lhs  ==  rhs];

Set up cubic solution equations given point path specification (with via points)

The functions below assume that the first point in the path description occurs at time = 0.

CubicEquations[pts_,  flag_:  False,  dθ0_:  0, 
          dθf_:  0] := 
Module[{ 
            tlist,  θ,  tintervals, n, 
            eqs}, 
{tlist, θ}  =  
            Transpose[
              pts]; 
tintervals  =  (#[[2]]  -  #[[1]]) &  /@  
              Partition[tlist,  2,  1]; 
n  =  
            Length[pts];   (* 
            number of points  =  
              via points  +  
                2 end points *) 
eqs  =  
            Table[0, {4 (n - 1)}]; 
 (* 
            end point constraints *) 
eqs[[1]]  =  
            eqFormat[θ[[1]] , cubicVia[a,  1,  0],  
              flag]; 
eqs[[2]]  =  
            eqFormat[dθ0 ,  DcubicVia[a,  1,  0],  
              flag]; 
eqs[[4 (n - 1) - 1]]  =  
            eqFormat[Last[θ] ,  
              cubicVia[a, n - 1,  tintervals[[n - 1]]], 
              flag]; 
eqs[[4 (n - 1)]]  =  
            eqFormat[dθf ,  
              DcubicVia[a, n - 1,  tintervals[[n - 1]]], 
              flag]; 
 (* position,  
            velocity and acceleration continuity constraints *) 
Do[
eqs[[4 (i - 1) + 
                    3]]  =  
              eqFormat[θ[[i + 1]] , 
                cubicVia[a, i, tintervals[[i]]],  
                flag];  
eqs[[4 (i - 1) + 
                    4]]  =  
              eqFormat[θ[[i + 1]] , cubicVia[a,  i + 1,  0],  
                flag]; 
eqs[[4 (i - 1) + 5]]  =  
              eqFormat[DcubicVia[a, i, tintervals[[i]]] , 
                DcubicVia[a, i + 1, 0], 
                flag]; 
eqs[[4 (i - 1) + 6 ]]  = 
              eqFormat[DDcubicVia[a, i, tintervals[[i]]] , 
                DDcubicVia[a, i + 1, 0], flag],  {i, 1, 
              n - 2}
]; 
eqs
];

Set up cubic solution equations with subscripted coefficients (for display)

CubicEquationsDisplay[pts_, dθ0_:  0, dθf_:  0] := 
        CubicEquations[pts,  True, dθ0,  dθf]  /.  
          a[i_,  j_] -> Subscript[a, i*10 + j];

Solve for coefficients of cubic trajectory with via points

CubicCoefficients[pts_,  dθ0_:  0, 
          dθf_:  0] := 
Module[{eqs,  
            aList}, 
eqs  =  
            CubicEquations[pts,  True,  dθ0,  
              dθf]; 
aVec  =  
            Table[a[i, j], {i, 1, Length[pts] - 1}, {j, 0, 3}]  //  
              Flatten; 
Solve[eqs,  aVec]  //  
            Flatten
];

Solve for coefficients of cubic trajectory with subscripted coefficients (for display)

CubicCoefficientsDisplay[pts_,  dθ0_:  0, dθf_:  0] := 
        CubicCoefficients[pts, dθ0,  dθf]  /.  
          a[i_,  j_] -> Subscript[a, i*10 + j];

Extract matrix representation of solution equations

CubicCoefficientMatrices[pts_, dθ0_:  0, 
          dθf_:  0] := 
Module[{eqs, aMat,  bMat,  
            aVec, mat}, 
eqs  =  
            CubicEquations[pts,  False, dθ0,  
              dθf]; 
aVec  =  
            Table[a[i, j], {i, 1, Length[pts] - 1}, {j, 0, 3}]  //  
              Flatten; 
aMat  =  Coefficient[#, 
                  aVec] &  /@  
              eqs; 
bMat  =  (eqs  -  
                  aMat . aVec)  //  
              Simplify; 
mat = {aMat, 
              aVec  /. a[i_,  j_] -> Subscript[a, i*10 + j], 
              bMat}; 
(mat[[1]]  //  
                  TraditionalForm) . ( ({#} & /@  
                      mat[[2]])  //  
                  TraditionalForm) == (({#} & /@  
                    mat[[3]])  //  
                TraditionalForm)
];

Cubic polynomial trajectory

u[t_, {t0_, tf_}] := 
        If[t ≥  t0  &&  t  <  tf, 1, 0];

CubicPolyTrajectory[pts_, dθ0_:  0, 
          dθf_:  0] := 
Module[{tintervals, 
            coeff}, 
tintervals  =  
            Partition[(pts  //  Transpose)[[1]], 2, 
              1]; 
coeff  =  
            CubicCoefficients[pts, dθ0,  
              dθf]; 
(Plus  @@  
                Table[Cubic[
                        Table[a[i, j], {j, 0, 3}], {t, 
                          tintervals[[i, 1]]}] 
                      u[t, tintervals[[i]]]  /.  coeff, {i, 1, 
                    Length[pts] - 1}]) + 
            If[t ≥ Last[pts][[1]], 
              Evaluate[Last[pts][[2]]], 
              0]
];

Cubic polynomial trajectories in multiple dimensions

MultipleCubicPolyTrajectories[
          pts_] := 
Module[{tvec,  θs,  
            pts2}, 
tvec  =  
            First  /@  
              pts; 
pts2  =  Transpose[{tvec, #}] &  /@  Drop[Transpose[pts], 1]; 
CubicPolyTrajectory  /@  
            pts2
];

Simple cubic trajectory generation

The functions below assume that the first point in the cubic path description occurs at time = 0.

Set up  simple cubic solution equations

SimpleCubicEquations[{t0_, θ0_, 
            dθ0_}, {tf_, θf_, 
            dθf_}] := 
CubicEquations[{{t0, θ0}, {tf, θf}}, True, dθ0, dθf];

Set up cubic solution equations with subscripted coefficients (for display)

SimpleCubicEquationsDisplay[p0_, pf_] := 
        SimpleCubicEquations[p0, pf]  /.  
          a[i_,  j_] -> Subscript[a, j];

Solve for simple cubic coefficients

SimpleCubicCoefficients[{t0_, θ0_, 
            dθ0_}, {tf_, θf_, 
            dθf_}] := 
CubicCoefficients[{{t0, θ0}, {tf, θf}}, dθ0, dθf];

Solve for coefficients of cubic trajectory with subscripted coefficients (for display)

SimpleCubicCoefficientsDisplay[p0_, pf_] := 
        SimpleCubicCoefficients[p0, pf]  /.  
          a[i_,  j_] -> Subscript[a, j];

Simple cubic polynomial trajectory

SimpleCubicPolyTrajectory[{t0_, θ0_, 
          dθ0_}, {tf_, θf_, 
          dθf_}] := 
CubicPolyTrajectory[{{t0, θ0}, {tf, θf}}, dθ0, dθf]

Cubic trajectory generation (velocity heuristic at via points)

Slope computation

DetermineSlope[{{t0_, θ0_}, {tf_, θf_}}] := 
(θf  -  θ0)/(tf  -  
              t0);

Compute velocity at via points

VelocityAtViaPoint[{m1_, m2_}] := 
If[
          Sign[m1]  ==  Sign[m2], m1 + m2/2,  0];

Generate velocities at via points

VelocityAugmentedPoints[pts_,  dθ0_:  0, 
          dθf_:  0] := 
Module[{dVias,  
            vel}, 
dVias  =  
            VelocityAtViaPoint  /@  
              Partition[DetermineSlope  /@  Partition[pts, 2, 1], 2, 
                1]; 
vel = {dθ0}~Join~dVias~
              Join~{dθf}; 
Transpose[
            Transpose[pts]~Join~{vel}]
];

Cubic polynomial trajectory with velocity heuristic

CubicPolyTrajectoryWithVelocityHeuristic[pts_,  dθ0_:  0, 
          dθf_:  0] := 
Module[{pts2, traj1, 
            traj2}, 
pts2  =  
            Partition[VelocityAugmentedPoints[pts, dθ0, dθf], 
              2, 1]; 
traj1 = SimpleCubicPolyTrajectory[#[[1]], #[[2]]] &  /@  
              pts2; 
Plus  @@  (First  /@  
                  traj1)  +  
            Last[Last[traj1]]
];

Cubic polynomial trajectories in multiple dimensions

MultipleCubicPolyTrajectoriesWithVelocityHeuristic[
          pts_] := 
Module[{tvec,  θs,  
            pts2}, 
tvec  =  
            First  /@  
              pts; 
pts2  =  Transpose[{tvec, #}] &  /@  Drop[Transpose[pts], 
                1]; 
CubicPolyTrajectoryWithVelocityHeuristic  /@  
            pts2
];

Cubic trajectory functional conversion routines

Convert point-specified trajectory to table

MultiplePointsToTable[
          pts_, {tmin_, tmax_, dt_}] := 
Module[{poly,  
            tTbl, trajTbl}, 
poly  =  
            MultipleCubicPolyTrajectories[pts]; 
tTbl  =  
            Table[t, {t, tmin, tmax, dt}]  //  
              N; 
trajTbl  =  ((poly  /.  
                        t -> #) &  /@  tTbl)  //  
              Transpose; 
Transpose[{tTbl,  #}] &  /@  trajTbl
];

Convert table trajectory to interpolation function

MultipleTableToInterpolationFunction[
          tbl_] := 
Interpolation[#, 
              InterpolationOrder -> 3] &  /@ tbl;

Convert point-specified trajectory to interpolation function

MultiplePointsToInterpolationFunction[
          pts_, {tmin_, tmax_, 
            dt_}] := 
MultipleTableToInterpolationFunction[
          MultiplePointsToTable[pts, {tmin, tmax, dt}]];

Convert polynomial trajectory to table

MultiplePolyToTable[
          poly_, {tmin_, tmax_, dt_}] := 
Module[{tTbl, 
            trajTbl}, 
tTbl  =  
            Table[t, {t, tmin, tmax, dt}]  //  
              N; 
trajTbl  =  ((poly  /.  
                        t -> #) &  /@  tTbl)  //  
              Transpose; 
Transpose[{tTbl,  #}] &  /@  trajTbl
];

Convert polynomial trajectory to interpolation function

MultiplePolyToInterpolationFunction[
          poly_, {tmin_, tmax_, 
            dt_}] := 
MultipleTableToInterpolationFunction[
          MultiplePolyToTable[poly, {tmin, tmax, dt}]];

Cubic trajectory plotting routines

Plot polynomial trajectory with points

PlotPolyTrajectory[pts_, {tmin_, tmax_}, color_:  Black, 
          fnc_:  CubicPolyTrajectory, dθ0_:  0, dθf_:  0,  
          opts___] := 
Module[{poly, g1, 
            g2}, 
poly  =  
            fnc[pts, dθ0, dθf]; 
g1 = 
            Plot[Evaluate[poly], {t, tmin, tmax},  
              DisplayFunction -> Identity, 
              PlotStyle -> color]; 
g2 = 
            Show[Graphics[{RGBColor[1, 0, 0], PointSize[ .02], 
                  Point /@ pts}], 
              DisplayFunction -> Identity]; 
Show[g1, 
            g2, PlotRange -> All, Frame -> True, 
            Axes -> False, DisplayFunction -> $DisplayFunction, 
            opts]
];

Plot derivative of polynomial trajectory

PlotDPolyTrajectory[pts_, {tmin_, tmax_}, color_:  Black, 
          fnc_:  CubicPolyTrajectory, dθ0_:  0, dθf_:  0,  
          opts___] := 
Module[{poly, 
            g1}, 
poly  =  
            fnc[pts, dθ0, dθf]; 
g1 = 
            Plot[Evaluate[D[poly, t]], {t, tmin, tmax},  
              DisplayFunction -> Identity, 
              PlotStyle -> color]; 
Show[g1, 
            PlotRange -> All, Frame -> True, Axes -> False, 
            DisplayFunction -> $DisplayFunction, 
            opts]
];

Plot 2nd derivative of polynomial trajectory

PlotDDPolyTrajectory[pts_, {tmin_, tmax_}, color_:  Black, 
          fnc_:  CubicPolyTrajectory, dθ0_:  0, dθf_:  0, 
          opts___] := 
Module[{poly, 
            g1}, 
poly  =  
            fnc[pts, dθ0, dθf]; 
g1 = 
            Plot[Evaluate[D[poly, {t, 2}]], {t, tmin, tmax},  
              DisplayFunction -> Identity, 
              PlotStyle -> color]; 
Show[g1, 
            PlotRange -> All, Frame -> True, Axes -> False, 
            DisplayFunction -> $DisplayFunction, 
            opts]
];

Multi-dimensional path plotting

PlotMultiplePointsTrajectories[pts_, {tmin_, tmax_}, colors_, 
          fnc_:  MultipleCubicPolyTrajectories,  
          opts___] := 
Module[{poly, gpts, tlist, g1, 
            g2}, 
poly  =  
            fnc[pts]; 
tlist  =  
            First  /@  pts; 
gpts  =  
            Partition[
              Flatten[Transpose[{tlist, #}] &  /@  
                  Drop[Transpose[pts], 1]], 2]; 
g1 = 
            Plot[Evaluate[poly], {t, tmin, tmax},  
              DisplayFunction -> Identity, 
              PlotStyle -> colors]; 
g2 = 
            Show[Graphics[{Black, PointSize[ .02], Point /@ gpts}], 
              DisplayFunction -> Identity]; 
Show[g1, 
            g2, PlotRange -> All, Frame -> True, 
            Axes -> False, DisplayFunction -> $DisplayFunction, 
            opts]
];

General differential equations definitions

Differential equations conversion routines

\(\(IsolateSecondDerivatives[eqsAll_, 
          vars_] := 
Module[{ddvars, \ ddcoeff, \ eqs, \ 
            fvec}, 
ddvars\  = \ \(Dt[#, {t, 2}] &\)\  /@ \ 
              vars; 
eqs\  = \ \(#[\([1]\)] &\)\  /@ \ 
              eqsAll; 
fvec\  = \ \(#[\([2]\)] &\)\  /@ \ 
              eqsAll; 
ddcoeff\  = 
            Outer[Coefficient[#1, \ #2] &, \ eqs, ddvars]\  // \ 
              Simplify; 
remainder\  = \ \((eqs\  - \ 
                  ddcoeff . ddvars)\)\  // \ Simplify; 
			Inverse[
              ddcoeff] . \((fvec - 
                remainder)\)
];\)\)

MakeGeneralDiffEq[eqsAll_,  
          vars_] := 
Module[{vt, eqs,  peq1,  
            eq1}, 
vt  =  (#[t]) &  /@  
              vars; 
eqs  = 
            IsolateSecondDerivatives[eqsAll,  
              vars]; 
peq1  =  
            eqs  /.  ((# -> #[t]) &  /@  
                  vars); 
eq1  =  (#[[1]] == #[[2]]) &  /@  
              Transpose[{D[#, {t, 2}] &  /@  vt,  
                  peq1}]; 
eq1  //  
            Chop
];

MakeInitialCondDiffEq[vars_,  xinit_,  dxinit_,  
          tcurr_] := 
Module[{vt,  eq2,  
            eq3}, 
vt  =  (#[t]) &  /@  
              vars; 
eq2  =  (#[[1]]  ==  #[[2]]) &  /@  Transpose[{vt  /.  t -> tcurr, xinit}]; 
          eq3  =  (#[[1]]  ==  #[[2]]) &  /@  
              Transpose[{D[vt, t]  /.  t -> tcurr,  dxinit}]; 
          Flatten[{eq2, eq3}]
];

Static differential equation solver

SimulateDiffEqStatic[deqA_,  vars_,  
          h_, {xinit_,  dxinit_, tmin_}, 
          opts___] := 
Module[{deqB,  deqs,  tmax,  
            sol,  xinitNew,  dxinitNew}, 
deqB  =  
            MakeInitialCondDiffEq[vars,  xinit,  dxinit,  
              tmin]; 
deqs  =  
            deqA~Join~deqB; 
tmax  =  
            tmin + h; 
sol  =  
            Flatten[vars  /.  
                NDSolve[deqs,  vars,  {t, tmin, tmax}, 
                  opts]]; 
sol
];

Solve one period

SimulateDiffEqOne[deqA_,  vars_,  h_, {xinit_,  dxinit_, tmin_}, 
          opts___] := 
Module[{deqB,  deqs,  tmax,  
            sol,  xinitNew,  dxinitNew}, 
deqB  =  
            MakeInitialCondDiffEq[vars,  xinit,  dxinit,  
              tmin]; 
deqs  =  
            deqA~Join~deqB; 
tmax  =  
            tmin + h; 
sol  =  
            Flatten[vars  /.  
                NDSolve[deqs,  vars,  {t, tmin, tmax}, 
                  opts]]; 
xinitNew  =  (#[t]  /.  
                    t ->  tmax) &  /@  
              sol; 
dxinitNew  =  (D[#[t],  
                      t]  /.  t -> tmax) &  /@  
              sol; 
{xinitNew,  dxinitNew, 
            tmax}
];

Solve all

SimulateDiffEqAll[eqs_,  vars_,  {xinit_,  dxinit_},  tmax_,  
          step_,  opts___] := 
Module[{deqA, traj,  
            ttraj,  xtraj}, 
deqA  =  
            MakeGeneralDiffEq[eqs,  vars]; 
traj  =  
            NestList[
              SimulateDiffEqOne[deqA,  vars,  step,  #,  
                  opts] &,  {xinit,  dxinit, 0},  
              Ceiling[tmax/step]]; 
ttraj  =  Transpose[
                traj][[3]]; 
xtraj  =  
            Transpose[Transpose[Drop[#, -1]] &  /@  
                traj]; 
Interpolation[
                Transpose[{ttraj, #}]] &  /@  
            xtraj
];

General controller definitions

Generate desired and error values for current time

GenerateControllerInputs[
          trajd_,  {x_,  dx_,  
            tcurr_}] := 
Module[{xd,  dxd,  ddxd, err, 
            derr}, 
xd  =  #[tcurr] &  /@  
              trajd [[1]]; 
dxd  =  #[
                  tcurr] &  /@  
              trajd[[2]]; 
ddxd  =  #[
                  tcurr] &  /@  
              trajd[[3]]; 
err  =  
            xd  -  x; 
derr  =  
            dxd  -  dx; 
{xd,  dxd,  ddxd,  err,  
            derr}
];

Simulate one controller step

ControlOne[deqA_,  vars_,  fvars_,  
          h_, {x_,  dx_, f_, counter_, tcurr_}, {controlLaw_, desired_,  
            params___}, 
          opts___] := 
Module[{deqB,  deqs,  tmax,  
            sol,  xNew,  dxNew,  fNew,  frule,  xd, dxd, ddxd, err, 
            derr}, 
deqB  =  
            MakeInitialCondDiffEq[vars,  x,  dx,  
              tcurr]; 
frule  =  (#[[1]] -> #[[2]]) &  /@  
              Transpose[{fvars,  f}]; 
deqs  =  
            deqA~Join~deqB  /.  frule; 
tNew =  
            tcurr + h; 
sol  = 
Flatten[
              vars  /.  NDSolve[deqs,  vars,  {t, tcurr, tNew}, 
                  opts]]; 
xNew  =  (#[t]  /.  
                    t ->  tNew) &  /@  
              sol; 
dxNew  =  (D[#[t],  t]  /.  
                    t -> tNew) &  /@  sol; 
{xd, 
              dxd,  ddxd,  err,  derr}  =  
            GenerateControllerInputs[
              desired, {xNew,  dxNew,  
                tNew}]; 
IntegralError  +=  
            err*h; 
fNew  =  
            controlLaw[x,  dx,  ddxd,  err,  derr, 
              params]; 
{xNew,  dxNew, fNew,  
            counter + 1, tNew}
];

Simulate entire controller

ControlAll[eqs_,  vars_,  fvars_, {xinit_,  dxinit_},  tmax_,  
          step_,  {controlLaw_,  desired_,  params___}, 
          opts___] := 
Module[{deqA, traj,  ttraj,  
            xtraj,  finit, xtraj2,  ftraj, ftraj2, xd, dxd,  ddxd,  
            err,  derr,  nSteps}, 
nSteps  =  
            Ceiling[tmax/step]; 
deqA  =  
            MakeGeneralDiffEq[eqs,  vars]; 
{xd, dxd,  
              ddxd,  err,  derr}  =  
            GenerateControllerInputs[
              desired, {xinit,  dxinit,  
                0}]; 
IntegralError  =  
            err; 
finit  =  
            controlLaw[xinit, dxinit,  ddxd,  err,  derr, 
              params]; 
traj  =  
            NestList[
              ControlOne[deqA,  vars,  fvars,  
                  step,  #,  {controlLaw,  desired,  params}, 
                  opts] &,  {xinit,  dxinit, finit, 2, 0}, 
              nSteps]; 
ttraj  =  
            Last[Transpose[traj]]; 
xtraj  =  
            Transpose[Transpose[Drop[#, -3]] &  /@  
                traj]; 
xtraj2  =  Interpolation[
                  Transpose[{ttraj, #}]] &  /@  
              xtraj; 
ftraj  =   Transpose[#[[3]] &   /@  traj]; 
ftraj2  =  Interpolation[
                  Transpose[{ttraj,  #}]] &  /@  
ftraj; 
{xtraj2,  
            ftraj2} 
];

General plotting routines

Short cuts

nS = DisplayFunction -> Identity;
yS = 
        DisplayFunction -> $DisplayFunction;
dsh  =  Dashing[{0.015, 0.015}];
imgSize  =  ImageSize -> 288;

Combine interpolation functions

CombineInterpolationFunctions[traj1_, traj2_, {tmin_, tmax_}, 
          fnc_] := 
Module[{f1, 
            f2}, 
f1  =  #[t] &  /@  
              traj1; 
f2  =  #[t] &  /@  
              traj2; 
FunctionInterpolation[#, {t, tmin, 
                  tmax}] &  /@  
            fnc[f1, f2]
];

Plot a multiple dimensional trajectory (from a list of interpolation)

PlotMultipleTrajectory[traj_, {tmin_, tmax_}, plStyleList_, 
          opts___] := 
Plot[
          Evaluate[#[t] &  /@  traj], {t,  tmin, tmax}, 
          PlotStyle -> plStyleList, Frame -> True, 
          PlotRange -> All, opts];

Manipulator specific definitions

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[{α_,  a_,  d_,  θ_,  
            type___}] := {If[NumericQ[α],  α Degree,  
              alpha],  a,  d,  
            If[NumericQ[θ],  θ Degree,  θ],  
            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}  (\(TraditionalForm\`\(\^j\)\(\(\_i\)\(T\)\)\))

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

Rotation matrix for robot from frame {j} to frame {i} (\(TraditionalForm\`\(\^j\)\(\(\_i\)\(R\)\)\))

R[dh_, i_, j_] := RFromT[T[dh,  i,  j]];

Position vector for robot from frame {j} to frame {i} ((^i)(Subscript[P, j, 
            ORG]))

P[dh_, i_, j_] := PFromT[T[dh,  i,  j]];

Velocity forward kinematics definitions

Angular velocity of link {i} in frame {j} coordinates ((^i)(Subscript[ω, i]), (^j)(Subscript[ω, i]))

ω[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 ((^i)(Subscript[v, i]), (^j)(Subscript[v, i]))

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)

Variable extraction and creation routines (from DH parameters)

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

ExtractFirstDerivatives[
          dh_] := 
Module[{vars}, 
vars  =  ExtractVariables[dh]; 
Dt[#, t] &  /@  
            vars
];

\(\(MakeVelocityVariables[
          dh_] := 
Module[{lvars}, 
lvars\  = \ Length[ExtractVariables[dh]]; 
          Table[ToExpression["V" <> ToString[i]], {i, 1, 
              lvars}]
];\)\)

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

Static force definitions

Propagation of static forces in robot manipulators

fs[dh_, i_, i_,  fend_: {fx, fy, 0}] := 
If[
          i  ==  Length[dh], fend, 
          R[dh, i, i + 1] . fs[dh,  i + 1,  i + 1,  fend]];

Propagation of static moments in robot manipulators

ns[dh_, i_, i_,  fend_: {fx, fy, 0}] := 
If[
          i  ==  Length[dh],  {0, 0, 0}, 
          R[dh, i, i + 1] . 
              ns[dh, i + 1, i + 1,  fend] + (P[dh, i, i + 1]~
                Cross~(R[dh, i, i + 1] . 
                    fs[dh, i + 1, i + 1,  fend]))];

Torques required to support static forces and moments at end-effector

τs[dh_, i_, fend_: {fx, fy, 0}] := 
If[
          dh[[i, 5]]  ==  
            0,  ns[dh, i, i,  fend][[3]], fs[dh, i, i, 
              fend][[3]]];

Dynamics definitions

Angular acceleration  of link {i} in frame {j} coordinates (\(TraditionalForm\`\(\^i\)\(\(ω\& . \)\_i\)\), \(TraditionalForm\`\(\^j\)\(\(ω\& . \)\_i\)\))

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

α[dh_,  i_,  i_] := 
        R[dh, i, i - 1] . α[dh, i - 1, 
              i - 1] + (R[dh, i, i - 1] . ω[dh, i - 1, i - 1])~
            Cross~If[dh[[i, 5]]  ==  0,  
              Dt[dh[[i, 4]], t] {0, 0, 1}, {0, 0, 0}]  + 
          If[dh[[i, 5]]  ==  0,  
            Dt[dh[[i, 4]],  {t, 2}] {0, 0, 1}, {0, 0, 0}];

α[dh_,  j_,  i_] := 
        R[dh,  j,  i] . α[dh,  i, i];

Linear acceleration  of link {i} in frame {j} coordinates (\(TraditionalForm\`\(\^i\)\(\(v\& . \)\_i\)\), \(TraditionalForm\`\(\^j\)\(\(v\& . \)\_i\)\))

a[dh_, 0, 0] := {0, g, 0};

a[dh_,  i_,  i_] := 
        R[dh, i,  
              i - 1] . (α[dh, i - 1, i - 1]~Cross~
                  P[dh, i - 1, i] + ω[dh, i - 1, i - 1]~
                  Cross~(ω[dh, i - 1, i - 1]~Cross~
                      P[dh, i - 1, i]) + a[dh,  i - 1,  i - 1]) + 
          2  ω[dh, i, i]~Cross~
              If[dh[[i, 5]]  ==  1, 
                Dt[dh[[i, 3]], t] {0, 0, 1},  {0, 0, 0}]  +  
          If[dh[[i, 5]] == 1, 
            Dt[dh[[i, 3]], {t, 2}] {0, 0, 1}, {0, 0, 0}];

a[dh_,  j_,  i_] := R[dh,  j,  i] . a[dh,  i, i];

Linear acceleration of center of mass of link {i} in frame {j} coordinates (\(TraditionalForm\`\(\^i\)\(\(v\& . \)\_\(C\_i\)\)\), \(TraditionalForm\`\(\^j\)\(\(v\& . \)\_\(C\_i\)\)\))

ac[{dh_, Pc_},  i_, 
          i_] := α[dh, i, i]~Cross~
            Pc[[i + 1]] + ω[dh, i, i]~
            Cross~(ω[dh, i, i]~Cross~Pc[[i + 1]]) + 
          a[dh, i, i];

ac[{dh_,  Pc_},  j_,  i_] := 
        R[dh, j, i] . ac[{dh, Pc}, i, i];

Force forward propagation ((^i)(Subscript[F, i]), (^i)(Subscript[N, i]))

F[{dh_, in_, m_, Pc_}, i_, i_] := 
        m[[i + 1]] ac[{dh, Pc},  i, i];

Nd[{dh_, in_, m_, Pc_}, i_, i_] := 
        in[[i + 1]] . α[dh,  i, i]  +  ω[dh,  i, i]~
            Cross~(in[[i + 1]] . ω[dh, i, i]);

Force backward propagation ((^i)(Subscript[f, i]), (^i)(Subscript[n, i]), (Subscript[τ, i]))

f[{dh_, in_, m_, Pc_}, i_, i_] := 
If[
          i  ==  Length[dh],  {0, 0, 0}, 
          R[dh, i, i + 1] . f[{dh,  in,  m,  Pc},  i + 1,  i + 1]  +  
            F[{dh,  in,  m,  Pc},  i, 
              i]
];

n[{dh_, in_, m_, Pc_}, i_, i_] := 
If[
          i  ==  Length[dh],  {0, 0, 0}, 
          Nd[{dh,  in,  m,  Pc}, i, i] + 
            R[dh, i, i + 1] . 
              n[{dh,  in,  m,  Pc}, i + 1, i + 1] + (Pc[[i + 1]]~
                Cross~F[{dh,  in,  m,  Pc}, i, i]) + (P[dh, i, i + 1]~
                Cross~(R[dh, i, i + 1] . 
                    f[{dh,  in,  m,  Pc}, i + 1, 
                      i + 1]))];

τ[{dh_, in_, m_, Pc_}, i_] := 
If[
          dh[[i, 5]]  ==  
            0,  n[{dh,  in,  m,  Pc}, i, 
              i][[3]], 
f[{dh,  in,  m,  Pc}, i, 
              i][[3]]];

\(TraditionalForm\`τ\  = \ 
        M(Θ) + V(Θ, Θ\& . ) + 
          G(Θ)\)

τAll[{dh_, in_, m_, 
            Pc_}] := 
Table[τ[{dh,  in,  m,  Pc}, 
              i]  //  TrigReduce, {i, 1, Length[dh] - 1}];

Separate M(Θ) matrix, and V(Θ, \(TraditionalForm\`Θ\& . \)) and G(Θ) vectors

τMatrixFromModel[τvec_,  
          vars_] := 
Module[{MM,  VV,  
            GV}, 
MM  =  
            Outer[Coefficient[#1,  Dt[#2,  {t, 2}]] &,  τvec,  
              vars];  GV  =  
            Flatten[Outer[Coefficient[#1,  #2] &,  τvec,  {g}]*g]; 
          VV  =  Flatten[τvec  -  
                  MM . ( Dt[#,  {t, 2}] &  /@  vars)  -  
                  GV  //  Simplify]; 
{MM,  VV,  
              GV}  //  Chop
];

τMatrix[{dh_, in_, m_, 
            Pc_}] := 
Module[{τvec, 
            vars}, 
vars  =  
            ExtractVariables[
              dh]; 
τvec  =  τAll[{dh,  in,  
                m,  Pc}]; 
τMatrixFromModel[τvec,  
            vars]
];

\(TraditionalForm\`Θ\&¨ = \(\(M(Θ)\)\^\(-1\)\)[τ\  - \ 
            V(Θ, Θ\& . ) - 
            G(Θ)]\)

ManipulatorDifferentialEquations[{dh_, in_, m_, Pc_},  τt_] := 
        Module[{τvec,  mm,  vars,  ddvars}, 
          vars  =  ExtractVariables[dh]; 
          ddvars  =  Dt[#, {t, 2}] &  /@  
              vars; τvec  =  τ[{dh,  in,  m,  
                    Pc},  #] &  /@  Table[i, {i, 1, Length[dh] - 1}]; 
          mm  = Outer[Coefficient[#1,  #2] &,  τvec, ddvars]  //  
              Simplify; 
          remainder  =  (τvec  -  mm . ddvars)  //  Simplify; 
          Inverse[mm] . (τt - 
                remainder)
];

ManipulatorDiffEqsFromModel[τvec_, 
          vars_] := 
Module[{mm, vv, gv, 
            fvars}, 
{mm, vv, 
              gv} = τMatrixFromModel[τvec, 
              vars]; 
fvars  =  
            Table[ToExpression["τ" <> ToString[i]], {i, 1, 
                Length[τvec]}]; 
Inverse[
                mm] . (fvars  -  vv  -  gv)  //  
            Chop
];

Nice display for \(TraditionalForm\`τ\  = 
        M(Θ) + V(Θ, Θ\& . ) + 
          G(Θ)\)

\(\(DisplayManipulatorDynamicModel[model_, 
          i_] := 
Module[{fvar}, 
fvar\  = ToExpression["τ" <> 
                ToString[i]]; 
fvar == \ 
              model[\([i]\)]\  // \ 
            TraditionalForm
];\)\)

Nice display of M(Θ), V(Θ, \(TraditionalForm\`Θ\& . \)) and G(Θ)

DisplayManipulatorMassMatrix[{mm_, vv_, 
            gv_}] := 
(M[Θ] ==  
              DisplayMatrixOrVector[mm])  //  
          TraditionalForm;

\(\(DisplayManipulatorVelocityVector[{mm_, vv_, 
            gv_}] := 
\((V[Θ, Θ\& . ] == \ DisplayMatrixOrVector[vv])\)\  // \ 
          TraditionalForm;\)\)

DisplayManipulatorGravityVector[{mm_, vv_, 
            gv_}] := 
(G[Θ] ==  
              DisplayMatrixOrVector[gv])  //  
          TraditionalForm;

Nice display of differential equations \(TraditionalForm\`Θ\&¨ = \(\(M(Θ)\)\^\(-1\)\)[τ\  - \ 
            V(Θ, \(Θ\& . \)) - 
            G(Θ)]\)

DisplayManipulatorDifferentialEquation[model_, dh_, 
          i_] := 
Module[{vars}, 
vars  =  ExtractVariables[
              dh]; 
Dt[vars[[i]], {t, 2}]  ==  
              model[[i]]  //  
            TraditionalForm
];

Nice display of inertia tensors ((Subscript[(^C), i])(Subscript[I, i]))

DisplayManipulatorInertiaTensor[in_, 
          i_] := (""^Subscript["C", i] Subscript["I", 
                  i]  //  TraditionalForm) == (in[[i + 
                    1]]  //  TraditionalForm);

Animation functions

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]; 
If[
            l  ==  
              2, 
Show[f[#],  PlotRange -> plrng, 
                  Frame -> True,  AspectRatio -> Automatic] &  /@  
              traj, 
Show[f[#],  PlotRange -> plrng,  
                  BoxRatios -> Automatic] &  /@  
              traj]
];

AnimateTrajectory[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]  //  Chop,  {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]
];

ConvertTrajToGraphics2[l_,  c1_, 
          c2_] := 
Join[{{Thickness[ .015],  c1,  
              Line[l]}}, {PointSize[ .02],  c2,  Point[#]} &  /@  
            l];

AnimateTwoTrajectories[dh_,  traj1_,  traj2_, l_:  2,  n_:  20,  
          tfin_:  False] := 
        Module[{nF,  joints,  p0,  ptake,  vars,  rul1, rul2,  traj1b, 
            traj2b,  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]  //  Chop,  {1, ptake}], {i, 1,  
                  nF}]]; 
rul1  = (#[[1]]  ->  #[[2]][t]) &  /@  ({vars,  traj1}  //  Transpose); 
          traj1b =  
            joints  /.  
              rul1; 
rul2  = (#[[1]]  ->  #[[2]][t]) &  /@  ({vars,  traj2}  //  Transpose); 
          traj2b =  joints  /.  rul2; 
If[
            tfin  ==  False, tf  =  traj1[[1, 1, 1, 2]],  
            tf  =  tfin,  tf = tfin]; 
          AnimateGraphicsList[
            Table[ConvertTrajToGraphics2[traj1b, Yellow, Green]~Join~
                ConvertTrajToGraphics2[traj2b, Blue, Red],  {t,  0,  tf,  
                tf/n}], l]
];

Velocity kinematic simulation definitions

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 (1, 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
];

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 (1, 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];

Dynamics simulation definitions

Euler integration

eulerIntegrateOne[{tt_, θ_,  dθ_}, traj_,  dt_, dh_] := 
        Module[{var,  dvar,  r1,  r2,  ddθ, θplus, 
            dθplus}, 
var  =  
            ExtractVariables[
              dh]; 
dvar  =  Dt[#, t] &  /@  
              var; 
r1  =  (Rule  @@  #) &  /@  Transpose[{var, θ }]; 
          r2  =  (Rule  @@  #) &  /@  
              Transpose[{var,  dθ}]; 
          ddθ  =  
            traj  /. Join[r1, r2]; 
ddθ  =  
            ddθ  /.  {t -> 
                  tt}; 
dθplus  =  
            dθ  +  
              ddθ*
                dt; 
θplus  =  θ  +  
              dθ*dt  +  
              0.5*ddθ*dt*dt; 
{tt + 
              dt, θplus,  
            dθplus}
];

eulerIntegrate[{θ_,  dθ_}, traj_, dt_, steps_, dh_] := 
        Module[{path,  p2, p3}, 
path  =  
            NestList[
              eulerIntegrateOne[#,  traj,  dt,  dh] &,  {0. , θ,  
                dθ},  
              steps]; 
p2  = (Take[#, {1, 
                      2}] &  /@  
                path); 
p3  = Table[{#[[1]],  #[[2, i]]}, {i, 1, Length[#[[2]]]}] &  /@  p2; 
          Interpolation  /@  
            Transpose[p3]
];

Mathematica integration

MakeSubstitutionRules[
          dh_] := 
Module[{vars, dvars, 
            vs}, 
vars  =  
            ExtractVariables[dh]; 
dvars  =  
            ExtractFirstDerivatives[dh]; 
          vs  =  MakeVelocityVariables[
              dh]; 
{(# -> #[t]) &  /@  
                vars,  (#[[1]] -> #[[2]][t]) &  /@  
                Transpose[{dvars,  vs}]}  //  
            Flatten
];

MakeDifferentialEquations[traj_,  {θ_,  dθ_}, 
          dh_] := 
Module[{vars,  rul, peq1, vs , eq1, 
            eq2, eq3, eq4}, 
          vars  =   ExtractVariables[
              dh]; 
vt  =  (#[t]) &  /@  
              vars; 
peq1  =  
            traj  /.  ((# -> #[t]) &  /@  
                  vars); 
eq1  =  (#[[1]] == #[[2]]) &  /@  Transpose[{D[#, {t, 2}] &  /@  vt,  peq1}]; 
          eq2  =  (#[[1]]  ==  #[[2]]) &  /@  
              Transpose[{vt  /.  
                    t -> 0, θ}]; 
eq3  =  (#[[1]]  ==  #[[2]]) &  /@  
              Transpose[{D[vt, t]  /.  t -> 0,  dθ}]; 
          Flatten[{eq1, eq2, eq3}]
];

SimulateDynamics[{θ_,  dθ_}, traj_, tmax_, dh_,  
          opts___] := 
Module[{vars, 
            eqs}, 
vars  =  
            ExtractVariables[dh]; 
eqs  =  
            MakeDifferentialEquations[traj,  {θ,  dθ},  dh]; 
          Flatten[vars  /.  
              NDSolve[eqs,  vars, {t, 0, tmax},  
                opts]]
];

Control simulation definitions

Create desired trajectory with derivatives as interpolation function

CreateDesiredTrajectory[pts_, {tmin_, tmax_, dt_},  
          fnc_:  MultipleCubicPolyTrajectories] := 
Module[{poly}, 
poly  =  
            fnc[pts]  //  
              Chop; 
{MultiplePolyToInterpolationFunction[
              poly, {tmin, tmax, 
                dt}], 
MultiplePolyToInterpolationFunction[
              Evaluate[D[poly, t]], {tmin, tmax, 
                dt}], 
MultiplePolyToInterpolationFunction[
              Evaluate[D[poly, {t, 2}]], {tmin, tmax, 
                dt}]}
];

Manipulator-specific controller functions

This function expects as input, the dynamic model (as output from τAll[...]), the DH parameters, {tvia, θvia}, which represent the points of the desired trajectory, the frequency of control updates, the length of simulation tmax, the controlLaw to apply, and any parameters that serve as input to the controlLaw function.
The output of this function is a list {desired, actual, torques} which are the desired trajectory (cubic function), the actual trajectory, and the corresponding torques.

ControlManipulatorFromModel[model_,  dh_, pts_,  freq_,  
          tmax_,  {controlLaw_,  params___},  
          opts___] := 
Module[{des, ddes, dddes, fvars, 
            vars,  eqs,  desired,  θinit,  dθinit,  
            traj}, 
vars  =  
            ExtractVariables[dh]; 
fvars  =  
            Table[ToExpression["τ" <> ToString[i]], {i, 1, 
                Length[model]}]; 
          eqs  =  (#[[1]] == #[[2]]) &  /@  
              Transpose[{model, fvars}]; 
desired  = 
            CreateDesiredTrajectory[
              pts, {0. , tmax,  
                1/freq}]; 
θinit  =  
            Drop[First[pts], -1]; 
dθinit  =  
            Table[0, {Length[vars]}]; 
traj  =  
            ControlAll[eqs ,  vars,  fvars, {θinit, dθinit}, 
              tmax, N[1/freq], {controlLaw,  desired, params},  opts]; 
          Partition[Flatten[{desired[[1]],  traj}],  
            Length[vars]]
];

\(\(ControlManipulatorFromModel2[model_, \ dh_, \ freq_, \ 
          tmax_, \ {controlLaw_, \ desired_, params___}, \ 
          opts___] := 
Module[{fvars, vars, \ 
            eqs, \ θinit, \ dθinit, \ 
            traj}, 
vars\  = \ 
            ExtractVariables[dh]; 
fvars\  = \ 
            Table[ToExpression["τ" <> ToString[i]], {i, 1, 
                Length[model]}]; 
eqs\  = \ \(\((#[\([1]\)] == #[\([2]\)])\) &\)\  /@ \ 
              Transpose[{model, 
                  fvars}]; 
θinit\  = \ \(#[
                    0. ] &\)\  /@ \ \((desired[\([1]\)])\)\  // \ 
              Chop; 
dθinit\  = \ 
            Table[0, {Length[vars]}]; 
traj\  = \ 
            ControlAll[eqs\ , \ vars, \ fvars, {θinit, dθinit}, 
              tmax, N[1/freq], {controlLaw, \ desired, params}, \ 
              opts]; 
Partition[
            Flatten[{desired[\([1]\)], \ traj}], \ 
            Length[vars]]
];\)\)

Sample  control laws

Control law descriptions

\(\(ControlLaws::usage\  = \ "Controller options:

" <> 
\*""\tOpenLoopControl[(Subscript[Θ, d]),\!\(Θ\& . \_d\),\!\(\(\(\\ \)\(Θ\)\)\&¨\_d\),,,{τ1,...,τn}, vars]:\n"" <> 
"		τ = {τ1,...,τn}
" <> 
\*""\t\t  = M((Subscript[Θ, d]))(Subscript[(Overscript[Θ, ¨]), d]) + V(Θ,\!\(Θ\& . \)) + G(Θ)\n\n"" <> 
\*""\tPID[,,,E, \!\(E\& . \), (Subscript[K, p]), (Subscript[K, V]), (Subscript[K, i])]:\n"" <> 
\*""\t\tτ = (Subscript[K, p]).E + (Subscript[K, V]).\!\(E\& . \) + (Subscript[K, i]).∫E\[DifferentialD]t\n\n"" <> 
\*""\tPIDWithAcceleration[,,(Subscript[(Overscript[Θ, ¨]), d]),E, \!\(E\& . \), (Subscript[K, p]), (Subscript[K, V]), (Subscript[K, i])]:\n"" <> 
\*""\t\tτ = (Subscript[K, p]).E + (Subscript[K, V]).\!\(E\& . \) + (Subscript[K, i]).∫E\[DifferentialD]t + (Subscript[(Overscript[Θ, ¨]), d])\n\n"" <> 
\*""\tGravityCompensation[(Subscript[Θ, d]),\!\(Θ\& . \_d\),(Subscript[(Overscript[Θ, ¨]), d]),E, \!\(E\& . \), (Subscript[K, p]), (Subscript[K, V]), (Subscript[K, i]),"" <> 
"{,,G(Θ)}, vars]:
" <> 
\*""\t\tτ = (Subscript[K, p]).E + (Subscript[K, V]).\!\(E\& . \) + (Subscript[K, i]).∫E\[DifferentialD]t + (Subscript[(Overscript[Θ, ¨]), d]) + G(Θ)\n\n"" <> 
\*""\tPartitionedControl[(Subscript[Θ, d]),\!\(Θ\& . \_d\),(Subscript[(Overscript[Θ, ¨]), d]),E, \!\(E\& . \), (Subscript[K, p]), (Subscript[K, V]), (Subscript[K, i]), "" <> \*""{M(Θ),V(Θ,\!\(Θ\& . \)),G(Θ)}, vars]:\n"" <> 
\*""\t\tτ = M(Θ)\!\(τ\&^\) + V(Θ,\!\(Θ\& . \)) + G(Θ) where "" <> 
\*""\!\(τ\&^\) = (Subscript[K, p]).E + (Subscript[K, V]).\!\(E\& . \) + (Subscript[K, i]).∫E\[DifferentialD]t + (Subscript[(Overscript[Θ, ¨]), d])"";\)\)

Open loop "control"

OpenLoopControl[θd_, dθd_,  ddθd_, _, _,  
          model_,  
          vars_] := 
Module[{r1, r2, r3,  dv,  
            ddv}, 
dv  =  Dt[#, t] &  /@  
              vars; 
ddv  =  Dt[#,  {t, 2}] &  /@  
              vars; 
r1  =  (#[[1]] -> #[[2]]) &  /@  
              Transpose[{vars,  θd}]; 
r2  =  (#[[1]] -> #[[2]]) &  /@  Transpose[{dv,  dθd}]; 
          r3  =  (#[[1]] -> #[[2]]) &  /@  
              Transpose[{ddv,  ddθd}]; 
          model  /.  (r1~Join~r2~Join~
                r3)
];

Simple PID control

The first three arguments are the desired θ, dθ and ddθ , and are not used for this controller.

PID[_,  _,  _,  err_,  derr_,  kp_,  kv_,  
          ki_] := 
kp . err  +  kv . derr  +  
          ki . IntegralError;

PID control with desired acceleration feedback

The first two arguments are the desired θ and dθ, and are not used for this controller.

PIDWithAcceleration[_,  _,  ddθd_,  err_,  derr_,  kp_,  
          kv_,  ki_] := 
        kp . err  +  kv . derr  +  ki . IntegralError + 
          ddθd;

Partitioned control

PartitionedControl[θd_,  dθd_,  ddθd_,  
          err_,  derr_,  kp_,  kv_,  ki_, model_, 
          vars_] := 
Module[{dvars, θ, dθ, 
            r1, r2, mm, vv, 
            gv, τp}, 
dvars  =  Dt[#, 
                  t] &  /@  
              vars; 
θ = θd - 
              err; 
dθ = 
            dθd - 
              derr; 
r1  =  (#[[1]] -> #[[2]]) &  /@  
              Transpose[{vars,  θ}]; 
r2  =  (#[[1]] -> #[[2]]) &  /@  
              Transpose[{dvars,  dθ}]; 
{mm, vv, 
              gv} = model  /.  (r1~Join~
                  r2); 
τp  =  
            kp . err  +  kv . derr  +  ki . IntegralError + 
              ddθd ; 
mm . τp  +  vv  +  
            gv
];

PID control with gravity compensation

GravityCompensation[θd_,  dθd_,  ddθd_,  
          err_,  derr_,  kp_,  kv_,  ki_, model_, 
          vars_] := 
Module[{n, mm, vv, 
            gv}, 
n  = 
            Length[vars]; 
mm  = 
            IdentityMatrix[n]; 
vv  =  
            Table[0. , {n}]; 
gv  =  
            Last[model]; 
PartitionedControl[θd,  
            dθd,  ddθd,  err,  derr,  kp,  kv,  
            ki, {mm, vv, gv}, vars]
];

C-code generation functions

Rewrite torque equations (dynamic model) in terms of second derivatives

Rewrite torque equations in terms of second derivatives

DynamicModelToSecondOrderDifferentialEquations[model_, 
          vars_] := 
Module[{fvars, 
            eqs}, 
fvars  =  
            Table[ToExpression["tau" <> ToString[i]], {i, 1, 
                Length[model]}]; 
          eqs  =  (#[[1]] == #[[2]]) &  /@  
              Transpose[{model, 
                  fvars}]; 
IsolateSecondDerivatives[eqs, 
            vars]
];

Generate list of arguments for a specific function type

part[exp_,  l_] :=  Part  @@  Join[{exp}, l];

GenerateFunctionArguments[cmodel_,  vars_,  
          fnc_] := 
Module[{tmp}, 
tmp = (part[cmodel,  Drop[#, -1]] &  /@  
                    Position[cmodel, fnc]) //  Sort //  
              Union ; 
#[[1]] &  /@  
            tmp
];

Create conversion rules

Generic conversion rule function

CreateConversionRule[one_,  {fnc_, params___},  str_,  
          n_] := 
Module[{tmp}, 
tmp  =  If[Length[one]  ==  0,  {one},  List  @@  one]; 
          fnc[one,  params] -> 
            ToExpression[
              ToString[str] <>  
                ToString[n]]
];

Conversion rules for cosine terms

\(\(CosineConversionRules[cmodel_, 
          vars_] := 
Module[{tmp}, 
tmp\  = \ GenerateFunctionArguments[cmodel, vars, Cos]; 
Table[
            CreateConversionRule[tmp[\([i]\)], {Cos}, \ "c", \ i], \ {i, 
              1, Length[tmp]}]
];\)\)

Conversion rules for sine terms

\(\(SineConversionRules[cmodel_, 
          vars_] := 
Module[{tmp}, 
tmp\  = \ GenerateFunctionArguments[cmodel, vars, Sin]; 
          Table[CreateConversionRule[tmp[\([i]\)], {Sin}, \ "s", \ 
              i], \ {i, 1, Length[tmp]}]
];\)\)

Conversion rules for derivative terms

\(\(DerivativeConversionRules[cmodel_, \ 
          vars_] := 
Module[{tmp}, 
tmp\  = \ GenerateFunctionArguments[cmodel, vars, Dt]; 
          Table[CreateConversionRule[tmp[\([i]\)], {Dt, t}, \ "dth", \ 
              i], \ {i, 1, Length[tmp]}]
];\)\)

Conversion rules for all terms

AllConversionRules[cmodel_, 
          vars_] := 
Join[
CosineConversionRules[cmodel, vars], 
SineConversionRules[
            cmodel, vars], 
DerivativeConversionRules[
            cmodel, vars]
];

Create C variable assignments for trig functions

Generic function (works for both cosine and sine)

\(\(MakeTrigVariableAssignments[one_, \ fnc_, \ str_, \ 
          n_] := 
Module[{str2}, 
str2\  = \ StringReplace[ToString[CForm[one]], \ "θ" -> "th"]; 
          str <> ToString[n] <> " = " <> ToString[fnc] <> "(" <> 
            str2 <> ")"
];\)\)

Cosine variable assignments

\(\(MakeCosVariableAssignments[cmodel_, 
          vars_] := 
Module[{tmp}, 
tmp\  = \ GenerateFunctionArguments[cmodel, \ vars, \ 
              Cos]; 
Table[
            MakeTrigVariableAssignments[tmp[\([i]\)], Cos, \ "c", \ 
              i], \ {i, 1, Length[tmp]}]
];\)\)

Sine variable assignments

\(\(MakeSinVariableAssignments[cmodel_, 
          vars_] := 
Module[{tmp}, 
tmp\  = \ GenerateFunctionArguments[cmodel, \ vars, \ 
              Sin]; 
Table[
            MakeTrigVariableAssignments[tmp[\([i]\)], Sin, \ "s", \ 
              i], \ {i, 1, Length[tmp]}]
];\)\)

Create C variable assignments and declarations

C variable assignments

MakeVariableAssignments[cmodel_, vars_,  
          tauInclude_:  True] := 
Module[{n, e1, e2, e3, 
            e4, e5}, 
n  =  
            Length[vars]; 
e1  =  
            Table["th" <> ToString[i] <> " = th[" <> 
                ToString[i - 1] <> "]",  {i, 1,  
                n}]; 
e2  =  
            Table["dth" <> ToString[i] <> " = dth[" <> 
                ToString[i - 1] <> "]",  {i, 1,  
                n}]; 
e3  =  
            Table["tau" <> ToString[i] <> " = tau[" <> 
                ToString[i - 1] <> "]",  {i, 1,  
                n}]; 
e4  =  
            MakeCosVariableAssignments[cmodel, 
              vars]; 
e5  =  
            MakeSinVariableAssignments[cmodel, vars]; 
If[
            tauInclude  ==  True, 
Join[e1, e2, e3, 
              e4, e5], 
Join[e1, e2, e4, 
              e5]
]
];

C variable declarations

MakeVariableDeclarations[
          varEqs_] := 
Module[{tmp1, 
            tmp2}, 
tmp1  = (# - 
                    1) &  /@  (First  /@  
                  Partition[
                    Flatten[StringPosition[#,  " ="] &  /@  
                        varEqs], 
                    2]); 
tmp2  =  {varEqs, 
                tmp1}  //  
              Transpose; 
Table["double " <> 
              StringTake[tmp2[[i, 1]],  tmp2[[i, 2]]], {i, 1, 
              Length[tmp2]}]
];

Convert dynamic model to robot C code (for Runge-Kutta integration)

Convert second-order diff. eqs. to set of first-order diff. eqs.

SecondOrderToFirstOrderDifferentialEquations[cmodel_, 
          vars_] := 
Module[{eqs1, 
            eqs2}, 
eqs1 =  Dt[#, t] &  /@  
              vars; 
eqs2  =  
            Join[eqs1, cmodel]  /.  
                AllConversionRules[cmodel  //  Chop, vars]  //  Chop; 
          Table["f[" <> ToString[i - 1] <> "] = " <> 
              ToString[CForm[eqs2[[i]]  //  N]], {i, 1, 
              Length[eqs2]}]
];

Convert dynamic model to robot C code

\(\(DynamicModelToDiffEqCCode[model_, vars_, nvals_] := 
        Module[{cmodel, e0, e1, e2, all}, 
          cmodel = 
            DynamicModelToSecondOrderDifferentialEquations[model\  // \ Chop, 
                vars]\  // \ Chop; 
e1 = 
            MakeVariableAssignments[cmodel, vars, \ 
              True]; 
e0 = 
            MakeVariableDeclarations[e1]; 
e2 = 
            SecondOrderToFirstOrderDifferentialEquations[
              cmodel\  /. \ nvals\  // \ Chop, 
              vars]; 
all = 
            Join[e0, e1, 
              e2]; 
\(\(("  " <> #)\) &\) /@ \((\(\((# <> ";")\) &\) /@ all)\)
];\)\)

Convert dynamic model to model C code (for model-based control)

Convert matrix/vector to C code

\(\(MatrixOrVectorToCCode[mat_, vars_, \ str_, 
          convRules_] := 
Module[{v, \ eqs, body, prelog, 
            epilog}, 
v\  = \ 
            Flatten[mat]; 
eqs\  = \ 
            v\  /. \ convRules; 
prelog = {"if(" <> 
                str <> " != NULL) {^^^"}; 
body = 
            Table[" " <> ToString[str] <> "[" <> 
                ToString[i - 1] <> "] = " <> 
                ToString[CForm[eqs[\([i]\)]]], {i, 1, 
                Length[eqs]}]; 
epilog\  = \ {"}^^^"}; 
prelog~Join~body~Join~
            epilog
];\)\)

Convert dynamic model to model C code

\(\(DynamicModelToMatrixCCode[model_, vars_, 
          nvals_] := 
Module[{cmodel, \ mm, vv, gv, e0, \ 
            e1, e2, e3, e4, \ all, \ 
            convRules}, 
cmodel\  = \ \(DynamicModelToSecondOrderDifferentialEquations[model\  // \ Chop, vars]\  // 
                Simplify\) // \ Chop; 
convRules\  = \ 
            AllConversionRules[Flatten[cmodel\  // \ Chop], 
              vars]; 
e1\  = \ 
            MakeVariableAssignments[cmodel, vars, \ 
              False]; 
e0\  = \ 
            MakeVariableDeclarations[e1]; 
{mm, vv, 
              gv} = \ \((τMatrixFromModel[model, vars]\  // 
                    Simplify)\) /. \ nvals\  // \ 
              Chop; 
e2\  = 
            MatrixOrVectorToCCode[mm, \ vars, \ "m", \ 
              convRules]; 
e3\  = 
            MatrixOrVectorToCCode[vv, \ vars, \ "v", \ 
              convRules]; 
e4\  = 
            MatrixOrVectorToCCode[gv, \ vars, \ "g", \ 
              convRules]; 
all\  = \ 
            Join[e0, e1, e2, e3, 
              e4]; 
\(StringReplace[#, "^^^;" -> ""] &\)\  /@ \ \((\(\(("  " <> #)\) &\)\  /@ \ \((\(\((# <> ";")\) &\)\  /@ \ all)\))\)
];\)\)

Generate file header information

Convert date to string

ConvertDateToString[] := 
Module[{yr,  mo,  day, 
            hr, min, sec,  date}, 
date =  
            Date[]; 
{yr,  mo,  day, hr, min, sec} =  
            ToString  /@  date; 
mo <> "/" <> 
            day <> "/" <> yr
];

Convert dynamic parameters to strings

\(\(ConvertDynamicParametersToString[dyn_, \ 
          prefix_] := 
Module[{s1, s2, s3, s4, s5, s6, s7, 
            s8, sn}, 
sn = {""}; 
s1 = {prefix <> " DH parameters:"}; 
s2 = \(StringReplace[#, "θ" -> "th"] &\)\  /@ \ {TableForm[
                    dyn[\([1]\)], \ TableSpacing -> {0, 2}]\  // \ 
                  ToString}; 
s3 = {prefix <> " inertia tensors:"}; 
s4 = \({\(ToString[
                        TableForm\ [#, TableSpacing -> {0, 2}]] &\) /@ \ 
                    Drop[dyn[\([2]\)], 1], 
                  Table[sn[\([1]\)], {Length[dyn[\([2]\)]] - 1}]}\  // \ 
                Transpose\)\  // \ 
              Flatten; 
s5 = {prefix <> \ " masses:"}; 
s6 = 
            ToString\  /@ \ 
              Drop[dyn[\([3]\)], 
                1]; 
s7 = {prefix <> " Pc vectors:"}; 
s8 = {TableForm[Drop[Last[dyn], 1], \ 
                  TableSpacing -> {1, 2}]\  // \ 
                ToString}; 
Join\  @@ \ {s1, sn, s2, sn, 
              s3, sn, s4, sn, s5, sn, s6, sn, s7, sn, s8, 
              sn}
];\)\)

Compose top file header

\(\(ComposeTopHeader[
          file_] := 
Module[{s1, s2, s3, spcs, s4tmp, 
            s4}, 
spcs = "                                                                           "; 
s1 = {"/*****************************************************************************/"}; 
s2 = {"/*                                                                           */"}; 
s3 = {"/*****************************************************************************/"}; 
s3 = {"/* Author: Michael C. Nechyba                                                */"}; 
s4tmp = " File: " <> file; 
s4 = {"/*" <> s4tmp <> 
                StringTake[spcs, 
                  StringLength[spcs] - 
                    StringLength[
                      s4tmp]] <> "*/"}; 
Join\  @@ \ {s1, s2, s3, s4, s2, s1}
];\)\)

Compose parameter file header

\(\(ComposeHeader[dynIdealized_, \ 
          dynReal_] := 
Module[{sn, so, sc, s1, s2, s3, s4, 
            s5, s6, \ 
            s7}, 
sn = {""}; 
so = {"/*"}; \ 
sc = \ {"*/"}; 
s1 = {"/*****************************************************************************/"}; 
s2 = {"  File generated by Mathematica on " <> ConvertDateToString[] <> "."}; 
s3 = 
            ConvertDynamicParametersToString[
              dynIdealized\  // \ 
                N, "Idealized"]; 
s4 = 
            ConvertDynamicParametersToString[
              dynReal\  // \ 
                N, "Real"]; 
s5 = "#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>"; 
s6 = "#include "manip.h""; 
s7 = "#include "robots.h""; 
Join\  @@ \ {s1, so, s2, sc, 
              s1, sn, s1, so, s3, sc, s1, sn, s1, so, s4, sn, sc, s1, 
              sn, {s5}, sn, {s6}, {s7}}
];\)\)

C code file output

Write C code function to file pointer

WriteCCodeFunction[fp_,  code_,  func_,  
          str_] := 
Module[{}, 
          WriteString[
            fp,  "\n\n\
/*****************************************************************************\
/\n"]; WriteString[fp,  "\nint " <> ToString[func] <> str]; 
          Do[WriteString[fp,  code[[i]] <> "\n"], {i, 1, 
              Length[code]}]; 
WriteString[
            fp,  "\n  return(1);\n}"];
];

Write C robot code to file pointer

\(\(WriteCCodeRobot[fp_, \ code_, \ 
          func_] := 
Module[{str}, 
str\  = \ "(double *f, double *th, double *dth,
double *tau)
{
"; 
WriteCCodeFunction[fp, code, func, 
            str];
];\)\)

Write C model code to file pointer

\(\(WriteCCodeModel[fp_, \ code_, \ 
          func_] := 
Module[{str}, 
str\  = "(double *m, double *v, double *g,
double *th, double *dth)
{
"; 
WriteCCodeFunction[fp, code, func, 
            str];
];\)\)

Write C code segments to file (default append)

This function appends the two code segments "cRobot" and "cModel" to a "file." The code segment "cRobot" corresponds to the dynamic model suitable for Runge-Kutta integration. The code segment "cModel" computes the dynamic model in parts. For \(TraditionalForm\`τ = \(M(Θ)\)\ Θ\&¨ + V(Θ, \(Θ\& . \)) + 
          G(Θ)\), it computes M(Θ), \(TraditionalForm\`V(Θ, \(Θ\& . \))\), and G(Θ)separately.The function names "fRobot" and "fModel" are assigned to each code segment.

\(\(AppendCCodeToFile[file_, \ cRobot_, \ fRobot_, \ cModel_, \ fModel_, 
          open_:  OpenAppend] := 
Module[{fp}, 
	
          fp\  = \ open[file]; 
	WriteCCodeRobot[fp, cRobot, fRobot]; 
	
          WriteCCodeModel[fp, cModel, fModel]; 
	
          Close[fp];
];\)\)

Complete C file

CompleCCodeToFile[outputFile_,  dyn_,  modelParameters_,  
          robotParameters_,  modelDiffEqs_, modelMatrices_,  
          robotDiffEqs_,  robotMatrices_, f1_, f2_, f3_, 
          f4_] := 
Module[{fp,  headerTop,  
            header}, 
fp  =  
            OpenWrite[outputFile]; 
headerTop  =  
            ComposeTopHeader[outputFile]; 
Do[
            WriteString[fp,  headerTop[[i]] <> "\n"], {i, 1, 
              Length[headerTop]}]; 
          header  =  
            ComposeHeader[dyn  /.  modelParameters ,  
              dyn  /.  robotParameters]; 
Do[
            WriteString[fp,  header[[i]] <> "\n"], {i, 1, 
              Length[header]}]; 
Close[
            fp]; 
AppendCCodeToFile[outputFile,  
            modelDiffEqs,  f1,  modelMatrices,  f2]; 
          AppendCCodeToFile[outputFile,  robotDiffEqs,  f3,  
            robotMatrices,  f4]; 
fp  =  
            OpenAppend[outputFile]; 
WriteString[
            fp, "\n\n\
/*****************************************************************************\
/\n"]; Close[fp]
];