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;
}
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); }