Message Boards Message Boards

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

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

Posted 4 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

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

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

Group Abstract Group Abstract