Day: November 17, 2016

それらしく見えるでしょうか？19行のIbodyと26行のLの値に注意して下さい。

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;

Matrix3d    Ibody;
Matrix3d    Ibodyinv;
Quaterniond q;
Quaterniond qdot;
Vector3d    L;
Matrix3d    Iinv;
Matrix3d    R;
Vector3d    omega;

int main(int argc, const char * argv[]) {

    Ibody << 1.0,0.0,0.0, 0.0,2.0,0.0, 0.0,0.0,4.0;
    Ibodyinv = Ibody.inverse();
    cout << "Ibody: "    << endl << Ibody    << endl;
    cout << "Ibodyinv: " << endl << Ibodyinv << endl;

    Quaterniond q(AngleAxisd(M_PI*0.0, Vector3d::UnitZ()));
    q.normalize();
    L << 0.00, 0.00, 1.00;
    
    
    double dt = 0.1;
    
    for(double t=0; t<200.0; t+= dt) {
        R     = q.toRotationMatrix();
        Iinv  = R * Ibodyinv * R.transpose();
        omega = Iinv * L;
        
        Quaterniond qq(0.0, omega.x()/2.0, omega.y()/2.0, omega.z()/2.0);
        qdot =  qq * q;
        
        cout << "t    : " << t << endl;
        cout << "q    : " << q.w() << " " << q.vec().transpose() << endl;
        cout << "L    : " << L.transpose() << endl;
        cout << "R    : " << endl << R << endl;
        cout << "Iinv : " << endl << Iinv << endl;
        cout << "omega: " << omega.transpose() << endl;
        cout << "qdot : " << qdot.w() << " " << qdot.vec().transpose() << endl;

        q.w()   += qdot.w()   * dt;
        q.vec() += qdot.vec() * dt;
        q.normalize();
    }
    
    return 0;
}

% ./MyEigenTest1 | grep "q    :" | colrm 1 6 | cat -n > t
% ./MyEigenTest1 | grep "omega:" | colrm 1 6 | cat -n > u
% gnuplot
gnuplot> plot "t" using 1:2, "t" using 1:5, "u" using 1:2, "u" using 1:3, "u" using 1:4

今度は、IbodyとLの両方を少し変えました。

    Ibody << 1.0,0.0,0.0, 0.0,4.0,0.0, 0.0,0.0,2.0;
    L << 0.01, 0.00, 1.00;

解は安定なようです。