ODE講座18：描画をしないでスピードアップ ODE 0.8 リリース
2 月 10

ODE Tutorial 16: A Robot Arm Petit Simulator

English Add comments

Most of this page was translated from http://demura.net/archives/9ode by Babel Fish Translation.

3 joint manipulator

A 3DOF Robot Arm Petit Simulator  (The source code is only about 100 lines)

It is 16th tutorial of this ODE(Open Dynamics Engine) Tutorial. ODE is an open source physics libary, and it is widely used  in various game software, and simulators in research.

This time in order to rearrange present knowledge, it will try making the petit simulator of the simple 3 joint manipulator. Thisiswhen ODE is used,"This kind of simulator is produced with only 100 lines."With being to be the program which is said it abbreviates the collision calculation part. Please try mounting the person who learned kinematics and opposite kinematics to this robot. When the robot moves, it is something which becomes matter of concern which is understood strangely.

So, the source code will be introduced next.

The robot can be controlled by a keyboard　with  following commads.

As for the 1st joint  is moved with  pushing 'j' (increase) or 'f' (decrease) key, the 2nd joint being the 'd' or 　'k' key, being the 's' or　'l' key, moves the 3rd joint. This has described in the command function of the program.

This program is simple enough for the person who has understood the past ODE lecture. You think that explanation almost does not need. If there is a place where it is not understood please write to comment.

