demura.net:RoboCup・ODE・夢考房

一昨年，レーザーラモンHGさんは小学生に人気があったそうですが今はそのブームを過ぎテレビでもあまり姿を見かけなくなりした．原ゆたか先生の怪傑ゾロリも一昨年までの勢いは少し失速し，オヤジギャグをかます小学生はすっかり影を潜め，私のようなオヤジ達が愛好する本来の姿に戻りました．このように流行は廃れるのも早いですが，ODEはますますユーザを拡大しています． ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジンODE(Open Dynamics  Engine、オープン ダイナミクスエンジン）を学ぶODE講座の第８回目です。 今回は関節（ジョイント）の動かし方を学びましょう。 ジョイントは以下の手順とAPIを使って動かします。

1. ジョイントの可動域を設定する dJointSetHingeParam(dJointID　joint_no, dParamLoStop）; dJointSetHingeParam(dJointID  joint_no,  dParamHiStop); joint_noにはジョイントのID番号、dParamLoStopは関節の可動域の下限[rad]、dParamHiStopは可動域の上限[rad]。
2. ジョイントの目標角速度とそれを実現するための最大トルクを設定する dJointSetHingeParam(dJointID, dParamVel); dJointSetHingeParam(dJointID, dParamFMax) dParamVelは関節の目標角速度[rad]/s]（直動式関節の場合は速度[m/s]）、dParamFMaxはその速度を達成するために発揮するトルク[Nm]（直動式関節の場合は力[N])。

