I am trying to perform iterative newton Euler method for dynamics of an RP manipulator but having trouble in finding the value of vdot. Please help.
DOF = 2 ;
(* D-H PARAMETERS AND JOINT TYPE (0-revolute,1-prismatic) *)
\[Alpha][0] = 0 ; a[0] = 0 ;
d[1] = 0 ; \[Theta][1] = Subscript[\[Theta], 1][t] ;
jtype[1] = 0 ;
\[Alpha][1] = Pi/2 ; a[1] = 0 ;
d[2] = Subscript[d, 2][t] ; \[Theta][2] = 0 ;
jtype[2] = 1 ;
m[1] = Subscript[m, 1] ;
m[2] = Subscript[m, 2] ;
inertia[1] = {{Subscript[I, xx1], 0, 0}, {0, Subscript[I, yy1],
0}, {0, 0, Subscript[I, zz1]}} ;
inertia[2] = {{Subscript[I, xx2], 0, 0}, {0, Subscript[I, yy2],
0}, {0, 0, Subscript[I, zz2]}} ;
pcm[1] = {{0}, {-Subscript[L, 1]}, {0}} ;
pcm[2] = {{0}, {0}, {0}} ;
gravity = {{-g}, {0}, {0}} ;
Print[" Denavit-Hartenburg Parameters"] ;
Print["Link i \[Alpha][i-1] a[i-1] d[i] \[Theta][i] \
J-type[i]"] ;
Print["-------------------------------------------------------------"]\
;
For [ i = 1, i <= DOF, i++,
Print[" ", i, " ", \[Alpha][i - 1], " ",
a[i - 1], " ", d[i],
" ", \[Theta][i], " ", jtype[i]]
] ;
Print[" Mass Parameters"] ;
Print["Link m pcm inertia"] ;
Print["---------------------------------------------------"] ;
For [ i = 1, i <= DOF, i++,
Print[" ", i, " ", m[i], " ", MatrixForm[pcm[i]],
" ", MatrixForm[inertia[i]]]
] ;
Print["gravity = ", MatrixForm[gravity]] ;
For [ i = 1, i <= DOF, i++,
{R[i] = {{Cos[\[Theta][i]], -Sin[\[Theta][i]], 0},
{Sin[\[Theta][i]]*Cos[\[Alpha][i - 1]],
Cos[\[Theta][i]]*Cos[\[Alpha][i - 1]],
-Sin[\[Alpha][i - 1]]},
{Sin[\[Theta][i]]*Sin[\[Alpha][i - 1]],
Cos[\[Theta][i]]*Sin[\[Alpha][i - 1]],
Cos[\[Alpha][i - 1]]}} ,
p[i] = {{a[i - 1]},
{-Sin[\[Alpha][i - 1]]*d[i]},
{Cos[\[Alpha][i - 1]]*d[i]}}
}
] ;
For [ i = 1, i <= DOF, i++,
Print["p[", i, "] = ", MatrixForm[p[i]], " R[", i, "] = ",
MatrixForm[R[i]]]
] ;
CrossP[x_, y_] := {{x[[2, 1]]*y[[3, 1]] - x[[3, 1]]*y[[2, 1]]},
{x[[3, 1]]*y[[1, 1]] - x[[1, 1]]*y[[3, 1]]},
{x[[1, 1]]*y[[2, 1]] - x[[2, 1]]*y[[1, 1]]}} ;
(* DEFINE Z-VECTOR *)
zhat = {{0}, {0}, {1}} ;
\[Omega][0] = Table[0, {3}, {1}] ;
\[Omega]dot[0] = Table[0, {3}, {1}] ;
vdot[0] = - gravity ;
For [ i = 1, i <= DOF, i++,
{\[Omega][i] =
Transpose[R[i]].\[Omega][i - 1] + zhat*D[\[Theta][i], t];
\[Omega]dot[i] = Transpose[R[i]].\[Omega]dot[i - 1]
+ zhat*D[\[Theta][i], {t, 2}]
+ CrossP[Transpose[R[i]].\[Omega][i - 1],
zhat*D[\[Theta][i], t]] };
vdot = Transpose[
R[i]].(CrossP[\[Omega]dot[i - 1], p[i]] +
CrossP[\[Omega][i - 1], CrossP[\[Omega][i - 1], p[i]]] +
vdot[i - 1])
]
For [ i = 1, i <= DOF, i++,
{Print["\[Omega][", i, "] = ", MatrixForm[\[Omega][i]],
" \[Omega]dot[",
i, "] = ", MatrixForm[\[Omega]dot[i]]] ,
Print["-------------------------------------------------------------------\
"] }
{Print["vdot[", i, "] = ", MatrixForm[vdot[i]]]
Print["-------------------------------------------------------------------\
"] }
] ;