コメントスパム対策をしましました。 ODE Tutorial 13: Let's make a pressure sensor !
1 月 09

ODE Tutorial 12: Let's make a torque sensor !

English Add comments
This page was maily translated from http://demura.net/archives/9ode by Babel Fish Translation. As been in the question from Jimmy, as for the command which inspects torque and the power which depend on the joint it  is net easy to find on the ODE user manual. With ODE the torque and the like which falls on the joint cannot be acquired immediately. In order to acquire, after appointing the joint with dJointSetFeedback, you acquire information with dJointGetFeedback. As for this is in order to improve performance. It is not the case that power and the torque which fall on all joints always are necessary don't you think?.
  • Void dJointSetFeedback (dJointID and dJointFeedback *); The joint which acquires the information of power and torque is appointed with dJointID. The dJointFeedback structure is as follows. 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. The box is raised the fixed joint (Fixed Joint) with the land (static object), the stick and the box where the weight has been attached with the fixed joint which becomes the sensor there are connected. It is the program which indicates the power which depends on the sensor. You can ignore the weight of the stick almost with 0.0001kg. It meaning that mass of the weight is 1kg, torque of the around x axis if it is 9.8Nm, is good reason. With my environment according to theory it had started hitting the value of 9.8.
PLAIN TEXT
CODE:
  1. // torque.cpp  by Kosei Demura  2006-12-05
  2. #include
  3. #include
  4.  
  5. #ifdef dDOUBLE
  6. #define dsDrawCylinder dsDrawCylinderD
  7. #define dsDrawSphere   dsDrawSphereD
  8. #endif
  9.  
  10. static dWorldID world;
  11. static dSpaceID space;
  12. static dGeomID  ground;
  13. static dJointID sensor,fixed[2];
  14. static dJointGroupID contactgroup;
  15. dJointFeedback *feedback = new dJointFeedback;
  16. dsFunctions fn;
  17.  
  18. typedef struct {
  19. dBodyID body;
  20. dGeomID geom;
  21. dReal   radius,length,width,height,mass;
  22. } myLink;
  23. myLink stick, ball, box;
  24.  
  25. static void nearCallback (void *data, dGeomID o1, dGeomID o2)
  26. {
  27. static int MAX_CONTACTS = 10;
  28. int i;
  29.  
  30. // exit without doing anything if the two bodies are connected by a joint
  31. dBodyID b1 = dGeomGetBody(o1);
  32. dBodyID b2 = dGeomGetBody(o2);
  33. if (b1 && b2 && dAreConnected(b1,b2)) return;
  34.  
  35. dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  36. int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
  37. sizeof(dContact));
  38. if (numc> 0) {
  39. for (i=0; i
  40. contact[i].surface.mode  =  dContactSoftCFM | dContactSoftERP;
  41. contact[i].surface.mu       = dInfinity;
  42. contact[i].surface.soft_cfm = 1e-8;
  43. contact[i].surface.soft_erp = 1.0;
  44. dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
  45.  
  46. dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
  47. dGeomGetBody(contact[i].geom.g2));
  48. }
  49. }
  50. }
  51.  
  52. static void simLoop (int pause)
  53. {
  54. static int steps = 0;
  55.  
  56. dSpaceCollide(space,0,&nearCallback);
  57. dWorldStep(world,0.01);
  58. dJointGroupEmpty(contactgroup);
  59.  
  60. feedback = dJointGetFeedback(sensor);
  61. printf("%5d Torque Tx=%6.2f ",steps++,feedback->t1[0]);
  62. printf("Ty=%6.2f ",feedback->t1[1]);
  63. printf("Tz=%6.2f \n",feedback->t1[2]);
  64.  
  65. // draw a box
  66. dsSetColor(1.3,1.3,0.0);
  67. dReal sides1[] = {box.length,box.width,box.height};
  68. dsDrawBoxD(dBodyGetPosition(box.body),
  69. dBodyGetRotation(box.body),sides1);
  70.  
  71. // draw a ball
  72. dsSetColor(1.3,0.0,0.0);
  73. dsDrawSphere(dBodyGetPosition(ball.body),
  74. dBodyGetRotation(ball.body),ball.radius);
  75.  
  76. // draw a stick
  77. dsSetColor(0.0,0.0,1.3);
  78. dsDrawCylinder(dBodyGetPosition(stick.body),
  79. dBodyGetRotation(stick.body),stick.length, stick.radius);
  80. }
  81.  
  82. void start()
  83. {
  84. static float xyz[3] = {3.0,0.0,1.0};
  85. static float hpr[3] = {180.0,0.0,0.0};
  86. dsSetViewpoint (xyz,hpr);
  87. dsSetSphereQuality(3);
  88. }
  89.  
  90. void  setDrawStuff() {
  91. fn.version = DS_VERSION;
  92. fn.start   = &start;
  93. fn.step    = &simLoop;
  94. fn.command = NULL;
  95. fn.stop    = NULL;
  96. fn.path_to_textures = "../../drawstuff/textures";
  97. }
  98.  
  99. int main(int argc, char **argv)
  100. {
  101. setDrawStuff();
  102.  
  103. world = dWorldCreate();
  104. space = dHashSpaceCreate(0);
  105. contactgroup = dJointGroupCreate(0);
  106. dWorldSetGravity(world,0,0,-9.8);
  107. dWorldSetERP(world,1.0);
  108.  
  109. // Create a ground
  110. ground = dCreatePlane(space,0,0,1,0);
  111.  
  112. dMass m1;
  113. dReal x0 = 0.0, y0 = 0.0, z0 = 0.5;
  114. dReal x1 = 0.0, y1 = 0.5, z1 = 1.0;
  115. dReal x2 = 0.0, y2 = 1.0, z2 = 1.0;
  116. dMatrix3 R;
  117.  
  118. // box
  119. box.length = 0.1;  box.width  = 0.1;
  120. box.height = 1.0;  box.mass   = 1.0;
  121. box.body   = dBodyCreate(world);
  122. dMassSetZero(&m1);
  123. dMassSetBoxTotal(&m1,box.mass,box.length,box.width,box.height);
  124. dBodySetMass(box.body,&m1);
  125. dBodySetPosition(box.body, x0, y0, z0);
  126. box.geom = dCreateBox(space,box.length,box.width,box.height);
  127. dGeomSetBody(box.geom,box.body);
  128.  
  129. // stick
  130. stick.length = 1.0;  stick.radius = 0.01;
  131. stick.mass   = 0.0001;
  132. stick.body   = dBodyCreate(world);
  133. dMassSetZero(&m1);
  134. dMassSetCylinderTotal(&m1,stick.mass,3, stick.radius, stick.length);
  135. dBodySetMass(stick.body,&m1);
  136. dBodySetPosition(stick.body, x1, y1, z1);
  137. dRFromAxisAndAngle(R, 1, 0, 0, M_PI/2);
  138. dBodySetRotation(stick.body, R);
  139. stick.geom = dCreateCylinder(space,stick.radius,stick.length);
  140. dGeomSetBody(stick.geom,stick.body);
  141.  
  142. // ball
  143. ball.radius = 0.1;
  144. ball.mass   = 1.0;
  145. ball.body   = dBodyCreate(world);
  146. dMassSetZero(&m1);
  147. dMassSetSphereTotal(&m1,ball.mass,ball.radius);
  148. dBodySetMass(ball.body,&m1);
  149. dBodySetPosition(ball.body, x2, y2, z2);
  150. ball.geom = dCreateSphere(space,ball.radius);
  151. dGeomSetBody(ball.geom,ball.body);
  152.  
  153. // fixed joint
  154. fixed[0] = dJointCreateFixed(world,0);
  155. dJointAttach(fixed[0],0, box.body);
  156. dJointSetFixed(fixed[0]);
  157.  
  158. fixed[1] = dJointCreateFixed(world,0);
  159. dJointAttach(fixed[1],stick.body,ball.body);
  160. dJointSetFixed(fixed[1]);
  161.  
  162. sensor = dJointCreateFixed(world,0);
  163. dJointAttach(sensor,box.body, stick.body);
  164. dJointSetFixed(sensor);
  165.  
  166. // set feed back
  167. dJointSetFeedback(sensor,feedback);
  168.  
  169. dsSimulationLoop(argc,argv,640, 480,&fn);
  170.  
  171. dWorldDestroy(world);
  172.  
  173. return 0;
  174. }

投稿：07年01月09日

Leave a Reply

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