Introductory dynamics examples

This notebook simulates the dynamic behavior of two simple manipulators (one link and planar 2-link). It is intended to illustrate how the dynamic model of a system becomes significantly more complex as multiple bodies interact with respect to one another. Specifically, the dynamic model for a one-link manipulator is significantly less complex than for a two-link planar manipulator. This notebook also illustrates in the last section that using Euler integration to numerically solve differential equations is, in general not a good idea, since error can quickly accumulate giving wrong numerical results. This notebook assumes that the definitions in "dynamics_defs.nb" have already been evaluated.

One-link manipulator example

Kinematic, mass and inertial parameters

DH parameters

DH  =  {{0, 0, 0, θ1, 0}, {0, L1,  0, 0, 0}}; 
DH  // TraditionalForm

Inertia tensors (links 0 through 2): point-mass assumption

in  =  
        Table[Table[0, {3}, {3}], {2}]; 
in  //  TraditionalForm

Link masses

mass  =  {0, m}; 
mass  //  TraditionalForm

Center-of-mass vectors

Pc  =  {{0, 0, 0}, {L1, 0, 0}}; 
Pc  //  TraditionalForm

Group parameters

dyn  =  {DH,  in,  mass,  Pc};

Dynamics simulation

Torque equation

tmp  =  τ[dyn, 1]  //  Simplify  //  
          ExpandAll;
τ1  ==  tmp  //  TraditionalForm

Angular acceleration equation

{ddθ1}  =  accDiff[dyn,  {τ1}];
ddθ  =  {ddθ1}; 
Dt[θ1, {t, 2}]  ==  ddθ1  //  
      TraditionalForm

Numeric values for simulation

vals  =  {L1 -> 1. ,  g -> 9.81, m -> 1. };

nDH  =  DH  /.  vals;

tmax  =  5. ;

Joint acceleration trajectory given torque τ1 = 0 and numerical values

traj  =  (ddθ  /.  vals)  /.  {τ1 -> 
              0};
Dt[θ1, {t, 2}]  ==  traj[[1]]  //  
      TraditionalForm

Integrate above differential equation

res  =  
        SimulateDynamics[{{0}, {0}},  traj,  tmax,  DH];

Plot resulting joint trajectory

Plot[res[[1]][t],  {t,  0, tmax},  
        Frame -> True];

Animate result

AnimateTrajectory[nDH,  res, 2, 20];

Energy analysis

Kinetic energy

ke  =  
        Sum[1/2 v[DH, i, i] . v[DH, i, i] mass[[i]]  //  
            Simplify, {i, 1, Length[DH]}];
KE  ==  ke  //  TraditionalForm

Potential energy

pe  =  
        Sum[-mass[[i]] {0, -g, 0} . P[DH, 0, i], {i, 1, 
              Length[DH]}]  //  Simplify;
PE  ==  pe  //  TraditionalForm

Total energy

ke  =  ke  /.  
            vals  /.  {θ1 -> res[[1]][
                t]};
pe  =  pe  /.  
            vals  /.  {θ1 -> res[[1]][
                t]};
te  =  (ke + pe);

Plot of total energy over time

Plot[te,  {t,  0,  tmax},  Frame -> True];

Separate plots of kinetic and potential energies

Plot[{ke, pe},  {t, 0, tmax},  
        PlotStyle -> {{RGBColor[1, 0, 0]}, {RGBColor[0, 0, 1]}},  
        Frame -> True];

Two-link planar manipulator example

Kinematic, mass and inertial parameters

DH parameters

DH  =  {{0, 0, 0, θ1, 0}, {0, L1,  0,  θ2, 
            0},  {0,  L2,  0,  0, 0}}; 
DH  // TraditionalForm

Inertia tensors (links 0 through 2): point-mass assumption

in  =  
        Table[Table[0, {3}, {3}], {3}]; 
in  //  TraditionalForm

Link masses

mass  =  {0, m1, m2};
m  //  TraditionalForm

Center-of-mass vectors

Pc  =  {{0, 0, 0}, {L1, 0, 0}, {L2, 0, 
            0}}; 
Pc  //  TraditionalForm

Group parameters

dyn  =  {DH,  in,  mass,  Pc};

Dynamic simulation

Torque equations

tmp  = τ[dyn, 1]  //  Simplify;
τ1  ==  tmp  //  TraditionalForm

