ODE講座27: 関節初期角度の設定法


2関節ロボットアーム.1番目の関節初期角度を15度,2番目の関節初期角度を30度に設定.


がきーさんから以下のご質問がありました.「ジョイントの角度の初期設定を行いたいとき(例えばHinge Jointをπ/4だけ曲げるとか)、どのように設定すれば良いのでしょうか?トルクがかからない状態で初期設定を行い、その状態からシミュレーションを始めたいのですが。。。ヒントがあればよろしくお願い致します.」

これに「ジョイントの初期角度は,ジョイントを生成した時が0[rad]になります.例えば,2つのリンクの成す角度π/4を初期角度としたい場合は,片方のリンクをπ/4回転させた後で,ジョイントで2つのリンクを結合します.」と回答しました.

それからまた,がきーさんが「dJointSetHingeAngleがあれば便利なのに...」という書き込みがあり,googleでdJointSetHingeAngle検索したら,ロシア語ですが関連する以下のページを見つけることができました.http://www.gamedev.ru/code/forum/?id=19481その中でShurikParさんが,以下のようにすれば関節を固定できるというコメントがあり,それをヒントに初期角度を設定するサンプルプログラムを作りました.

  • dJointSetHingeParam (Joint, dParamLoStop, angle – 0.001f);
  • dJoitSetHingeParam (Joint, dParamHiStop, angle);

というわけで,すっかり前置きが長くなりましたが,ODE(Open Dyamics Engine)講座27を開講します.関節の初期角度を自由に設定するには以下の手順で行います

    1. モデルの生成
    2. 初期関節角度の設定
      dJointSetHingeParam()を使い,関節角の下限と上限を初期角度の近傍に設定する.
    3. シミュレーションの更新
      これにより関節角が初期角度に移動する.
    4. 各ボディの位置と姿勢の取得
      各関節の中心位置と回転軸の取得する.
    5. モデルの破壊
      ボディとジョイントを破壊する.
    6. モデルの再生
      4.で取得したボディの位置と姿勢,関節の中心位置と回転軸などを初期値として,モデルを再生する.

以下にサンプルプログラムの主要部分を掲載します.完全なプログラムはここからダウンロードしてください.

[cpp]
// 2関節,2リンクのロボットアームの生成
// 関節,リンク用の配列は要素数3です.
// これは,0番目のリンクを土台,0番目の関節を固定関節として
// 地面と結合しているためです.
void  makeArm()
{
  dReal l0[3]  = { 0.00, 0.00, 0.05};  // link0の位置
 dReal l1[3]  = { 0.25, 0.00, 0.04};  // link1の位置
 dReal l2[3]  = { 0.75, 0.00, 0.04};  // link2の位置

 dReal anc0[3] = { 0.00, 0.00, 0.00}; // 関節0の中心位置(ダミー)
 dReal anc1[3] = { 0.00, 0.00, 0.04}; // 関節1の中心位置
 dReal anc2[3] = { 0.50, 0.00, 0.04}; // 関節2の中心位置
 dReal ax0[3]  = { 0.00, 0.00, 1.00}; // 関節0の回転軸(ダミー)
 dReal ax1[3]  = { 0.00, 0.00, 1.00}; // 関節1の回転軸
 dReal ax2[3]  = { 0.00, 0.00, 1.00}; // 関節2の回転軸

 // pos0は要素数NUMの配列.各要素がconst dRealへのポインタ
  const dReal *pos[NUM];
  const dReal *R[NUM];

 /*** 1. モデルを作る ***/
  // 初期位置
  pos[0] = l0;	pos[1] = l1;	pos[2] = l2;

 // 初期姿勢
  dMatrix3 tmpR0, tmpR1;
  dRSetIdentity(tmpR0);  // 単位行列,回転しない
 dRFromAxisAndAngle(tmpR1,0,1,0,0.5*M_PI); // y軸を中心にπ/2回転
  R[0] = tmpR0;	R[1] = tmpR1;	R[2] = tmpR1;

  // ジョイント
  const dReal *anchor[NUM];
  const dReal *axis[NUM];

 // 関節中心と回転軸の設定
  anchor[0] = anc0; anchor[1] = anc1; anchor[2] = anc2;
  axis[0]   = ax0;  axis[1]   = ax1;  axis[2]   = ax2;

  makeLinks(pos,R);          // リンクの生成
  makeJoints(anchor, axis);	 // ジョイントの生成

  /*** 2. 初期関節角度の設定 ***/
  // 関節角の上限と下限を初期角度の近傍にすることで強制的に目標角度にする
  for (int i = 1; i < NUM; i++) {
   dJointSetHingeParam(joint[i],dParamLoStop, ANGLE0[i] * M_PI/180.0 - 1e-6);
    dJointSetHingeParam(joint[i],dParamHiStop, ANGLE0[i] * M_PI/180.0);
  }

 /*** 3. シミュレーションを進める ***/
  // これがないと関節が動かない
  for (int j = 0; j < 100; j++) {
    dWorldStep(world, 0.01);
   dJointGroupEmpty(contactgroup);
  }

 /*** 4. リンクの位置と姿勢の取得 ***/
  for (int k = 0; k < NUM; k++) {
  pos[k] = dBodyGetPosition(rlink[k].body);
    R[k]   = dBodyGetRotation(rlink[k].body);
  }

  /*** 5. ジョイント中心の位置と回転軸の取得 ***/
  anchor[0] = anc0;
 axis[0]    = ax0;
  for (int k = 1; k < NUM; k++) {
   dJointGetHingeAnchor(joint[k], (dReal *) anchor[k]);
   dJointGetHingeAxis(joint[k], (dReal *) axis[k]);
  }

 // 何故かmingw環境では,printf文を以下に入れないとanchor, axisが反映されない
 // Linuxでは未確認.printf("n");だけでもOK.
  for (int k = 0; k < NUM; k++) {
     printf("nanchor[%d]=%5.2f %5.2f %5.2f n",k, anchor[k][0],anchor[k][1],anchor[k][2]);
     printf("axis[%d]  =%5.2f %5.2f %5.2f n", k, axis[k][0],axis[k][1],axis[k][2]);
 }

  /*** 6. モデルの破壊 ***/
  destroyJoints(); // ジョイントの破壊
 destroyLinks();  // リンクの破壊

  /*** 7. モデルの再生 ***/
  makeLinks(pos,R);          // リンクの再生成
 makeJoints(anchor,axis);  // ジョイントの再生成
}
[/cpp]

こよれよりもっと簡単な方法があれば,コメントを頂ければうれしい限りです.

3 Comments
  1. つい先日,このページのサンプルプログラムをダウンロードさせていただいたんですが、このページのサンプル画像のような状態に成っていませんでした。
    dRFromAxisAndAngle(tmpR1,0,1,0,0.5*M_PI) で 『y軸を中心にπ/2回転(反時計回り)』しているはずのlink1もlink2もz軸に沿ったままに成っていて、シミュレーションができませんでした。
    サンプルプログラム内のどこを修正すればよいでしょうか。ご教授お願いいたします。

  2. 失礼します.質問よろしいでしょうか.
    VC++環境で自己流にてプログラムをいじっている者です.

    勉強のために「関節初期角度の設定法」のサンプルプログラムをダウンロードさせていただいたのですが,コンパイルは通るのに実行時にリンクが表示されません.
    また.exeファイルの方から開くと,何故かジョイントだけが表示されてしまいます.

    VC++だから起こる問題なのでしょうか?解決方法の検討が今一つ思いつきません…

コメントを残す

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