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

ODE本　Step2 扉図のカラー画像

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

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

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

• ソースコード
  • プログラム2.1：　ヒンジジョイントの制御(P32)
  • プログラム2.2：　ヒンジジョイントの力制御(P33)
  • プログラム2.3：　スライダージョイントの作り方(P34)
  • プログラム2.4：　スライダージョイントの動かし方(P35)
• EX2.1補足　(P33)
  ヒントで「controlHinge関数をsimLoop関数の中に入れます。」という意味は、simLoop関数の中で呼び出すという意味です。関数の定義（本体）は、simLoop関数のすぐ上に挿入してください。プログラム2.5(P37)が参考になると思います。ここでは、controlSlider関数がsimLoop関数の中で呼び出されていますね。

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

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

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

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

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

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

• グリッド，芝とグリッド

• プログラム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);
  }