ODE Tutorial 12: Let's make a torque sensor ! ODE Tutorial 14: Let a simulate be interactive !
1 月 09

ODE Tutorial 13: Let's make a pressure sensor !

English Add comments
This page was maily translated from http://demura.net/archives/9ode by Babel Fish Translation. It continues to the last time, utilizes the dJointFeedback structure. This time the joint (the joint) the sample cord/code which acquires the power which catches is introduced. When this is applied, you attach to the sole of a humanoid robot, you can simulate the pressure sensor which is utilized in the measurement of ZMP.
  • void dJointSetFeedback (dJointID and dJointFeedback *); The dJointFeedback structure is set to the joint JointID which acquires the information of power and torque. typedef struct dJointFeedback { dVector3 f1;    // The power which the joint has caused to body 1 dVector3 t1;    // The torque which the joint has caused to body 1 dVector3 f2;    //  The power which the joint has caused to body 2 dVector3 t2;    // The torque which the joint has caused to body 2 } dJointFeedback;
  • dJointFeedback *dJointGetFeedback (dJointID); Information of power and torque of the joint which it is designated with dJointID is acquired.
Next, the sample program which used this API is introduced. It is the program which indicates the power which the fixed joint (Fixed Joint) with attaches two boxes, depends on there. It meaning that weight of the box is each 1kg, z axial direction (upper part direction) the power of 9.8N being applied, you insert and it is according to theory. With my environment according to theory it had started hitting the value of 9.8. Furthermore, in the program you regard among and under two boxes the pressure sensor. When the pressure sensor of the humanoid robot''s  sole is simulated, size of the box under the paragraph and number should have been increased small reason are.
PLAIN TEXT
CODE:
  1. // sensor.cpp  by Kosei Demura  2006-7-26
  2. #include
  3. #include
  4.  
  5. static dWorldID world;
  6. static dSpaceID space;
  7. static dGeomID  ground;
  8. static dJointID fixed;
  9. static dJointGroupID contactgroup;
  10. dJointFeedback *feedback = new dJointFeedback;
  11. dsFunctions fn;
  12.  
  13. typedef struct {
  14. dBodyID body;
  15. dGeomID geom;
  16. dReal   radius,length,width,height,mass;
  17. } myLink;
  18. myLink box,sensor;
  19.  
  20. static void nearCallback (void *data, dGeomID o1, dGeomID o2)
  21. {
  22. static int MAX_CONTACTS = 10;
  23. int i;
  24.  
  25. //  do not collide if two bodies are conneted by a joint
  26. dBodyID b1 = dGeomGetBody(o1);
  27. dBodyID b2 = dGeomGetBody(o2);
  28. if (b1 && b2 && dAreConnected (b1,b2)) return;
  29.  
  30. dContact contact[MAX_CONTACTS];   //
  31. int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
  32. sizeof(dContact));
  33. if (numc> 0) {
  34. for (i=0; i
  35. contact[i].surface.mode  =  dContactSoftCFM | dContactSoftERP;
  36. contact[i].surface.mu       = dInfinity;　　//
  37. contact[i].surface.soft_cfm = 1e-8;
  38. contact[i].surface.soft_erp = 1.0;
  39. dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
  40. dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
  41. dGeomGetBody(contact[i].geom.g2));
  42. }
  43. }
  44. }
  45.  
  46. static void simLoop (int pause)
  47. {
  48. static int steps = 0;
  49.  
  50. dSpaceCollide(space,0,&nearCallback);
  51. dWorldStep(world,0.01);
  52. dJointGroupEmpty(contactgroup);
  53.  
  54. feedback = dJointGetFeedback(fixed);　//  get  force and torque information
  55. printf("%5d Force fx=%6.2f ",steps++,feedback->f1[0]);　//  x coordinate
  56. printf("fy=%6.2f ",feedback->f1[1]);　　　　　　　　　　　　　//  y coordinate
  57. printf("fz=%6.2f \n",feedback->f1[2]);　　　　　　　　　　　　//  z coordinate
  58.  
  59. //  draw a box
  60. dsSetColor(1.0,0.0,0.0);
  61. dReal sides1[] = {box.length,box.width,box.height};
  62. dsDrawBoxD(dBodyGetPosition(box.body),
  63. dBodyGetRotation(box.body),sides1);
  64.  
  65. //  draw a sensor
  66. dsSetColor(0.0,0.0,1.0);
  67. dReal sides2[] = {sensor.length,sensor.width,sensor.height};
  68. dsDrawBoxD(dBodyGetPosition(sensor.body),
  69. dBodyGetRotation(sensor.body),sides2);
  70. }
  71.  
  72. void start()
  73. {
  74. static float xyz[3] = {0.0,-3.0,1.0};
  75. static float hpr[3] = {90.0,0.0,0.0};
  76. dsSetViewpoint (xyz,hpr);
  77. }
  78.  
  79. void  setDrawStuff() {
  80. fn.version = DS_VERSION;
  81. fn.start   = &start;
  82. fn.step    = &simLoop;
  83. fn.command = NULL;
  84. fn.stop    = NULL;
  85. fn.path_to_textures = "../../drawstuff/textures";
  86. }
  87.  
  88. int main (int argc, char **argv)
  89. {
  90. setDrawStuff();
  91.  
  92. world = dWorldCreate();
  93. space = dHashSpaceCreate(0);
  94. contactgroup = dJointGroupCreate(0);
  95. dWorldSetGravity(world,0,0,-9.8);
  96.  
  97. ground = dCreatePlane(space,0,0,1,0);
  98.  
  99. dMass m1;
  100. dReal x0 = 0.0, y0 = 0.0, z0 = 0.0;
  101.  
  102. //  a sensor  (blue box)
  103. sensor.length = 0.2;  sensor.width  = 0.2;
  104. sensor.height = 0.2;  sensor.mass   = 1.0;
  105. sensor.body   = dBodyCreate(world);
  106. dMassSetZero(&m1);
  107. dMassSetBoxTotal(&m1,sensor.mass,sensor.length,sensor.width,sensor.height);
  108. dBodySetMass(sensor.body,&m1);
  109. dBodySetPosition(sensor.body, x0, y0, 0.5 * sensor.height + z0);
  110.  
  111. sensor.geom = dCreateBox(space,sensor.length,sensor.width,sensor.height);
  112. dGeomSetBody(sensor.geom,sensor.body);
  113.  
  114. //  a box   (red box）
  115. box.length = 0.2;  box.width  = 0.2;
  116. box.height = 0.2;  box.mass   = 1.0;
  117. box.body   = dBodyCreate(world);
  118. dMassSetZero(&m1);
  119. dMassSetBoxTotal(&m1,box.mass,box.length,box.width,box.height);
  120. dBodySetMass(box.body,&m1);
  121. dBodySetPosition(box.body, x0, y0, sensor.height + 0.5 * box.height + z0);
  122.  
  123. box.geom = dCreateBox(space,box.length,box.width,box.height);
  124. dGeomSetBody(box.geom,box.body);
  125.  
  126. //  a fixed joint
  127. fixed = dJointCreateFixed(world,0);
  128. dJointAttach(fixed,box.body,sensor.body);
  129. dJointSetFixed(fixed);
  130.  
  131. //  Set a joint to retrieve force and torque information
  132. dJointSetFeedback(fixed,feedback);
  133.  
  134. dsSimulationLoop(argc,argv,352,288,&fn);
  135. dWorldDestroy(world);
  136. return 0;
  137. }

投稿：07年01月09日

Leave a Reply

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