Eigenで力学してみる（1）
C++向けの行列計算ライブラリEigenを使って姿勢運動を扱う話．
初回はウォーミングアップということで，Quaternionクラスの使い方の練習をする．
はじめに
僕はC++をメインで使い、行列計算ライブラリとしてEigenを使っている．C++の高速性とMatlab的な行列演算を両立するためだ．Eigenには姿勢演算に便利なQuaternion（4元数）機能があるのだけど，これの使い方を解説してる記事は少ない．Quaternion(厳密にはそのオイラーパラメータ)によるキネマティクス処理は強力なので，ポイントを解説しておく．
Quaternionの時間発展を追う
Quaternionを使うとベクトルの回転操作が容易だ．ただし，
- 座標系を固定し，ベクトルを回転させる
- ベクトルを固定し，座標系を回転させる
のどちらを計算しているのか意識することがとても重要．ここでは前者を行っている．
#include "Eigen/Dense" #include "Eigen/Geometry" #include <iostream> using namespace std; using namespace Eigen; int main(){ Quaterniond q; Vector3d vec; Matrix3d DCM; // z軸の単位ベクトル Vector3d Z = Vector3d::UnitZ(); cout << "Z = " << Z.transpose() << endl; // x軸周りに+π/4回転させるクォータニオンを生成 q = AngleAxisd(M_PI/4, Vector3d::UnitX()); cout << "q = " << q.coeffs().transpose() << endl; // ベクトルZをクォータニオンによって回転させる vec = q * Z; cout << "vec = " << vec.transpose() << endl; // クォータニオンからDCMを生成する DCM = q.toRotationMatrix(); cout << "DCM = " << endl << DCM << endl; // クォータニオンを作用させることと，DCMを作用させることは等価 vec = DCM * Z; cout << "vec = " << vec.transpose() << endl; return EXIT_SUCCESS; }
なお，ベクトルを固定して座標系を回転させる場合は，共役クォータニオンを作用させればいい．
//vec = q * Z; <-------この行を修正する vec = q.conjugate() * Z;
ただし，クォータニオンが規格化されていることが前提となる．クォータニオンの規格化は次のように行う．
q.normalize();