tmp  =  τ[dyn, 2]  //  
          Simplify;
τ2  ==  tmp  //  TraditionalForm

Angular acceleration equations

{ddθ1,  ddθ2}  =  
        accDiff[dyn,  {τ1,  τ2}];
ddθ  =  {ddθ1,  ddθ2};

Dt[θ1,  {t, 2}]  ==  ddθ1  //  
      TraditionalForm

Dt[θ2,  {t, 2}]  == ddθ2  //  
      TraditionalForm

Numeric values for simulation

vals  =  {L1 -> 1. ,  L2 ->  .5,  g -> 9.81, m1 -> 1. ,  
          m2 -> 1. };

nDH  =  DH  /.  vals;

tmax  =  7;

Joint acceleration trajectory given torques τ1 = τ2 = 0 and numerical values

traj  =  (ddθ  /.  vals)  /.  {τ1 -> 
              0, τ2 -> 0};

Dt[θ1,  {t, 2}]  ==  traj[[1]]  // 
      TraditionalForm

Dt[θ2,  {t, 2}]  ==  traj[[2]]  //  
      TraditionalForm

Integrate above differential equations

res  =  
        SimulateDynamics[{{0, 0}, {0, 0}},  traj,  tmax,  DH,  
          MaxSteps -> 3000];

Plot resulting joint trajectories

Plot[{res[[1]][t], res[[2]][t]}, {t, 0, tmax},  
        PlotStyle -> {{RGBColor[1, 0, 0]}, {RGBColor[0, 0, 1]}},  
        Frame -> True];

Animate result

AnimateTrajectory[nDH,  res, 2, 80];

Energy analysis

Kinetic energy

ke  =  
        Sum[1/2 v[DH, i, i] . v[DH, i, i] mass[[i]]  //  
            Simplify, {i, 1, Length[DH]}];
KE  ==  ke  //  TraditionalForm

Potential energy

pe  =  
        Sum[-mass[[i]] {0, -g, 0} . P[DH, 0, i], {i, 1, 
              Length[DH]}]  //  Simplify;
PE  ==  pe  //  TraditionalForm

Total energy

ke  =  ke  /.  
            vals  /.  {θ1 -> res[[1]][
                t],  θ2 -> res[[2]][t]};
pe  =  pe  /.  
          vals  /.  {θ1 -> res[[1]][
              t],  θ2 -> res[[2]][t]}; 
    te  =  (ke + pe);

Plot of total energy over time

Plot[te,  {t,  0,  tmax},  Frame -> True,  
        PlotRange -> All];

Separate plots of kinetic and potential energies

Plot[{ke, pe},  {t, 0, tmax},  
        PlotStyle -> {{RGBColor[1, 0, 0]}, {RGBColor[0, 0, 1]}},  
        Frame -> True];

Badness of Euler integration (2-link example)

Simulation parameters

steps  =  700;
deltat  =  0.01;
tmax  =  steps*deltat;

Euler integration

tst  =  
        eulerIntegrate[{{0, 0}, {0, 0}},  traj,  deltat, steps,  
          DH];

Plot resulting joint trajectories

Plot[{tst[[1]][t], tst[[2]][t]}, {t, 0, tmax},  
        PlotStyle -> {{RGBColor[1, 0, 0]}, {RGBColor[0, 0, 1]}},  
        Frame -> True];

Animate result

AnimateTrajectory[nDH,  tst, 2, 40];

Energy analysis

Kinetic energy

ke  =  
        Sum[1/2 v[DH, i, i] . v[DH, i, i] mass[[i]]  //  
            Simplify, {i, 1, Length[DH]}];
ke  //  TraditionalForm

Potential energy

pe  =  
        Sum[-mass[[i]] {0, -g, 0} . P[DH, 0, i], {i, 1, 
              Length[DH]}]  //  Simplify;
pe  //  TraditionalForm

Total energy

ke  =  ke  /.  
            vals  /.  {θ1 -> tst[[1]][
                t],  θ2 -> tst[[2]][t]};
pe  =  pe  /.  
          vals  /.  {θ1 -> tst[[1]][
              t],  θ2 -> tst[[2]][t]}; 
    te  =  (ke + pe);

Plot of total energy over time

Plot[te,  {t,  0,  tmax},  Frame -> True];