Group Abstract Group Abstract

Message Boards Message Boards

0
|
7.1K Views
|
1 Reply
|
0 Total Likes
View groups...
Share
Share this post:

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

Posted 6 years ago

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["-------------------------------------------------------------------\
"] }
  ] ;
POSTED BY: Anshuman Singh
POSTED BY: Neil Singer
Reply to this discussion
Community posts can be styled and formatted using the Markdown syntax.
Reply Preview
Attachments
Remove
or Discard