な お，ODEでは関節の目標角度を直接指定するAPIがありませんので，目標角度に関節角をもっていくためには，現在の関節角を dJointGetHingeAngleで取得し，その角度になるまでdJointSetHingeParamを使って角速度を関節に与え，目標角度に なったら角速度を０にするという方法を使います． では，ソー スコードを以下に示します。前回との違いはオブジェクトを胴体(torso)、上腿、下腿をもつ１本足ロボットMonoBotに変更しています。自由度は 腰関節(hip joint）が１、膝関節(knee joint)が１の計２自由度となっていますが、今回は腰関節だけ動かします。

PLAIN TEXT

CODE:

1. //　sample4.c by  by でむ
2.  
3. #ifdef  dDOUBLE
4. #define dsDrawSphere dsDrawSphereD   // 単精度と倍精度の描画関数に対応するおまじない
5. #define dsDrawCapsule  dsDrawCapsuleD
6. #endif
7.  
8. myLink torso,leg[2];
9. dJointID joint[2];
10.  
11. enum jointNo {
12. HIP,
13. KNEE
14. };
15.  
16. // ジョイントの制御
17. void control()
18. {
19. double velCoeff = 1.0;　// 比例定数
20. double p; 現在の関節角度
21. static double angle = 0.5 * M_PI; // M_PIは円周率
22.  
23. p =  dJointGetHingeAngle(joint[HIP]);　// 現在の関節角度の取得
24. dReal z = angle  -  p;　　　　　　　　　　　   // angleは目標角度[rad]
25. if (p>   0.25 * M_PI)    angle  = - 0.25 * M_PI;  // 目標角度の下限
26. if (p <- 0.25 * M_PI)    angle  =    0.25 * M_PI;　// 目標角度の上限
27.  
28. dJointSetHingeParam(joint[HIP], dParamVel,  velCoeff*z);　// 目標角速度[rad/s]の設定
29. dJointSetHingeParam(joint[HIP], dParamFMax, 100);　      // 最大トルク[Nm]の設定
30. }
31.  
32. static void simLoop (int pause)
33. {
34. const dReal *pos1,*R1,*pos2,*R2;
35.  
36. time++;
37. control();  　　　　　　　　　　　　　　　　　　// ジョイントの制御
38. dSpaceCollide(space,0,&nearCallback);  // 衝突検出計算
39. dWorldStep(world,0.01);　　　　　　　　　//  動力学計算
40. dJointGroupEmpty(contactgroup);　　　// 衝突検出計算の後始末（接触点グループの解放）
41.  
42. // draw a torso   胴体を描画
43. dsSetColor(1.0,0.0,0.0);　　　　　　　　　　　　// 色の設定(r, g, b) ここでは赤
44. pos1 = dBodyGetPosition(torso.body);　　// 位置の取得
45. R1   = dBodyGetRotation(torso.body);　　 //  回転行列の取得
46. dsDrawSphere(pos1,R1,torso.radius);         // 球の描画
47.  
48. // draw legs    脚を描画
49. for (int i = 0; i<2; i++) {
50. pos2 = dBodyGetPosition(leg[i].body);
51. R2   = dBodyGetRotation(leg[i].body);
52. dsDrawCapsule(pos2,R2,leg[i].length,leg[i].radius);　　// カプセルの描画
53. }
54. }
55.  
56. // 　Create a monobot  一本足ロボットの生成
57. void createMonoBot()
58. {
59. dMass m1;
60. dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
61.  
62. // torso   胴体
63. torso.radius = 0.2;　　　　//  半径[m]
64. torso.mass   = 10.0;        //  質量[kg]
65. torso.body = dBodyCreate(world);
66. dMassSetZero(&m1);
67. dMassSetSphereTotal(&m1,torso.mass,torso.radius);
68. dBodySetMass(torso.body,&m1);
69. dBodySetPosition(torso.body, x0, y0, z0);
70. torso.geom = dCreateSphere(space,torso.radius);
71. dGeomSetBody(torso.geom,torso.body);
72.  
73. // legs    上腿、下腿
74. for (int i = 0; i <2; i++) {
75. leg[i].radius = 0.025;    leg[i].length = 0.5;　 leg[i].mass   = 1.0;
76. leg[i].body = dBodyCreate(world);
77. dMassSetZero(&m1);
78. dMassSetCapsuleTotal(&m1,leg[i].mass,3,leg[i].radius,leg[i].length);
79. dBodySetMass(leg[i].body,&m1);
80. dBodySetPosition(leg[i].body, x0, y0, z0 -torso.radius
81. - 0.5 * (2 * i + 1) *leg[i].length);
82. leg[i].geom = dCreateCapsule(space,leg[i].radius,leg[i].length);
83. dGeomSetBody(leg[i].geom,leg[i].body);
84. }
85.  
86. // hip joint         腰関節
87. joint[HIP] = dJointCreateHinge(world, 0);
88. dJointAttach(joint[HIP], torso.body,leg[0].body);
89. dJointSetHingeAnchor(joint[HIP], x0, y0, z0 - torso.radius);
90. dJointSetHingeAxis(joint[HIP], 1, 0, 0);
91. dJointSetHingeParam(joint[HIP], dParamLoStop, -0.25 * M_PI);　// 可動域設定 下限
92. dJointSetHingeParam(joint[HIP], dParamHiStop,  0.25 * M_PI);　// 上限　単位はradian
93.  
94. // knee joint    膝関節
95. joint[KNEE] = dJointCreateHinge(world, 0);
96. dJointAttach(joint[KNEE],leg[0].body,leg[1].body);
97. dJointSetHingeAnchor(joint[KNEE], x0, y0, z0 - torso.radius - leg[0].length);
98. dJointSetHingeAxis(joint[KNEE], 1, 0, 0);
99. dJointSetHingeParam(joint[KNEE], dParamLoStop, 0.0 * M_PI);　// 可動域
100. dJointSetHingeParam(joint[KNEE], dParamHiStop, 0.0 * M_PI);　// M_PIは円周率π（パイ）
101.  
102. }
103.  
104. int main (int argc, char **argv)
105. {
106. setDrawStuff();
107.  
108. world = dWorldCreate();
109. space = dHashSpaceCreate(0);
110. contactgroup = dJointGroupCreate(0);
111.  
112. dWorldSetGravity(world,0,0,-9.8);
113. ground = dCreatePlane(space,0,0,1,0);
114. createMonoBot();　// 一本足ロボットの生成
115. dsSimulationLoop (argc,argv,352,288,&fn);
116. dWorldDestroy(world);
117. return 0;
118. }

ジョ イントを動かしている関数controlは目標値と現在値の差に比例して制御するＰ制御と なっています。ジョイントのパラメータdParamVelは目標角速度で、dParamFMaxはその角速度を実現するためにジョイントが発揮できる最大 トルクです。この値を０に設定すると目標角速度を設定してもトルクが０なのでジョイントとは動いてくれません。 ここからsample4.cのソースコードをダウンロードして遊んでください。では、また次回。 更新履歴

• 2008-1-13: コードを整形した．
• 2007-1-14: サンプルプログラムのコメントをより詳しくした。

投稿：06年11月19日

4 Responses to “ODE講座８：関節（ジョイント）を動かそー！”

1. 悩める初心者 Says at 2008-04-20 :
   

   でむさん、ご返信ありがとう御座いました。

   PARTごとに分けようと、フォルダに区切ろうと新しいフォルダを
   作成したのが原因でした。
   自分なりに調べまして、新しいフォルダにも対応できるように
   修正しました。

2. demu Says at 2008-04-18 :
   

   悩める初心者さん、

   以下のメッセージなのでdrawstuff.hの場所が分からないようです。恐らくファイルを解凍した場所が悪いのだと思います。サンプルプログラムは正しい場所に置かないとコンパイルできないので注意してください。

   sample4.cpp:7:33: drawstuff/drawstuff.h: No such file or directory

   次のディレクトリに解凍してください。
   c:\msys\1.0\home\ユーザ名\src\ode-バージョン番号\myprog

   解凍したら以下のファイルがあるか確認してください。あれば正しい場所に解凍できています。
   c:\msys\1.0\home\ユーザ名\src\ode-バージョン番号\myprog\sample4\sample4.cpp

   でむ

3. 悩める初心者 Says at 2008-04-18 :
   

   でむさん、こんにちわ

   上記のファイルをコンパイルしましたが以下のエラーが出てきました。
   g++ -Wall -fno-exceptions -fno-rtti -g -DWIN32 -c sample4.cpp -L../../drawst
   uff/src -L/usr/X11R6/lib -L/usr/local/lib -L/usr/lib/w32api/lib -I. -I../../inc
   lude -I/usr/X11R6/include -I/usr/include -I/usr/include/w32api/include
   sample4.cpp:7:33: drawstuff/drawstuff.h: No such file or directory
   sample4.cpp:18: error: 'dsFunctions' is used as a type, but is not defined as a

   type.
   sample4.cpp: In function `void simLoop(int)':
   sample4.cpp:84: error: `dsSetColor' undeclared (first use this function)
   sample4.cpp:84: error: (Each undeclared identifier is reported only once for
   each function it appears in.)
   sample4.cpp:89: error: `dsDrawSphereD' undeclared (first use this function)
   sample4.cpp:95: error: `dsDrawCapsuleD' undeclared (first use this function)
   sample4.cpp: In function `void start()':
   sample4.cpp:103: error: `dsSetViewpoint' undeclared (first use this function)
   sample4.cpp: In function `void prepDrawStuff()':
   sample4.cpp:107: error: `fn' undeclared (first use this function)
   sample4.cpp:107: error: `DS_VERSION' undeclared (first use this function)
   sample4.cpp: In function `int main(int, char**)':
   sample4.cpp:190: error: `dsSimulationLoop' undeclared (first use this function)
   make: *** [sample4.o] Error 1

   私の予想では何か関数内でエラーがしょうじてるのではないでしょうか？
   自分なりに考えましたが答えが見つからなかったので、よろしければ教えてもらえないでしょうか？

4. CohCoh Says at 2008-01-15 :
   

   おそらく該当するのはこの講座では無いかと思い、
   ここに質問させていただきます。
   以前はシミュレーション中に描画しない方法を教えて頂き、
   本当にありがとうございました。

   いまODEを用いて、腰、膝、踝の3関節と
   上腿、下腿、足の3関節からなるヒトの脚を模した
   力学モデルを作成し、運動のシミュレーションを行っているのですが、
   各関節に初期角速度を設定する方法が分からなくて困っています。

   dJointSetHingeParam(joint, dParamVel, 値);

   という関数で設定できるのかと思ったのですが、
   ODE本やこのサイトを見ると「目標角速度」と記述が有り、
   どうやら求めている、初期角速度の設定では無いようで、
   どのように設定したら良いか分かりません。

   運動途中の一部分を抜き出して、
   その部分に対しシミュレーションを行っているので、
   初期速度(加速度)が設定できないと、
   正確なシミュレーションが出来ません。

   もしよろしければ、設定方法を教えていただけないでしょうか。

Leave a Reply