Dynamic model example

This notebook illustrates the iterative Newton-Euler derivation of a manipulator's dynamics. This notebook assumes that the definitions in "dynamics_defs.nb" have already been evaluated.

Short hand display function

dsp[tmp_] := ({#} &  /@  tmp)  //  MatrixForm  //  
          TraditionalForm;
TF :=  TraditionalForm;

Manipulator specifications

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  //  TF

Link masses

mass  =  {0, m1, m2};
mass   //  dsp

Center-of-mass vectors

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

Group parameters

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

Outward propagation

Initialization

Angular velocity  \(TraditionalForm\`\(\^0\)\(ω\_\(\(\ \)\(0\)\)\)\)

ω[DH, 0, 0]  //  dsp

Angular acceleration: \(TraditionalForm\`\(\^0\)\(\(ω\& . \)\_\(\(\ \)\(0\)\)\)\)

α[DH,  0,  0]  //  dsp

Linear acceleration: \(TraditionalForm\`\(\^0\)\(\(v\& . \)\_\(\(\ \)\(0\)\)\)\)

a[DH,  0,  0]  //  dsp

Frame {1}

Angular velocity  \(TraditionalForm\`\(\^1\)\(ω\_\(\(\ \)\(1\)\)\)\)

ω[DH, 1, 1]  //  dsp

Angular acceleration: \(TraditionalForm\`\(\^1\)\(\(ω\& . \)\_\(\(\ \)\(1\)\)\)\)

α[DH,  1,  1]  //  dsp

Linear acceleration: \(TraditionalForm\`\(\^1\)\(\(v\& . \)\_\(\(\ \)\(1\)\)\)\)

a[DH,  1,  1]  //  dsp

Linear acceleration: \(TraditionalForm\`\(\^1\)\(\(v\& . \)\_\(\(\ \)\(C\_1\)\)\)\)

ac[{DH, Pc},  1, 1]  //  dsp

Net force : \(TraditionalForm\`\(\^1\)\(F\_\(\(\ \)\(1\)\)\)\)

F[dyn,  1, 1]  //  Simplify  //  dsp

Net moment: (^1)(Subscript[N, 1])

Nd[dyn,  1, 1]  //  dsp

Frame {2}

Angular velocity  \(TraditionalForm\`\(\^2\)\(ω\_\(\(\ \)\(2\)\)\)\)

ω[DH, 2, 2]  //  dsp

Angular acceleration: \(TraditionalForm\`\(\^2\)\(\(ω\& . \)\_\(\(\ \)\(2\)\)\)\)

α[DH,  2,  2]  //  dsp

Linear acceleration: \(TraditionalForm\`\(\^2\)\(\(v\& . \)\_\(\(\ \)\(2\)\)\)\)

a[DH,  2,  2]  //  Simplify  //  dsp

Linear acceleration: \(TraditionalForm\`\(\^2\)\(\(v\& . \)\_\(\(\ \)\(C\_2\)\)\)\)

ac[{DH, Pc},  2, 2]  //  Simplify  //  dsp

Net force : \(TraditionalForm\`\(\^2\)\(F\_\(\(\ \)\(2\)\)\)\)

F[dyn,  2, 2]  //  Simplify  //  dsp

Net moment: (^2)(Subscript[N, 2])

Nd[dyn,  2, 2]  //  dsp

Inward propagation

Link 2

Link force \(TraditionalForm\`\(\^2\)\(f\_\(\(\ \)\(2\)\)\)\)

f[dyn, 2, 2]  //  Simplify  //  dsp

Link moment: (^2)(Subscript[n, 2])

n[dyn,  2, 2]  //  Simplify  //  dsp

Link 2 torque τ

τ2  =   (τ[dyn, 2]  //  Simplify);  τ2  //  
      TF

Link 1

Link force \(TraditionalForm\`\(\^1\)\(f\_\(\(\ \)\(1\)\)\)\)

f[dyn, 1, 1]  //  Simplify  //  dsp

Link moment: (^1)(Subscript[n, 1])

n[dyn,  1, 1]  //  Simplify  //  TF

Link 1 torque τ

τ1  =  (τ[dyn, 1]  //  Simplify);  τ1  //  
      TF

Equivalent dynamic model

Here we check whether our model corresponds to the dynamic model form: \(\(τ\_\ \) = \ \(M(Θ)\)\ Θ\&¨ + V(Θ, \(Θ\& . \)) + 
              G(Θ)\)

Mass matrix M(Θ)

Grab coefficients of second joint derivatives and arrange in matrix

MM  =  
        Outer[Coefficient[#1,  
              Dt[#2,  {t, 
                  2}]] &,  {τ1,  τ2},  {θ1, θ2}]; 
MM  //  TF

Equivalent result with defined function

MM  =  τMatrixFromModel[{τ1, τ2}, {θ1, θ2}][[1]];
MM  //  TF

Gravity term G(Θ)

Grab coefficients of gravity term

GV  =  
        Outer[Coefficient[#1,  #2] &,  {τ1,  τ2},  {g}] g;
GV  //  TF

Equivalent result with defined function

GV  =  τMatrixFromModel[{τ1, τ2}, {θ1, θ2}][[3]];
GV  //  dsp

Coriolis and centrifugal terms V(Θ, \(TraditionalForm\`Θ\& . \))

Grab remaining terms and group

VV  =  {τ1,  τ2}  -  
            MM . ( Dt[#,  {t, 
                        2}] &  /@  {θ1,  θ2})  -  
            GV  //  Simplify; 
VV  //  TF

Equivalent result with defined function

VV  =  τMatrixFromModel[{τ1, τ2}, {θ1, θ2}][[2]];
VV  //  dsp

Is the mass matrix M(Θ) positive definite?

Mass matrix M(Θ)

MM  =  τMatrixFromModel[{τ1, τ2}, {θ1, θ2}][[1]];
MM  //  TF

| M(Θ) | > 0?

Det[MM]  //  TF

Det[MM]  //  Simplify  //  TF