21万アクセス ODE講座6補講：衝突検出
1 月 16

ODE講座24：AMOTORはエー(良い)モータ

ODE, ODE講座 Add comments

AMOTOR
図：２リンク機構に初期角速度を与えるシミュレーション
CohCohさんのご質問で初期の関節角速度の設定法がありましたので、久しぶりにODE(Open Dynamics Engine)講座を開講します。

ODEでは関節にモータが標準で付属していますが、関節角速度を設定しようとしても、目標角速度に到達するまで時間がかかりますし、その角速度を維持するためにはそれなりの制御が必要です。現実のモータを単純にモデル化したものです。

これでは、初期角速度を設定できません。この問題を解決する方法として、ODEには理想的なAMOTORがあります。マニュアルでは１ステップで目標角速度に到達するとありますが、関節に与える最大トルクdParamFmaxの値が0だと目標角速度に到達しません。また、小さな値の場合は、目標角速度に到達するまで数ステップかかることを確認しました。

そこで、十分大きな値を設定してください。以下のサンプルプログラムでは目標角速度dParamVelに無限大dInifinityを与えています。

また、初期角速度だけ与えたい場合は、角速度が目標値に到達したら、dJointDestroy()でamotorを破壊する必要があります。これを忘れると常に角速度は目標角速度のままです。amotorを破壊したくない場合は、dJointSetAMotorNumAxes(amotor, 0)で制御する軸数を０に設定し、無効化してもＯＫです。

次に、APIを紹介します。

dJointID dJointCreateAMotor(dWorldID, dJointGroupID);
AMOTORの生成。

void dJointSetAMotorMode(dJointID, int mode);
AMOTORのモード設定。modeはユーザが独自に設定するdAMotorUserまたは、自動的にオイラー角が計算されるdAMotorEulerの２つのモードがある。

void dJointSetAMotorNumAxes(dJointID, int num);
AMOTORでコントロールされる軸数を設定する。numの値は0から3までで、0の場合はジョイントを無効化する。dAMotorEulerモードの場合は自動的に3が設定される。

void dJointSetAMotorAxis (dJointID, int anum, int rel, dReal x, dReal y, dReal z);
AMOTORの回転軸を設定する。引数anumは変更する軸(0, 1, 2)、relは以下の相対回転モードを設定する。最後の(x,y,z)は回転軸ベクトル。

  • 0: 軸は絶対座標系に固定
  • 1: 1番目のボディに固定
  • 2: 2番目のボディに固定

void dJointSetAMotorParam (dJointID, int parameter, dReal value);
AMOTORのパラメータを設定する。 parameterについては他のジョイントと同じ。この例では角速度（スライダージョイントの場合は速度）dParamVel、最大発揮トルクdParamFMaxを使用した。

AMOTORはAngular Motorの略ですが、エー（良い）モータですね。ソースコードを以下に示しますので試してください。
なお、簡単にするために衝突検出機能は実装されていません。ここからダウンロード可能です(amotor2-080115.tgz)。

