Step 2：　シミュレータを作ろう

2 月 07

ODE本　Step2 扉図のカラー画像

EX2.1 補足を追加　(2008-2-7)

ODE本「簡単!実践!ロボットシミュレーション - Open Dynamics Engineによるロボットプログラミング」のStep2です．ここでは，ロボットを動かす上で必要になる関節を動かす方法，シミュレーションの再実行法，高速化，キーボードからの操作を学びホッピングロボットのシミュレータを作ります．以下にソースコードと動画を掲載します．zipで圧縮されているソースコードはここを参考にして解凍・コンパイル・実行してください．それ以外のソースコードはプログラムの一部なので，そのままではコンパイルできません．なお，本サンプルプログラムのコンパイル・実行は自己責任で行ってください．特に，本物のロボットに適用するための安全性などを全く考慮していませんので，そのような用途には向きません．あくまで，教育目的とお考えください．著者及び森北出版は本プログラムによって生じたあらゆる結果についての責任を負いかねます．ご了承した方だけダウンロードしてください．また，再配布はお止めください．

ソースコード
EX2.1補足　(P33)

プログラム2.5：　ホッピングロボット(P37)

説明：スライダ（直動式）ジョイントの動かし方を学び，一本脚ロボットをちょっとジャンプさせます．
ソースコード：pro2-5.zip (2007-5-19)

プログラム2.6: ドロースタッフの設定(P40)

プログラム2.7: comman関数(P41)

プログラム2.8: simLoop関数の変更(P42)

プログラム2.9：　ホッピングロボット，再実行可能版 (P44)

説明：シミュレーションは普通は１回では終わらず，何度も繰り返し実行しなければなりませんよね．ここでは，ミュレーションの再実行法を学びます．
ソースコード：pro2-9.zip (2007-5-19)

プログラム2.10：　ホッピングロボット，描画無版 (P50)

pro2-10.zip (2007-5-19)

EX2.8：　テクスチャファイル　(P53)

グリッド，芝とグリッド

