# Perform Newton Euler iterative method for dynamics of an RP manipulator?

Posted 1 year ago
1921 Views
|
|
0 Total Likes
|
 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["-------------------------------------------------------------------\ "] } ] ;