PLAIN TEXT
CODE:
  1. //  arm.cpp  3 DOF manipulator    by   Kosei Demura  2006
  2. #include                     // ODE
  3. #include   // the drawing library of ODE
  4. #ifdef dDouble
  5. #define dsDrawCapsule  dsDrawCapsuleD
  6. #endif
  7. #define NUM 4                             // the number of links (base it includes)
  8. dWorldID    world;                         // Kinetics calculation world
  9. dBodyID     link [ NUM ];            // Link link [ as for 0 ] base
  10. dJointID      joint [ NUM ];           // Joint    joint [ 0 ] locking base and the land
  11. static double THETA [ NUM ] = {0.0, 0.0, 0.0, 0.0};  // Joint target angle [ rad ]
  12. static double l [ NUM ]  = {0.10, 0.90, 1.00, 1.00};     // Link length [ m ]
  13. static double r [ NUM ]  = {0.20, 0.04, 0.04, 0.04};     // Link radius [ m ]
  14.  
  15. // P control
  16. void control () {
  17. static int step = 0;     // The number of steps of simulation
  18. double k1 =  10.0,  fMax  = 100.0;     // k1: Proportional gain,  fMax: Largest torque [ Nm ]
  19. printf ("\r%6d: "Step++);
  20. for (int j = 1; J <NUM; J++) {
  21. double tmpAngle = dJointGetHingeAngle (joint [ j ]);     // Present joint angle [ rad ]
  22. double z = THETA [ j ] - tmpAngle;                               // z:  = target joint angle - Present joint angle
  23. dJointSetHingeParam (joint [ j ],  dParamVel and  k1*z);     // Setting of angular velocity
  24. dJointSetHingeParam (joint [ j ], dParamFMax and fMax);  // Setting of largest torque
  25. }
  26. }
  27.  
  28. //Drawing API initialization
  29. //As for the API of ODE as for the unit of all joints [ rad ] is, but the argument 3 dimensional arrangement //hpr of dsSetViewpoint which is the API of drawstuff [ the unit of each element of 3 ] [ has become the ｰ ]. //Just this being to be different, you will pay attention!
  30. void START () {
  31. float xyz [ 3 ] = {  3.04, 1.28, 0.76};     // Point of view x, y and z [ m ]
  32. float hpr [ 3 ] = {-160.0, 4.50, 0.00};    // Gaze (heading, pitch and roll) [ ｰ ]
  33. dsSetViewpoint (xyz and hpr);              // Setting of point of view and gaze
  34. }
  35.  
  36. / /Key operation function
  37. / /At each time there is input of the key value of that key is substituted in cmd.
  38. void command (int cmd) {/
  39. switch (cmd) {
  40. case ‘j’:  THETA [ 1 ] += 0.05; Break;  // When the j key is pushed, THETA [ angle of 1 ] increases 0.05 [ rad ]
  41. case ‘f’:  THETA [ 1 ] -= 0.05; Break;
  42. case ‘k’:  THETA [ 2 ] += 0.05; Break;
  43. case ‘d’:  THETA [ 2 ] -= 0.05; Break;
  44. case ‘l’:  THETA [ 3 ] += 0.05; Break;
  45. case ’s’:  THETA [ 3 ] -= 0.05; Break;
  46. }
  47. //In order/ or target angle not to exceed the joint movable limits, it restricts
  48. // The joint movable limits dJointSetHingeParam (dJointID, int parameter and dReal value) with, in case of the //joint lower limit in case of dParamLoStop and the joint upper limit set parameter to dParamHiStop. Value //inserts the value. As for unit [ rad ].
  49. if (THETA [ 1 ] <  - M_PI)  　　THETA [ 1 ]     =      - M_PI;   // M_PI is π
  50. if (THETA [ 1 ]>      M_PI)  　　THETA [ 1 ]     =       M_PI;
  51. if (THETA [ 2 ] < -2*M_PI/3)  THETA [ 2 ] =  - 2*M_PI/3;
  52. if (THETA [ 2 ]>   2*M_PI/3)  THETA [ 2 ] =   2*M_PI/3;
  53. if (THETA [ 3 ] < -2*M_PI/3)  THETA [ 3 ] =  - 2*M_PI/3;
  54. if (THETA [ 3 ]>   2*M_PI/3)  THETA [ 3 ] =   2*M_PI/3;
  55. }
  56.  
  57. //  Simulation loop
  58. //  Each time it is called the execution time simulation. With the source code it is not understood, but this part // has entered into the while loop. In addition, here as for the cord/code regarding collision detection it is
  59. // abbreviated.
  60. void simLoop (int pause) {
  61. control ();
  62. dWorldStep (world, 0.02);
  63. // Drawing of robot
  64. dsSetColor (1.0,1.0,1.0); / Setting of color (r, g and b) as for each value 0? 1, here it sets to white
  65. for (int i = 0; I <NUM; I++) / Drawing the link of the robot in the capsule
  66. dsDrawCapsule (dBodyGetPosition (link [ i ]), dBodyGetRotation (link [ i ]), l [ i ], r [ i ]);
  67. }
  68.  
  69. int main (int argc and char *argv [ ]) {
  70. dsFunctions fn; 　// Drawing function
  71. dMass mass;        // Mass parameter
  72. double x [ NUM ] = {0.00}, y [ NUM ] = {0.00};              // Balance station
  73. double z [ NUM ]         = {0.05, 0.50, 1.50, 2.55};
  74. double m [ NUM ] = {10.00, 2.00, 2.00, 2.00};                   // Mass
  75. double anchor_x [ NUM ]  = {0.00}, anchor_y [ NUM ] = {0.00};
  76. double anchor_z [ NUM ] = {0.00, 0.10, 1.00, 2.00}  ;       //Rotary center
  77. double axis_x [ NUM ]  = {0.00, 0.00, 0.00, 0.00};              //Axis of rotation
  78. double axis_y [ NUM ]  = {0.00, 0.00, 1.00, 1.00};
  79. double axis_z [ NUM ]  = {1.00, 1.00, 0.00, 0.00};
  80. fn.version = DS_VERSION;  Fn.start   = &start;
  81. fn.step   = &simLoop;      Fn.command = &command;
  82. fn.path_to_textures = "../.. /drawstuff/textures";
  83.  
  84. world = dWorldCreate ();   // Formation of the kinetics calculation world
  85. dWorldSetGravity (world, 0, 0, -9.8);    / / Gravity setting
  86.  
  87. for (int i = 0; I <NUM; I++) {                     // Formation of link it sets
  88. link [ i ] = dBodyCreate (world);                // Formation of link
  89. dBodySetPosition (link [ i ], x [ i ], y [ i ], z [ i ]);  // Setting of position
  90. dMassSetZero (&mass);                        // Initialization of mass parameter
  91. dMassSetCappedCylinderTotal (&mass, m [ i ],3, r [ i ], l [ i ]);  // Mass calculation of link
  92. dBodySetMass (link [ i ], &mass);            // Setting of mass
  93. }
  94. // Formation of the joint it sets
  95. joint [ 0 ] = dJointCreateFixed (world, 0);  // Fixed joint (fixing of base and land)
  96. dJointAttach (joint [ 0 ], link [ 0 ] and 0);  // Installation of fixed joint
  97. dJointSetFixed (joint [ 0 ]);                         // Setting of fixed joint
  98. for (int j = 1; J <NUM; J++) {
  99. joint [ j ] = dJointCreateHinge (world, 0);  // Hinge joint formation
  100. dJointAttach (joint [ j ], link [ j-1 ], link [ j ]);  // Installation of joint
  101. dJointSetHingeAnchor (joint [ j ], anchor_x [ j ], anchor_y [ j ], anchor_z [ j ]);  // Setting in joint center
  102. dJointSetHingeAxis (joint [ j ], axis_x [ j ], axis_y [ j ], axis_z [ j ]);  // Setting of joint axis of rotation
  103. }
  104. dsSimulationLoop (argc, argv and 640, 570, &fn); / Simulation loop
  105. return 0;
  106. }

Furthermore, the program is download possible from here. There is no warranty for this software. In order to operate, mingw is necessary. Please refer past tutorial.

  1. Arm070112.tar is copied in /home/ user name /src/ode-0.7/myprog. You are not concerned the last directory name myprog even with another name.
  2. Command below is executed with terminal software.
    • dd  - /src/ode-0.7/myprog
    • tar   xvf   arm070112.tar
    • cd   arm
    • make
    • . /arm

    End.

    投稿：07年02月10日

    Leave a Reply

    カウンタ (since 2008-1-11）
    Copyright © 1998-2008    Kosei Demura