demura.net:RoboCup・ODE・夢考房

らりほーさんから回転行列のご質問があり，その回答も兼ねて久しぶりにODE (Open Dynamics Engine)講座を開講します． 回転行列は，基準の姿勢から現在の姿勢を行列で表したものです．回転行列は3行3列の行列ですが，ODEでは計算を高速化させるために，回転行列の各成分を要素数12個の１次元配列に格納しています． 回転行列のi行j列成分をrijとすると，回転行列の配列は以下で表すことができます(ODE本P103参照）．４番目、８番目、１２番目の要素は数字の０（ゼロ）です。 dReal R[12] = {r11, r12, r13, 0, r21, r22, r23, 0, r31, r32, r33,0}; なお、ODEの回転行列はdMatrix3型で、src/ode-バージョン番号/include/ode/common.hで次のように定義されています。

• typedef dReal dMatrix3[4*3] ;

つまり、要素数12個のdReal(ODEの実数型）型配列です。 これがわかるとロール(roll)・ピッチ(pitch)・ヨー(yaw)角をODEの回転行列から求めることができます．ロール・ピッチ・ヨー角の回転行列は，絶対座標系のx軸まわりにroll角度回転，次にy軸のまわりにpitch角度回転，最後にｚ軸のまわりにyaw角度回転させたものとなります．ここでは導出しませんが，次式で求めることができます（ODE本P149参照) roll = atan2(r32, r33); pitch = atan2(-r31, sqrt(r32*r32+r33*r33))　または atan2(-r31, -sqrt(r32*r32+r33*r33)); yaw = atan2(r21, r11); これを使ったサンプルプログラムの主要部分を以下に示します．完全なプログラムのダウンロードはこちらから． 今回はこの辺で...

PLAIN TEXT
C++:

// roll, pitch, yawのサンプルプログラム
// rpy.cpp by Kosei Demura (2008-5-12)
 
dWorldID world;  // 動力学計算用ワールド
dReal START_X = 0, START_Y = 0, START_Z=1.20; // 初期位置
dReal weight    = 9.4;                        // ボックスの質量
dReal size[3]   = {0.5,0.1,0.3};              // ボックスのサイズ
 
typedef struct {            // MyObject構造体
    dBodyID body;       // ボディ(剛体)のID番号（動力学計算用）
    dReal  lx, ly, lz;  // 長さ[m]
} MyObject;
 
MyObject box;  // MyObject構造体
 
// ロール・ピッチ・ヨー角の計算 ODE本P150
static void rollPitchYaw()
{
    const dReal *rot = dBodyGetRotation(box.body);
    dReal r11,r12,r13,r21,r22,r23,r31,r32,r33;
    dReal pitch, yaw, roll;
 
    r11 = rot[0];    r12 = rot[1];    r13 = rot[2];
    r21 = rot[4];    r22 = rot[5];    r23 = rot[6];
    r31 = rot[8];    r32 = rot[9];    r33 = rot[10];
    // ポインタ的表記では　r11 = *(rot+0); r12 = *(rot+1); r13 = *(rot+2);
 
    pitch = atan2(-r31, sqrt(r32*r32+r33*r33));
    yaw   = atan2(r21,r11);
    roll  = atan2(r32,r33);
 
    printf("roll=%f pitch=%f yaw=%f  r", 180.0*roll/M_PI, 180.0*pitch/M_PI, 180.0*yaw/M_PI);
}
 
// 絶対座標系x軸,ｙ軸,ｚ軸の順番で回転
static void rotation()
{
    static dReal angle_x = 45, angle_y = 0, angle_z = 90;
    const dReal *R0;
    dMatrix3 R1, R2, R3, R10, R20, R30;
 
    R0 = dBodyGetRotation(box.body);
 
    dRFromAxisAndAngle(R1, 1, 0, 0, M_PI*angle_x/180.0); // x軸まわり
    dRFromAxisAndAngle(R2, 0, 1, 0, M_PI*angle_y/180.0); // y軸まわり
    dRFromAxisAndAngle(R3, 0, 0, 1, M_PI*angle_z/180.0); // z軸まわり
 
    dMultiply0(R10, R1,  R0, 3, 3, 3);
    dMultiply0(R20, R2, R10, 3, 3, 3);
    dMultiply0(R30, R3, R20, 3, 3, 3);
 
    dBodySetRotation(box.body, R30);
}
 
static void simLoop(int pause)
{
    rollPitchYaw();
    dWorldStep(world,0.01);
    dsSetColor(1.0,0.0,0.0);
    dsDrawBox(dBodyGetPosition(box.body),
    dBodyGetRotation(box.body),size);
}
 
// ボックスの生成
void makeBox()
{
    dMass mass;
 
    box.body  = dBodyCreate(world);
    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass, weight, size[0], size[1], size[2]);
    dBodySetMass(box.body, &mass);
    dBodySetPosition(box.body,START_X,START_Y,START_Z);
}
 
int main(int argc, char **argv)
{
    setDrawStuff();
    world = dWorldCreate();                  // 動力学計算ワールドの作成
    makeBox();                               // ボックスの作成
    rotation();                              // 回転
    dsSimulationLoop(argc,argv,640,480,&fn); // シミュレーションループ
    dWorldDestroy(world);                    // ワールドの破壊
    return 0;
}