Dynamics definitions

Angular acceleration  of link {i} in frame {j} coordinates

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

α[dh_, i_, i_] := R[dh, i, i - 1] . α[dh, i - 1, i - 1] + <br />   &n ... p;     If[dh[[i, 5]] == 0, Dt[dh[[i, 4]], {t, 2}] {0, 0, 1}, {0, 0, 0}] ;

α[dh_, j_, i_] := <br />    R[dh, j, i] . α[dh, i, i]

Linear acceleration  of link {i} in frame {j} coordinates

a[dh_, 0, 0] := {0, g, 0} ; (* 0 gravity *)

a[dh_, i_, i_] := R[dh, i, i - 1] . (α[dh, i - 1, i - 1] ~ Cross ~ P[dh, i - 1, 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_] := <br />    R[dh, j, i] . a[dh, i, i]

Linear acceleration of center of mass of link {i} in frame {j} coordinates

ac[{dh_, Pc_}, i_, i_] := <br />    α[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_] := <br />    R[dh, j, i] . ac[{dh, Pc}, i, i] ;

Force forward propagation

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

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

Force backward propagation

f[{dh_, in_, m_, Pc_}, i_, i_] := <br />    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_] := <br />    If[i == Length[dh], {0, 0, 0}, ... Pc}, i, i]) + (P[dh, i, i + 1] ~ Cross ~ (R[dh, i, i + 1] . f[{dh, in, m, Pc}, i + 1, i + 1]))] ;

(* need to fix for prismatic joints *)

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

Get complete dynamic model

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

M(Θ), V(Θ, Overscript[Θ, .]) and G(Θ)

τMatrixFromModel[τvec_, vars_] := <br />    Module[{MM, VV, GV}, ... @ vars) - GV // Simplify] ; <br />         {MM, VV, GV}] ;

τMatrix[{dh_, in_, m_, Pc_}] := <br />    Module[{τvec, vars}, < ... ;    τMatrixFromModel[τvec, vars] <br />    ] ;

Created by Mathematica  (October 21, 2003)