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

Open Dynamics Engine

ODE本 Step2 扉図のカラー画像


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


ODE本「簡単!実践!ロボットシミュレーション – Open Dynamics Engineによるロボットプログラミング

」のStep2です.

ここでは,ロボットを動かす上で必要になる関節を動かす方法,シミュレーションの再実行法,高速化,キーボードからの操作を学びホッピングロボットのシミュレータを作ります.

以下にソースコードと動画を掲載します.zipで圧縮されているソースコードはここを参考にして解凍・コンパイル・実行してください.それ以外のソースコードはプログラムの一部なので,そのままではコンパイルできません.

なお,本サンプルプログラムのコンパイル・実行は自己責任で行ってください.特に,本物のロボットに適用するための安全性などを全く考慮していませんの で,そのような用途には向きません.あくまで,教育目的とお考えください.著者及び森北出版は本プログラムによって生じたあらゆる結果についての責任を負いかねます.ご了承した方だけダウンロードしてください.また,再配布はお止めください.


  • プログラム2.5: ホッピングロボット(P37)
  • 説明:スライダ(直動式)ジョイントの動かし方を学び,一本脚ロボットをちょっとジャンプさせます.
  • ソースコード:pro2-5.zip (2007-5-19)

  • プログラム2.6: ドロースタッフの設定(P40)
  • プログラム2.7: comman関数(P41)
  • プログラム2.8: simLoop関数の変更(P42)

    • プログラム2.9: ホッピングロボット,再実行可能版 (P44)
    • 説明:シミュレーションは普通は1回では終わらず,何度も繰り返し実行しなければなりませんよね.ここでは,ミュレーションの再実行法を学びます.
    • ソースコード:pro2-9.zip (2007-5-19)

    • プログラム2.10: ホッピングロボット,描画無版 (P50)
  • 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);
      }

    7 Comments
    1. かつさん,

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

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

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

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

      でむ

    2. demuさん

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

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

      解決方法があるようでしたらご教授願います.

    3. かつさん,

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

      解決方法は2つあります.

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

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

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

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

      でむ

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

      ご教授願います.

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

      カプセルについて無事に理解することができました。

    6. 悩める初心者さん、

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

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

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

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

      でむ

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

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

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

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

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

    コメントを残す

    メールアドレスが公開されることはありません。