void myDifferentialEquation( double *x, double *f, void *user_data )
{
    for (i = 0 ; i<6 ; i++) {
        v[i] = x[i+6];
        a[i] = 0.;
        ei[i] = 0.;
    }

//    for (i=0 ; i<16 ; i++) {
//        std::cout << " x   : " << x[i] << std::endl;
//    }

    for (i=0 ; i<3 ; i++) {
        q[i] = x[i];
    }


    //RPY to quaternion
    r = x[3]; p = x[4]; y = x[5];
    Mrot(0,0) = cos(y)*cos(p);
    Mrot(0,1) = cos(y)*sin(p)*sin(r) - sin(y)*cos(r);
    Mrot(0,2) = cos(y)*sin(p)*cos(r) + sin(y)*sin(r);
    Mrot(1,0) = sin(y)*cos(p);
    Mrot(1,1) = sin(y)*sin(p)*sin(r) + cos(y)*cos(r);
    Mrot(1,2) = sin(y)*sin(p)*cos(r) - cos(y)*sin(r);
    Mrot(2,0) = -sin(p);
    Mrot(2,1) = cos(p)*sin(r);
    Mrot(2,2) = cos(p)*cos(r);

    Eigen::Quaternion<double> quat(Mrot);
    q[6] = quat.w(); q[3] = quat.x(); q[4] = quat.y(); q[5] = quat.z();

//    std::cout << " q   : \n" << q << std::endl;

    MomentRPY(0) = x[14];//100;
    MomentRPY(1) = x[15];//100;
    MomentRPY(2) = x[16];//100;

   // MomentRPY = Mrot*MomentRPY;

    u[0] = 0;//- (x[13])*sin(p);
    u[1] = 0;//(x[13])*sin(r)*cos(p);
    u[2] = x[13];//(x[13])*cos(r)*cos(p);
    u[3] = MomentRPY(0);
    u[4] = MomentRPY(1);
    u[5] = MomentRPY(2);


    b0 = rnea(robot, data, q, a, a);

//    std::cout << "b0 : \n" << b0 << std::endl;

    for (i=0 ; i<6 ; i++) {
        ei(i)=1;
        Mi = rnea(robot, data, q, a, ei);
        for (j=0 ; j<6 ; j++ ) {
            M(i,j) = Mi(j)-b0(j);
        }
        ei(i)=0;
    }

//    std::cout << " M rnea :\n" <<  M << std::endl << std::endl;
//    std::cout << "crba :\n" << crba(robot, data, q) << std::endl << std::endl;
//    self adjoint upper ???

    b = rnea(robot, data, q, v, a);

    a = M.llt().solve(u-b);
//    std::cout << "F tot moteurs : \n" << x[12]+x[13]+x[14]+x[15] << std::endl << std::endl;
//    std::cout << "Forces : \n" << u << std::endl << std::endl;
//    std::cout << "Acceleration : \n" << a << std::endl << std::endl;

    // --------------------   COMPUTE OUTPUT -----------------------  //
    // vitesses lineaires Freeflyer
    for (i=0 ; i<3 ; i++ ) {
        f[i] = v[i];
    }

    // vitesses angulaires Freeflyer (transformation RPY)
    f[3] = v[3] + sin(p)*v[5];
    f[4] = cos(r)*v[4] - sin(r)*cos(p)*v[5];
    f[5] = sin(r)*v[4] + cos(r)*cos(p)*v[5];

    // accelerations lineaires
    for (i=0 ; i<3 ; i++) {
        al[i] = a[i];
        aa[i] = a[i+3];
    }

    al2 = Mrot*al;
    for (i=0 ; i<3 ;i++) {
        f[i+6] = al2[i];
    }

    //accelerations angulaires
    for (i=0 ; i<3 ;i++) {
        f[i+9] = aa[i];
    }


    // cost
    f[12]=((x[0]-20)*(x[0]-20)+x[2]*x[2]+x[1]*x[1])*0.001;

}
Exemplo n.º 2
0
 static Eigen::VectorXd rnea_proxy( const ModelHandler& model, 
                                   DataHandler & data,
                                   const VectorXd_fx & q,
                                   const VectorXd_fx & v,
                                   const VectorXd_fx & a )
 { return rnea(*model,*data,q,v,a); }