ODE講座25: 回転行列とRoll-Pitch-Yaw角

らりほーさんから回転行列のご質問があり,その回答も兼ねて久しぶりにODE (Open Dynamics Engine)講座を開講します.

回転行列は,基準の姿勢から現在の姿勢を行列で表したものです.回転行列は3行3列の行列ですが,ODEでは計算を高速化させるために,回転行列の各成分を要素数12個の1次元配列に格納しています.

回転行列のi行j列成分をrijとすると,回転行列の配列は以下で表すことができます(ODE本P103参照).4番目、8番目、12番目の要素は数字の0(ゼロ)です。

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角度回転,最後にz軸のまわりに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);

これを使ったサンプルプログラムの主要部分を以下に示します.完全なプログラムのダウンロードはこちらから.

今回はこの辺で…

[cpp]

// 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軸,y軸,z軸の順番で回転
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;
}

[/cpp]
ode
スポンサーリンク
シェアする
demura.netをフォローする

コメント

タイトルとURLをコピーしました