C-code manipulator control experiment visualization

Local definitions

General definitions

Turn off annoying warnings messages

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

Load in necessary packages

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

Visualization definitions

Plot colors/styles

colors  =  {Red,  Blue,  Magenta,  
          Green};
actualStyle  =  {#} &  /@  
          colors;
desiredStyle  =  {dsh, #} &  /@  colors;

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

Empty DH list

DH  =  Table[,{3}];

Robot #1 (2-link, point-mass assumption)

DH[[1]] =  {{0, 0, 0, θ1, 0}, {0, L1,  0,  θ2, 
                0},  {0,  L2,  0,  0, 0}}  /.  {L1 -> 1, 
              L2 -> 1/2}  //  N;

Robot #2 (2-link, cylindrical link assumption)

DH[[2]] =  {{0, 0, 0, θ1, 0}, {0, L1,  0,  θ2, 
                0},  {0,  L2,  0,  0, 0}}  /.  {L1 -> 1, 
              L2 -> 1/2}  //  N;

Robot #3 (4-link, point-mass assumption)

DH[[3]] =  {{0, 0, 0, θ1, 0}, {π/2, 0,  
                0,  θ2, 0},  {0,  L1,  0, θ3, 0}, {0, L2, 
                0, θ4, 0}, {0, L3, 0, 0, 0}}  /. {L1 -> 1,  
              L2 -> 3/4, L3 -> 1/2}  //  N;

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

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

Animation functions

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

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

Read "msim" results file definition

ReadResultsFile[file_, 
          n_:  4] := 
Module[{res}, 
res  =  Partition[ReadList[file, Number], 3*n + 1]  //  
              Transpose; 
{res[[1]]  //  Last}~Join~
            Partition[Interpolation[
                    Transpose[{res[[1]],  #}]] &  /@  
                Drop[res,  1], n]
];

Control experiment visualization

Read control results from file

Set data file

\(\(dataFile\  = \ "/Users/nechyba/Desktop/hw4/manip/results.dat";\)\)

Read control parameters from file

{tmax, desiredOut, actual, torques} = 
        ReadResultsFile[dataFile];

Visualization of results

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desiredOut, {0, tmax}, desiredStyle, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, actualStyle, 
          nS];
Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[desiredOut, actual, {0, tmax}, 
          Subtract];
gerror = 
        PlotMultipleTrajectory[error, {0, tmax}, actualStyle, 
          imgSize];

Applied torques

gtorques = 
        PlotMultipleTrajectory[torques, {0, tmax}, actualStyle, 
          imgSize];

Animation

Robot types

Animation parameters

dim  =  3;
nFrames  =  150;
robotType  =  3;

Animation

Animate just actual trajectory (result not shown to minimize file size)

AnimateTrajectory[DH[[robotType]], actual, dim, nFrames, 
        tmax];

Animate both desired and actual trajectory (result not shown to minimize file size)

AnimateTwoTrajectories[DH[[robotType]], desiredOut,  actual, 
        dim, nFrames, tmax];