プログラム2.1：　ヒンジジョイントの制御(P32) void controlHinge(dReal target) { static const dReal kp 　= 5.0; // 比例定数 static const dReal fmax = 200; // 最大トルク [Nm] dReal tmp = dJointGetHingeAngle(joint);　// 現在の角度を取得 dReal u = kp * (target - tmp);　　　　　　　　// 操作量dJointSetHingeParam(joint,dParamVel,u); 　　　// 角速度の設定 dJointSetHingeParam(joint,dParamFMax,fmax); // 最大トルクの設定 }
プログラム2.2：　ヒンジジョイントの力制御(P33) void controlHinge2(dReal target) { static const dReal kp = 5.0, kt = 2.0; // 比例定数dReal tmp = dJointGetHingeAngle(joint);　 // 現在の角度を取得 dReal u = kp * (target - tmp);　　　　 // 操作量 dReal omega = dJointGetHingeAngleRate(joint); // 角速度 dReal trq = kt * omega;　　　　　　　　　　　　　　　// 摩擦トルク dJointAddHingeTorque(joint, u - trq); // トルクを加える }
プログラム2.3：　スライダージョイントの作り方(P34) s_joint = dJointCreateSlider(world, 0);　　　　　 // ジョイントの生成 dJointAttach(s_joint, leg[0].body,leg[1].body);　// ジョイントの取付 dJointSetSliderAxis(s_joint, 0, 0, 1);　　　　 // 軸ベクトルの設定 dJointSetSliderParam(s_joint, dParamLoStop, -0.5); // 最大収縮長[m] dJointSetSliderParam(s_joint, dParamHiStop, 0.5); // 最大伸展長[m]
プログラム2.4：　スライダージョイントの動かし方(P35) static void controlSlider(dReal target) { static dReal kp = 25.0 // 比例定数 static dReal fmax = 400; // 最大力[N] dReal tmp = dJointGetSliderPosition(s_joint); // スライダの現在位置 dReal u = kp * (target - tmp); 　　　　　// 残差dJointSetSliderParam(s_joint, dParamVel, u); dJointSetSliderParam(s_joint, dParamFMax, fmax); }
プログラム2.6: ドロースタッフの設定(P40) void setDrawStuff() { fn.version = DS_VERSION; // drawStuffのバージョン fn.start = &start;　　// シミュレーションループの前に呼び出される関数 fn.step = &simLoop;　 // ステップ毎に呼びだされる関数のアドレス fn.command = &command;　 // キー入力により呼び出される関数のアドレス fn.path_to_textures = “../../drawstuff/textures”; // テクスチャのパス }
プログラム2.7: comman関数(P41) void command(int cmd) { float xyz[3],hpr[3];　　　　　　　　// 視点,視線switch (cmd) { case ‘a’:funcA();break; // aキーを押すとfuncAを実行 case ‘1′:func1();break;　　　　　 // 1キーを押すとfunc1を実行 case ’s’: // sキーを押すと視点，視線を表示 dsGetViewpoint(xyz,hpr); // 視点，視線を取得 printf(”xyz=%4.2f %4.2f %4.2f “,xyz[0],xyz[1],xyz[2]); printf(”hpr=%6.2f %6.2f %5.2f \n”,hpr[0],hpr[1],hpr[2]); break; default:printf(”Input a or 1\n”);break;// 上記以外のキーを押すとき } }
プログラム2.8: simLoop関数の変更(P42) static void simLoop(int pause) { const dReal *pos1, *R1, *pos2, *R2; int s = 200; 　　　　　// 跳躍する周期(ステップ) if (!pause) { 　　　　　 // 一時停止 STEPS++; 　　　　　 // ステップ数 printf(”STEPS:%4d\n”,STEPS);if ((0 <= (STEPS % s)) &&((STEPS % s) <= 10)) controlSlider(0.5); else controlSlider(0.0); dSpaceCollide(space,0,&nearCallback);　　// 衝突検出計算 dWorldStep(world,0.01); 　　 // 1ステップ進める dJointGroupEmpty(contactgroup); } dsSetColor(1.0,0.0,0.0); // 赤色の設定 dsDrawSphere(dBodyGetPosition(torso.body), // 球の描画 dBodyGetRotation(torso.body), torso. r); }

投稿：08年02月07日

7 Responses to “Step 2：　シミュレータを作ろう”

demu Says at 2008-06-13 :

かつさん，

ODEは２つの物体が衝突した後にお互いに貫通することを許さない拘束(non-penetration constratint)を使っています．物体の速度が速かったり，ステップサイズが大きい場合は貫通してしまうので，特別な力をボディにかけてそれを防いでいます．ERPはその修正具合を調整するパラメータで，ERPが１なら次のステップで誤差を０に修正し，ERPが０なら誤差を修正しません．

さて，ご質問の件ですが，一時停止と１ステップ実行をしたい場合は，ODE本P42の下から７行目にあるように，一時停止したい処理の部分を以下のようにif文で囲みます．そうするとキーで操作できるはずです．私の環境MSYS+MinGWではできました．

if (!pause) {
一時停止したい処理
}

なお，より詳しい説明をODE講座２６としてアップしましたので，参考にしてください．

でむ
かつ Says at 2008-06-12 :

demuさん

早速の回答ありがとうございます．誤差修正は値をオフセットするという数値的なものではなく，実際に力が働いてしまうんですね．なるほどです．

続けて質問ですいません．
2.4.2のキーコマンドのところで，テクスチャおよび影のON/OFFは可能なのですが，
シミュレーションに直接関わる”強制終了””一時停止””ステップ実行”ができません．

解決方法があるようでしたらご教授願います．
demu Says at 2008-06-12 :

かつさん，

演習問題に関するコメントありがとうございます．
かつさんのご指摘のとおりの現象が見られますね．

解決方法は２つあります．

１．createMonoBot関数の最後に以下を加える．
dJointSetSliderParam(s_joint, dParamStopERP, 0.2);
dJointSetSliderParam(s_joint, dParamStopCFM, 1e-5);

サンプルプログラムではmain関数内でdWorldSetERP(), dWorldSetCFM()でERPを1.0，CFMを0.0にしています．これはWorld内の全てのジョイントに作用します．ERPが１ということは１ステップで関節誤差を修正します．このサンプルプログラムではステップサイズが0.01なので，状況により関節の可動域をリンクが超える場合があります．次のステップで関節誤差を０にするために大きな力がリンクにかかることになり，スーパージャンプしてどこかに飛んでいくことになります．

今回のサンプルプログラムではスライダージョイントが問題になるので，その可動域の両端で作用するdParamStopERP, dParamStopCFMを上のように設定すればOKです．

２．　ステップサイズを小さくする
現状のODEでは拘束力の計算以外は１次のオイラー法を使っているため計算精度が高くないので，このような現象が生ずる場合があります．　simLoop関数のdWorldStep()のステップサイズを0.002に変更するとこのような現象が見られなくなりました．　当然ですが，ステップサイズを小さくすると精度は向上しますが，速度は遅くなります．ODEの次期バージョン1.0から４次のルンゲクッタが実装される予定です．そうなると改善するかもしれませんが，当然速度は遅くなるでしょう．

でむ
かつ Says at 2008-06-11 :

EX2.3で質問があります．
パラメータを変更して試してみると，デフォルトの400Nではちょっとしか跳ねないのに，
150Nや100Nだと空に吹っ飛ぶくらい飛んでいきます．
力が小さくなったのに飛んで行ってしまうというのはどうもイメージと合わないのですが，
何が起こっているのでしょうか？

ご教授願います．
悩める初心者 Says at 2008-04-25 :

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

カプセルについて無事に理解することができました。
demu Says at 2008-04-24 :

悩める初心者さん、

１．dMassSetCapsuleTotal()はODE本の６６ページに説明があります。３番目の引数はカプセルの長軸方向がどの方向を向いているかを指定します。1ならx軸、２ならy軸、３ならz軸です。なお、カプセルや円柱は姿勢を指定しない場合は、長軸方向がz軸になります。これは慣性テンソルの計算に必用です。

２．dsで始まるAPIは描画に関するものです。違いは、カプセルで描画するか、円柱で描画するかです。

３．カプセルは円柱の両端に半円がついたものです。ODE本の６６ページ図3.3をご覧ください。

長軸方向の指定はわかりづらいところかもしれませんね。

でむ
悩める初心者 Says at 2008-04-24 :

プログラム2.9で質問でいくつか質問があります。

1つ目は、足の生成での
dMassSetCapsuleTotal(&mass,leg[i].m,3,leg[i].r,leg[i].l);
のソースで、3は何を表している数なんでしょうか？

2つ目は、足の描画で、プログラム2.5では、上脚、下脚共に
dsDrawCapsuleでしたが、2.9では上脚は、dsDrawCylinderに
なっております。この違いは何を意味してるのですか？

最後に、円柱とカプセルの違いが上手く理解できません。
私には２つとも同じに見えてしまいます。

是非よろしくお願いします。

demura.net:RoboCup・ODE・夢考房