Message Boards Message Boards

GROUPS:

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

Posted 1 year ago
1284 Views
|
1 Reply
|
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["-------------------------------------------------------------------\
"] }
  ] ;

Anshuman,

Unless I am missing something, I think you are using the wrong tool for this job. I would use Wolfram SystemModeler. You can construct your manipulator by wiring elements together. you can simulate it there. If you use the Multibody objects you will get an animation. Later, If you need the equations you can import them into Mathematica and manipulate them in MMA.

Can you tell us the goal of what you are trying to do?

Regards,

Neil

Reply to this discussion
Community posts can be styled and formatted using the Markdown syntax.
Reply Preview
Attachments
Remove
or Discard

Group Abstract Group Abstract