// [[Rcpp::depends(RcppEigen)]]
Eigen::MatrixXd random_multivariate_normal(const Eigen::MatrixXd mu, const Eigen::MatrixXd Sigma)
{
int P = mu.rows(), i = 0;
Eigen::MatrixXd y(Eigen::MatrixXd(P, 1).setZero());
Eigen::MatrixXd z(Eigen::MatrixXd(P, 1).setZero());
for( i = 0 ; i < P ; i++ )
z(i, 0) = Rf_rnorm( 0, 1 );
y = mu + Sigma.llt().matrixL() * z;
return y;
}
Eigen::VectorXd multivariateGaussian(const Eigen::MatrixXd &covariance, Eigen::VectorXd *mean){
int size = covariance.rows();
Eigen::VectorXd sample(size);
for (int i=0; i<size; i++)
{
sample(i) = gaussian();
}
Eigen::MatrixXd mL = covariance.llt().matrixL(); // Cholesky decomposition
if (mean != NULL)
return mL*sample + *mean;
else
return mL*sample;
}
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;
}
void GaussianCovProposal::update(uint id, const Eigen::MatrixXd &cov)
{
sigL_[id] = cov.llt().matrixL();
}
Cholesky_LLT (const Eigen::MatrixXd &A){
U= A.llt().matrixU();
}