PLAIN TEXT
CODE:
  1. // 2008-1-15
  2. // amotor2.c: 2 Links with an amotor by Kosei Demura
  3. #include
  4. #include
  5. #include
  6. #include
  7.  
  8. #define NUM 2
  9. #ifdef dDOUBLE
  10. #define dsDrawCylinder dsDrawCylinderD
  11. #endif
  12.  
  13. dWorldID      world;
  14. dSpaceID      space;
  15. dGeomID       ground;
  16. dJointID      joint[NUM], amotor;
  17. dsFunctions fn;
  18.  
  19. typedef struct {
  20. dBodyID body;
  21. dGeomID geom;
  22. } MyLink;
  23.  
  24. MyLink rlink[NUM];
  25.  
  26. void  createArm()
  27. {
  28. dReal init_avel = 1.0; // 初期角速度[rad/s]
  29. dMass mass;
  30. dReal x[] = { 0.00, 0.00}, y[] = { 0.00, 0.00}, z[]= { 0.75 ,0.25}; // 重心
  31. dReal length[] = { 0.50, 0.50};   // リンク長
  32. dReal weight[] = { 1.00, 1.00};   // 質量
  33. dReal r[]        = { 0.02, 0.02}; // 半径
  34. dReal anchor_x[] = { 0.00, 0.00}; // 回転軸中心
  35. dReal anchor_y[] = { 0.00, 0.00};
  36. dReal anchor_z[] = { 1.00, 0.50};
  37. dReal axis_x[] = { 1.00, 1.00};   // 回転軸ベクトル
  38. dReal axis_y[] = { 0.00, 0.00};
  39. dReal axis_z[] = { 0.00, 0.00};
  40.  
  41. // リンクの生成
  42. for (int i = 0; i <NUM; i++) {
  43. rlink[i].body = dBodyCreate(world);
  44. dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
  45. dMassSetZero(&mass);
  46. dMassSetCylinderTotal(&mass,weight[i],3,r[i],length[i]);
  47. dBodySetMass(rlink[i].body, &mass);
  48. rlink[i].geom = dCreateCylinder(space,r[i],length[i]);
  49. dGeomSetBody(rlink[i].geom,rlink[i].body);
  50. }
  51.  
  52. //ジョイントの生成と結合
  53. for (int j = 0; j <NUM; j++) {
  54. joint[j] = dJointCreateHinge(world, 0);
  55.  
  56. // リンク１はjoint[0]により静的環境0と結合
  57. if (j == 0) dJointAttach(joint[0], rlink[0].body, 0);
  58. else         dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
  59. dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j], anchor_z[j]);
  60. dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
  61. }
  62.  
  63. // リンク1とリンク2を結合しているジョイントを可動域設定
  64. dJointSetHingeParam(joint[1], dParamLoStop, -0.0 * M_PI); // 可動域の下限
  65. dJointSetHingeParam(joint[1], dParamHiStop,  0.0 * M_PI); // 可動域の上限
  66.  
  67. // amotorの生成
  68. amotor = dJointCreateAMotor(world, 0);    // AMotorの生成
  69. dJointAttach(amotor, rlink[0].body, 0);   // AMotorとリンクとの結合
  70. dJointSetAMotorMode(amotor, dAMotorUser); // ユーザモードに設定、デフォルト、省略可
  71. dJointSetAMotorNumAxes(amotor, 1);        // AMotorでコントロールする軸数
  72.  
  73. // 軸の設定：回転軸0を絶対座標系に固定、回転軸ベクトルは(1, 0, 0)
  74. dJointSetAMotorAxis(amotor,  0 /*軸0*/, 0 /*絶対座標系に固定*/, 1 /*x成分*/, 0 /*y成分*/, 0 /*z成分*/);
  75.  
  76. // amotorの設定, 初期角速度をinit_avelに設定
  77. dReal fmax = dInfinity;　// 無限大
  78. dJointSetAMotorParam(amotor, dParamVel, init_avel);
  79. dJointSetAMotorParam(amotor, dParamFMax, fmax);
  80. }
  81.  
  82. void drawLink()
  83. {
  84. dReal r,length;
  85.  
  86. for (int i = 0; i <NUM; i++ ) {
  87. if (i == 1) dsSetColor(1.3, 1.3, 0.0);
  88. else        dsSetColor(0.0, 0.0, 1.3);
  89. dGeomCylinderGetParams(rlink[i].geom, &r,&length);
  90. dsDrawCylinder(dBodyGetPosition(rlink[i].body),
  91. dBodyGetRotation(rlink[i].body),length,r);
  92. }
  93. }
  94.  
  95. void start()
  96. {
  97. float xyz[3] = {  2,  0, 1};
  98. float hpr[3] = {-180, 0, 0.0};
  99. dsSetViewpoint(xyz,hpr);
  100. }
  101.  
  102. // 簡単にするため衝突検出は未実装
  103. void simLoop(int pause)
  104. {
  105. static int step = 0;
  106.  
  107. drawLink();
  108. const dReal ang_rate = dJointGetHingeAngleRate(joint[0]);
  109. printf("step=%4d: angle rate=%8.6f \n", step, ang_rate);
  110. dWorldStep(world, 0.001);
  111.  
  112. dReal k1 = 0.0; // 摩擦トルク用比例係数 負値を設定
  113. dJointAddHingeTorque(joint[0], k1 * ang_rate); // 摩擦トルクを付加
  114.  
  115. if (step == 0) dJointDestroy(amotor); // amotorの破壊
  116. // amotorを破壊したくない場合は以下でも良い
  117. // if (step == 0) dJointSetAMotorNumAxes(amotor, 0);
  118.  
  119. step++;
  120. }
  121.  
  122. void setDrawStuff() {
  123. fn.version = DS_VERSION;
  124. fn.start   = &start;
  125. fn.step    = &simLoop;
  126. fn.path_to_textures = "../../drawstuff/textures";
  127. }
  128.  
  129. int main(int argc, char *argv[])
  130. {
  131. setDrawStuff();
  132. world = dWorldCreate();
  133. space = dHashSpaceCreate(0);
  134. // dWorldSetGravity (world, 0, 0, -9.8); // 重力加速度
  135. createArm();
  136. dsSimulationLoop(argc,argv,640, 480,&fn);
  137. dSpaceDestroy(space);
  138. dWorldDestroy(world);
  139. return 0;
  140. }

終わり。

投稿：08年01月16日

Leave a Reply

カウンタ (since 2008-1-11）