ODE Tutorial 11: Position and Orientation ODEプチキャンプ3：iMacライクなロボットアームを作ろう！
12 月 12

ODE講座13：圧力センサを作ろう！

ODE, ODE講座 Add comments

ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジンODE(Open Dynamics  Engine、オープン ダイナミクスエンジン）を学ぶODE講座の第１３回目です。

今回は ジョイント（関節）にかかる力を取得する方法を勉強します．

これを応用するとヒューマノイドロボットの足裏に付け，ZMPの計測に利用する圧力センサをシミュレートできます．

  • void dJointSetFeedback (dJointID, dJointFeedback *);
    力とトルクの情報を取得する関節JointIDにdJointFeedback構造体を設定する．
    typedef struct dJointFeedback {
    dVector3 f1; // 関節がボディ1に及ぼしている力
    dVector3 t1; // 関節がボディ1に及ぼしているトルク
    dVector3 f2; // 関節がボディ2に及ぼしている力
    dVector3 t2; // 関節がボディ2に及ぼしているトルク
    } dJointFeedback;
  • dJointFeedback *dJointGetFeedback (dJointID);
    dJointIDで指定している関節の力とトルクの情報を取得します．

次 に，このAPIを使ったサンプルプログラムを紹介します．２つのボックスを固定ジョイント(Fixed Joint)でくっつけ，そこにかかる力を表示するプログラムです．ボックスの重さが各1kgなので，z軸方向（上方向）には9.8Nの力がかかっていれ ば理論どおりです．私の環境では理論どおり9.8の値をたたき出していました．

なお，プログラムでは２つのボックスのうち，下を圧力センサとみなしています．ヒューマノイド足裏の圧力センサをシミュレートする場合は，下のボックスのサイズを小さくし，数を増やせばよいわけです．

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. // 2つのボディがジョイントで結合されていたら衝突検出しない
  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);　// 力とトルク情報の取得
  55. printf("%5d Force fx=%6.2f ",steps++,feedback->f1[0]);　// x座標成分
  56. printf("fy=%6.2f ",feedback->f1[1]);　　　　　　　　　　　　　// y座標成分
  57. printf("fz=%6.2f \n",feedback->f1[2]);　　　　　　　　　　　　// z座標成分
  58.  
  59. // ボックスの描画
  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. // センサの描画
  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. // センサ（下のボックス）
  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. // ボックス（上のボックス）
  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. // 固定ジョイント
  127. fixed = dJointCreateFixed(world,0);
  128. dJointAttach(fixed,box.body,sensor.body);
  129. dJointSetFixed(fixed);
  130.  
  131. // 力とトルク情報を取得する関節を指定
  132. dJointSetFeedback(fixed,feedback);
  133.  
  134. dsSimulationLoop(argc,argv,352,288,&fn);
  135. dWorldDestroy(world);
  136. return 0;
  137. }

投稿：06年12月12日

Leave a Reply

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