9. 位置と姿勢

ODE座標系

ODE座標系 (右手系)

ODE (Open Dynamics Engine) 初級講座の第9回目です。

今回はODEの単位系並びに座標系のお話をし、剛体の位置・姿勢を設定及び取得する方法を説明します。

単位系

ODEの単位系は基本的に何でもかまいません。ただし、角度だけはradianになっています。本講座では世界標準であるSI単位系を用います。 これは物理などで御なじみの長さはm、質量はkg、時間はsです。

座標系

ODEの座標系は上図に示すように物理や数学で一般的に使われている右手系の直交座標です。原点は9個ある小さいピラミッドの中心で、中心から赤いピラミッド方向がx軸、青いピラミッド方向がy軸、上空方向がz軸となっています。なお、単位系は何でもよいのですが、この講座ではSI単位系を採用しているので、各ピラミッドは1mずつの間隔で並んでいます。

位置と姿勢の設定

剛体を3次元空間上に設定するためには、位置と姿勢を指定する必要があります。姿勢は回転変換行列で決めることができ、ODEでも以下のAPIにより設定可能です。

  • void dBodySetPosition(dBodyID, dReal x, dReal y, dReal z);
    剛体dBodyIDの重心を絶対座標系の位置(x, y, z)に設定する。
  • void dRFromAxisAndAngle(dMatrix3 R, dReal ax, dReal ay, dReal az, dReal angle);
    回転軸ベクトル(ax, ay, az)の回りを反時計方向にangle[rad]回転したときの回転変換行列Rを取得する。
  • void dBodySetRotation(dBodyID, const dMatrix3 R);
    剛体dBodyIDの姿勢を回転行列Rに設定する。

また、ODEではこのほかにクオータニオンに関するAPI、void dBodySetQuaternion (dBodyID, const dQuaternion q)もあります。

位置と姿勢の取得

  • const dReal * dBodyGetPosition(dBodyID);
    剛体dBodyIDの位置を取得する。戻り値は絶対座標系での位置が格納されている配列へのポインタ。
  • const dReal * dBodyGetRotation(dBodyID);
    剛体dBodyIDの回転行列取得する。戻り値は回転行列が格納されている配列へのポインタ。

サンプルコード
次のサンプルコードは円柱を位置(0.0, 0.0, 1.0)、x軸のまわりに45度(M_PI/4.0)回転した姿勢に設定しています。さらに、物体の重心位置を表示する関数printPos()も追加しました。なお、円柱は姿勢を設定しないと長軸がz軸方向と一致した直立の姿勢が初期値となっています。

typedef struct {
  dBodyID body;
  dGeomID geom;
} MyObject;

MyObject pillar;

void createPillar()
{
  dMass m1;
  dReal radius = 0.1, length = 0.5, mass = 1.0; // 半径、長さ、質量

  pillar.body = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetCylinderTotal(&m1,mass,3,radius,length);
  dBodySetMass(pillar.body,&m1);
  dBodySetPosition(pillar.body,0.0,0.0,1.0); // (0.0,0.0,1.0)[m]に重心を設定

  dMatrix3 R;
  dRFromAxisAndAngle(R,1.0,0.0,0.0, M_PI/4.0); //x軸周りにπ/4[rad]回転
  dBodySetRotation(pillar.body,R);
  pillar.geom = dCreateCylinder(space,radius,length);
  dGeomSetBody(pillar.geom,pillar.body);
}

void printPos(dBodyID obj_body) // 重心位置の表示
{
  const dReal *pos;  // constを忘れるとエラーになる
  dReal x, y, z;

  pos = dBodyGetPosition(obj_body); // 戻り値は配列へのconst ポインタ
  x = pos[0]; // 配列posにはx, y, zの順番で座標値が格納されている
  y = pos[1];
  z = pos[2];
  printf("x=%f y=%f z=%f \n", x, y, z);
}

なお、このプログラムを次からダウンロードできます。 また次回!

私は、コンソールに表示するprintPos()を使用することはできません!

  • 返信ありがとうございます。
    dsDrawSphere()の事ですが、それって107行目のdsDrawSphere(dGeomGetPosition(gID),dGeomGetRotation(gID),dGeomSphereGetRadius(gID));のことですよね。
    case内にもうひとつ入力してみたのですがやはりボールが増えません。
    追加ボールの関数はサンプル内にあったボールの関数の直後に入力しましたが、入れ方が違うんでしょうか?

  • こんにちは。
    このページからダウンロードしたサンプルプログラムを元に表示される球体を増やそうとしていますが、ボールが増えず、追加したボールしか表示されません。どうなっているんでしょうか?

    • カノッサさん,

      ご迷惑をおかけしてすみませんが,この情報量だけでは私にもどうなっているかわかりません.

      でむ

      • 返信ありがとうございます。
        サンプルプログラムにボールをさらに表示させるプログラムを追加したのですが、「識別子が見つかりません」というエラーが発生するのです。そして、そこを修正すると追加した方のボールは表示されるのですが、もともとサンプル内にあったボールが表示されなくなってしまうのです。
        個人的にはボールを二つ表示させるプログラムを作りたいのですが・・・

        • カノッサさん、

          大変遅くなりすみません。

          外しているかもしれませんが、ボールを2個表示させるためには、dsDrawSphere()を2回呼び出さなければいけません。

          されていますか?

          でむ

  • コメントを残す

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