ODE講座20:好きな姿勢に設定しよう!

ODE orientation

お待たせしました。約半年ぶりのODE講座です。さて、任意のベクトルの向きに沿うように円柱を表示させる方法に関してご質問があったので、試しにサンプルプログラムを作ってみました。

ここではdRFromAxisAndAngle()とdRFromZAxis()のAPIを使って実現しています。 この他にdRFromEulerAngles()のAPIもありますが、ODEのオイラー角はロボット関係者にとっては標準的ではないので省略します。 詳細については、よくある質問をご覧ください。

このプログラムは、2点a[3], b[3]を通るベクトルと同じ姿勢を取るカプセルを2個表示させます。一つはdRFromAxisAndAngle()、もう一つはより簡単なdRFromZAxis()を使った方法です。dRFromZAxisはODEのメーリングリストで紹介された方法で、dRFromAxisAndAngle()は自分で考えてみました。

  • void dRFromAxisAndAngle(dMatrix3 R, dReal rx, dReal ry, dReal rz,  dReal angle)
    回転軸(rx, ry, rz)を中心にangle [rad]回転したときの回転行列Rを取得する。ソースはode/src/rotation.cppにある。

このAPIを使ってベクトルの向きにカプセルの姿勢を設定するためには、自分で回転軸とその姿勢に必要な角度を計算する必要があります。ジオメトリのデフォルトの初期姿勢は、カプセルや円柱なら直立した状態です。これを重心を通るある軸を中心に回転させ、その姿勢が設定したいベクトルの向きと同じになれば良いわけです。サンプルプログラムではこれを実装しています。

  • void dRFromZAxis(dMatrix3 R, dReal rx, dReal ry, dReal rz)
    回転後のz軸(rx, ry, rz)から回転行列Rを取得する。ソースはode/src/rotation.cppにある。

このAPIは回転後のz軸を入力すると自動的にx軸、y軸を求め、それから回転行列Rを求めます。カプセルや円柱をベクトルの向きにするためには、rxにb[0]-a[0]、ryにb[1]-a[1]、rzにb[2]-a[2]を入れてください。カプセルや円柱の長軸方向を中心とした回転を考える必要がない場合は、こちらが簡単です。

なお、サンプルプログラムは簡単にするために衝突検出などは省いています。


演習問題

  • 以下のサンプルプログラムを参考に2点a, bを端点とする円柱を表示しよう。

// orientation.cpp: 初期姿勢の設定 by Kosei Demura (2007-10-24)<br /><br />
#include<ode/ode.h>
#include<drawstuff/drawstuff.h>
#ifdef dDOUBLE
#define dsDrawBox      dsDrawBoxD
#define dsDrawSphere   dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule  dsDrawCapsuleD
#endif

// extern void dsStartGraphics(int window_width, int window_height, struct dsFunctions *fn);
static dWorldID world;
static dSpaceID space;
static dGeomID  ground;
static dGeomID  capsule, capsule2;
static dJointGroupID contactgroup;
dsFunctions fn;

static void simLoop(int pause)
{
    const dReal *pos, *pos2;
    const dReal *R, *R2;
    dReal r, l;

    pos  = dGeomGetPosition(capsule);
    pos2 = dGeomGetPosition(capsule2);
    R    = dGeomGetRotation(capsule);
    R2   = dGeomGetRotation(capsule2);
    dGeomCapsuleGetParams(capsule, &r, &l);
    dsSetColor(1.2, 0.0, 0.0);
    dsDrawCapsule(pos, R, l, r);
    dsSetColor(0.0, 1.2, 0.0);
    dsDrawCapsule(pos2, R2, l, r);
}

void start()
{
    static float xyz[3] = {   3.0, 0.0, 1.0};
    static float hpr[3] = {-180.0, 0.0, 0.0};
    dsSetViewpoint(xyz, hpr);
    dsSetSphereQuality(3);
}

void  setDrawStuff()
{
    fn.version = DS_VERSION;
    fn.start   = &start;
    fn.step    = &simLoop;
    fn.command = NULL;
    fn.stop    = NULL;
    fn.path_to_textures = "../../drawstuff/textures";
}

// カプセルの生成
void makeCapsule()
{
    dReal r = 0.05, l = 1.0;
    dReal x  = 0, y  = -1,  z  = 1;            // 重心の座標
    dReal x2 = 0, y2 =  0,  z2 = 1;          // 重心の座標
    dReal angle;                                  // 回転角
    dReal a[3] = {0, 0, 0};                    // ベクトルの始点
    dReal b[3] = {1, 1, 1};                    // ベクトルの終点

    capsule  =  dCreateCapsule(space, r, l);    // 直方体ジオメトリの生成
    capsule2 =  dCreateCapsule(space, r, l);    // 直方体ジオメトリの生成

    dMatrix3 R, R2;
    if (b[1] > 0.0) angle = atan2(-sqrt(b[0]*b[0]+b[1]*b[1]), b[2]);
    else            angle = atan2( sqrt(b[0]*b[0]+b[1]*b[1]), b[2]);

    if (b[1] != 0.0)  dRFromAxisAndAngle(R, 1, - b[0]/b[1], 0, angle);
    else             dRFromAxisAndAngle(R, 0, 1, 0, angle);

    dRFromZAxis(R2, b[0]-a[0], b[1]-a[1], b[2]-a[2]);
    dGeomSetPosition(capsule,  x , y , z );       // 位置の設定
    dGeomSetPosition(capsule2, x2, y2, z2);       // 位置の設定
    dGeomSetRotation(capsule,  R);               // 姿勢の設定
    dGeomSetRotation(capsule2, R2);               // 姿勢の設定
}

int main(int argc, char *argv[])
{
    setDrawStuff();
    world = dWorldCreate();
    space = dHashSpaceCreate(0);
    contactgroup = dJointGroupCreate(0);
    makeCapsule();
    dsSimulationLoop(argc,argv,640,480,&fn);
    dWorldDestroy(world);
    return 0;
}

コメントを残す

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