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

2関節ロボットアーム．１番目の関節初期角度を15度，２番目の関節初期角度を30度に設定．

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

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

それからまた，がきーさんが「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)講座２７を開講します．関節の初期角度を自由に設定するには以下の手順で行います．

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

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

[cpp]
// ２関節，２リンクのロボットアームの生成
// 関節，リンク用の配列は要素数３です．
// これは，０番目のリンクを土台，０番目の関節を固定関節として
// 地面と結合しているためです．
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); // ｙ軸を中心にπ/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");だけでもＯＫ．
  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]

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