Commit 817b57f7 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add computeRNEADerivatives algo

parent 8a0a281e
......@@ -40,6 +40,30 @@ namespace se3
computeGeneralizedGravityDerivatives(const Model & model, Data & data,
const Eigen::VectorXd & q,
Eigen::MatrixXd & gravity_partial_dq);
///
/// \brief Computes the derivatives of the Recursive Newton Euler Algorithms
/// with respect to the joint configuration and the joint velocity.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint velocity vector (dim model.nv).
/// \param[in] a The joint acceleration vector (dim model.nv).
/// \param[out] rnea_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration.
/// \param[out] rnea_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity.
///
/// \remark rnea_partial_dq and rnea_partial_dv must be first initialized with zeros (gravity_partial_dq.setZero).
///
/// \sa se3::rnea
///
inline void
computeRNEADerivatives(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
Eigen::MatrixXd & rnea_partial_dq,
Eigen::MatrixXd & rnea_partial_dv);
} // namespace se3
......
......@@ -58,26 +58,25 @@ namespace se3
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock J_cols = jmodel.jointCols(data.J);
ColsBlock axS_cols = jmodel.jointCols(data.axS);
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
J_cols = data.oMi[i].act(jdata.S());
motionSet::motionAction(oa,J_cols,axS_cols);
motionSet::motionAction(oa,J_cols,dAdq_cols);
}
};
struct computeGeneralizedGravityDerivativeBackwardStep : public fusion::JointVisitor<computeGeneralizedGravityDerivativeBackwardStep>
struct computeGeneralizedGravityDerivativeBackwardStep : public fusion::JointModelVisitor<computeGeneralizedGravityDerivativeBackwardStep>
{
typedef boost::fusion::vector<const Model &,
Data &,
Eigen::MatrixXd &
> ArgsType;
JOINT_VISITOR_INIT(computeGeneralizedGravityDerivativeBackwardStep);
JOINT_MODEL_VISITOR_INIT(computeGeneralizedGravityDerivativeBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointDataDerived> & jdata,
const Model & model,
Data & data,
Eigen::MatrixXd & gravity_partial_dq)
......@@ -90,10 +89,10 @@ namespace se3
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock J_cols = jmodel.jointCols(data.J);
ColsBlock axS_cols = jmodel.jointCols(data.axS);
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
motionSet::inertiaAction(data.oYo[i],axS_cols,dFdq_cols);
motionSet::inertiaAction(data.oYo[i],dAdq_cols,dFdq_cols);
gravity_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
......@@ -103,7 +102,7 @@ namespace se3
lhsInertiaMult(data.oYo[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
gravity_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.axS.col(j);
gravity_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j);
jmodel.jointVelocitySelector(data.g).noalias() = J_cols.transpose()*data.f[i].toVector();
if(parent>0)
......@@ -142,11 +141,223 @@ namespace se3
for(size_t i=(size_t) (model.njoints-1);i>0;--i)
{
computeGeneralizedGravityDerivativeBackwardStep::run(model.joints[i],data.joints[i],
computeGeneralizedGravityDerivativeBackwardStep::run(model.joints[i],
computeGeneralizedGravityDerivativeBackwardStep::ArgsType(model,data,gravity_partial_dq));
}
}
struct computeRNEADerivativesForwardStep : public fusion::JointVisitor<computeRNEADerivativesForwardStep>
{
typedef boost::fusion::vector< const se3::Model &,
se3::Data &,
const Eigen::VectorXd &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(computeRNEADerivativesForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::Model & model,
se3::Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
const Model::JointIndex & i = jmodel.id();
const Model::JointIndex & parent = model.parents[i];
Motion & ov = data.ov[i];
Motion & oa = data.oa[i];
jmodel.calc(jdata.derived(),q,v);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.v[i] = jdata.v();
if(parent > 0)
{
data.oMi[i] = data.oMi[parent] * data.liMi[i];
data.v[i] += data.liMi[i].actInv(data.v[parent]);
}
else
data.oMi[i] = data.liMi[i];
data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
if(parent > 0)
{
data.a[i] += data.liMi[i].actInv(data.a[parent]);
}
data.oYo[i] = data.oMi[i].act(model.inertias[i]);
ov = data.oMi[i].act(data.v[i]);
oa = data.oMi[i].act(data.a[i]) + data.oa[0]; // add gravity contribution
data.h[i] = data.oYo[i] * ov;
data.f[i] = data.oYo[i] * oa + ov.cross(data.h[i]);
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock J_cols = jmodel.jointCols(data.J);
ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
J_cols = data.oMi[i].act(jdata.S());
motionSet::motionAction(ov,J_cols,dJ_cols);
motionSet::motionAction(data.oa[parent],J_cols,dAdq_cols);
dAdv_cols = dJ_cols;
if(parent > 0)
{
motionSet::motionAction(data.ov[parent],J_cols,dVdq_cols);
motionSet::motionAction<ADDTO>(data.ov[parent],dVdq_cols,dAdq_cols);
dAdv_cols += dVdq_cols;
}
else
dVdq_cols.setZero();
// computes variation of inertias
data.doYo[i] = data.oYo[i].variation(ov);
addForceCrossMatrix(data.h[i],data.doYo[i]);
}
template<typename ForceDerived, typename M6>
static void addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout)
{
M6 & mout_ = const_cast<Eigen::MatrixBase<M6> &>(mout).derived();
typedef Eigen::Matrix<typename M6::Scalar,3,3,EIGEN_PLAIN_TYPE(M6)::Options> M3;
const M3 fx(skew(f.linear()));
mout_.template block<3,3>(Force::LINEAR,Force::ANGULAR) -= fx;
mout_.template block<3,3>(Force::ANGULAR,Force::LINEAR) -= fx;
mout_.template block<3,3>(Force::ANGULAR,Force::ANGULAR) -= skew(f.angular());
}
};
struct computeRNEADerivativesBackwardStep : public fusion::JointModelVisitor<computeRNEADerivativesBackwardStep>
{
typedef boost::fusion::vector<const Model &,
Data &,
Eigen::MatrixXd &,
Eigen::MatrixXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(computeRNEADerivativesBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
const Model & model,
Data & data,
Eigen::MatrixXd & rnea_partial_dq,
Eigen::MatrixXd & rnea_partial_dv)
{
const Model::JointIndex & i = jmodel.id();
const Model::JointIndex & parent = model.parents[i];
Data::Matrix6R & M6tmpR = data.M6tmpR;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock J_cols = jmodel.jointCols(data.J);
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv);
ColsBlock dFda_cols = jmodel.jointCols(data.dFda);
// tau
jmodel.jointVelocitySelector(data.tau).noalias() = J_cols.transpose()*data.f[i].toVector();
// dtau/da similar to data.M
motionSet::inertiaAction(data.oYo[i],J_cols,dFda_cols);
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFda.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
// dtau/dv
motionSet::inertiaAction(data.oYo[i],dAdv_cols,dFdv_cols);
dFdv_cols += data.doYo[i] * J_cols;
rnea_partial_dv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
// dtau/dq
motionSet::inertiaAction(data.oYo[i],dAdq_cols,dFdq_cols);
if(parent>0)
dFdq_cols += data.doYo[i] * dVdq_cols;
rnea_partial_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
motionSet::act<ADDTO>(J_cols,data.f[i],dFdq_cols);
if(parent > 0)
{
lhsInertiaMult(data.oYo[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j);
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdv.col(j);
M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYo[i];
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.dVdq.col(j);
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.J.col(j);
}
if(parent>0)
{
data.oYo[parent] += data.oYo[i];
data.doYo[parent] += data.doYo[i];
data.f[parent] += data.f[i];
}
}
template<typename Min, typename Mout>
static void lhsInertiaMult(const Inertia & Y,
const Eigen::MatrixBase<Min> & J,
const Eigen::MatrixBase<Mout> & F)
{
Mout & F_ = const_cast<Mout &>(F.derived());
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose());
}
};
inline void
computeRNEADerivatives(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
Eigen::MatrixXd & rnea_partial_dq,
Eigen::MatrixXd & rnea_partial_dv)
{
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(a.size() == model.nv && "The joint acceleration vector is not of right size");
assert(rnea_partial_dq.cols() == model.nv);
assert(rnea_partial_dq.rows() == model.nv);
assert(rnea_partial_dv.cols() == model.nv);
assert(rnea_partial_dv.rows() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
data.oa[0] = -model.gravity;
for(Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i)
{
computeRNEADerivativesForwardStep::run(model.joints[i],data.joints[i],
computeRNEADerivativesForwardStep::ArgsType(model,data,q,v,a));
}
for(size_t i=(size_t) (model.njoints-1);i>0;--i)
{
computeRNEADerivativesBackwardStep::run(model.joints[i],
computeRNEADerivativesBackwardStep::ArgsType(model,data,rnea_partial_dq,rnea_partial_dv));
}
}
} // namespace se3
......
......@@ -383,6 +383,9 @@ namespace se3
/// all external forces acting on the body and expressed in the local frame of the joint..
container::aligned_vector<Force> f;
/// \brief Vector of spatial momenta.
container::aligned_vector<Force> h;
/// \brief Vector of absolute joint placements (wrt the world).
container::aligned_vector<SE3> oMi;
......@@ -418,12 +421,15 @@ namespace se3
/// \brief The Coriolis matrix (a square matrix of dim model.nv).
Eigen::MatrixXd C;
/// \brief Variation of the forceset with respect to q.
/// \brief Variation of the forceset with respect to the joint configuration.
Matrix6x dFdq;
/// \brief Variation of the forceset with respect to qdot.
/// \brief Variation of the forceset with respect to the joint velocity.
Matrix6x dFdv;
/// \brief Variation of the forceset with respect to the joint acceleration.
Matrix6x dFda;
/// \brief Right variation of the inertia matrix
container::aligned_vector<Inertia::Matrix6> vxI;
......@@ -501,8 +507,14 @@ namespace se3
/// \brief Derivative of the Jacobian with respect to the time.
Matrix6x dJ;
/// \brief Second time derivative of the Jacobian.
Matrix6x axS;
/// \brief Variation of the spatial velocity set with respect to the joint configuration.
Matrix6x dVdq;
/// \brief Variation of the spatial acceleration set with respect to the joint configuration.
Matrix6x dAdq;
/// \brief Variation of the spatial acceleration set with respect to the joint velocity.
Matrix6x dAdv;
/// \brief Vector of joint placements wrt to algorithm end effector.
container::aligned_vector<SE3> iMf;
......
......@@ -240,6 +240,7 @@ namespace se3
,v((std::size_t)model.njoints)
,ov((std::size_t)model.njoints)
,f((std::size_t)model.njoints)
,h((std::size_t)model.njoints)
,oMi((std::size_t)model.njoints)
,liMi((std::size_t)model.njoints)
,tau(model.nv)
......@@ -252,6 +253,7 @@ namespace se3
,C(model.nv,model.nv)
,dFdq(6,model.nv)
,dFdv(6,model.nv)
,dFda(6,model.nv)
,vxI((std::size_t)model.njoints)
,Ivx((std::size_t)model.njoints)
,oYo((std::size_t)model.njoints)
......@@ -271,7 +273,9 @@ namespace se3
,nvSubtree_fromRow((std::size_t)model.nv)
,J(6,model.nv)
,dJ(6,model.nv)
,axS(6,model.nv)
,dVdq(6,model.nv)
,dAdq(6,model.nv)
,dAdv(6,model.nv)
,iMf((std::size_t)model.njoints)
,com((std::size_t)model.njoints)
,vcom((std::size_t)model.njoints)
......@@ -312,6 +316,7 @@ namespace se3
ov[0].setZero();
a_gf[0] = -model.gravity;
f[0].setZero();
h[0].setZero();
oMi[0].setIdentity();
liMi[0].setIdentity();
oMf[0].setIdentity();
......
......@@ -22,6 +22,7 @@
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include <iostream>
......@@ -71,267 +72,181 @@ BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives)
}
BOOST_CHECK(g_partial_dq.isApprox(g_partial_dq_fd,sqrt(alpha)));
// std::cout << "dg/dq ref:\n" << g_partial_dq_fd.block<10,10>(0,0) << std::endl << std::endl;
// std::cout << "dg/dq:\n" << g_partial_dq.block<10,10>(0,0) << std::endl;
// std::cout << "symmetric: " << (g_partial_dq_fd.bottomRightCorner(model.nv-6,model.nv-6) - g_partial_dq_fd.bottomRightCorner(model.nv-6,model.nv-6).transpose()).norm() << std::endl;
}
BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
{
using namespace Eigen;
using namespace se3;
// std::cout << "dg/dq[:6,:] ref:\n" << g_partial_dq_fd.rightCols<6>() << std::endl;
// std::cout << "dg/dq[,6:].T ref:\n" << g_partial_dq_fd.topRows<6>().transpose() << std::endl;
Model model;
buildModels::humanoidSimple(model);
Data data(model), data_fd(model), data_ref(model);
}
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill(1.);
VectorXd q = randomConfiguration(model);
VectorXd v(VectorXd::Random(model.nv));
VectorXd a(VectorXd::Random(model.nv));
/// Check againt computeGeneralizedGravityDerivatives
MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
computeRNEADerivatives(model,data,q,0*v,0*a,rnea_partial_dq,rnea_partial_dv);
MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
computeGeneralizedGravityDerivatives(model,data_ref,q,g_partial_dq);
BOOST_CHECK(rnea_partial_dq.isApprox(g_partial_dq));
BOOST_CHECK(data.tau.isApprox(data_ref.g));
VectorXd tau0 = rnea(model,data_fd,q,0*v,0*a);
MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();
VectorXd v_eps(VectorXd::Zero(model.nv));
VectorXd q_plus(model.nq);
VectorXd tau_plus(model.nv);
const double alpha = 1e-8;
for(int k = 0; k < model.nv; ++k)
{
v_eps[k] += alpha;
q_plus = integrate(model,q,v_eps);
tau_plus = rnea(model,data_fd,q_plus,0*v,0*a);
rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
v_eps[k] -= alpha;
}
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
//BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity)
//{
// using namespace Eigen;
// using namespace se3;
//
// Model model;
// buildModels::humanoidSimple(model);
//
// Data data(model), data_ref(model);
//
// model.lowerPositionLimit.head<3>().fill(-1.);
// model.upperPositionLimit.head<3>().fill(1.);
// VectorXd q = randomConfiguration(model);
// VectorXd v(VectorXd::Random(model.nv));
// VectorXd a(VectorXd::Random(model.nv));
//
// computeForwardKinematicsDerivatives(model,data,q,v,a);
//
// const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
// Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero();
// Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero();
// Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero();
// Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero();
//
// getJointVelocityDerivatives<WORLD>(model,data,jointId,
// partial_dq,partial_dv);
//
// getJointVelocityDerivatives<LOCAL>(model,data,jointId,
// partial_dq_local,partial_dv_local);
//
// Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
// Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
// computeJacobians(model,data_ref,q);
// getJacobian<WORLD>(model,data_ref,jointId,J_ref);
// getJacobian<LOCAL>(model,data_ref,jointId,J_ref_local);
//
// BOOST_CHECK(partial_dv.isApprox(J_ref));
// BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));
//
// // Check against finite differences
// Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero();
// Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero();
// Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero();
// Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero();
// const double alpha = 1e-6;
//
// // dvel/dv
// Eigen::VectorXd v_plus(v);
// Data data_plus(model);
// forwardKinematics(model,data_ref,q,v);
// Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId]));
// Motion v0_local(data_ref.v[jointId]);
// for(int k = 0; k < model.nv; ++k)
// {
// v_plus[k] += alpha;
// forwardKinematics(model,data_plus,q,v_plus);
//
// partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
// partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
// v_plus[k] -= alpha;
// }
//
// BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha)));
// BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha)));
//
//
// // dvel/dq
// Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
// forwardKinematics(model,data_ref,q,v);
// v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]);
//
// for(int k = 0; k < model.nv; ++k)
// {
// v_eps[k] += alpha;
// q_plus = integrate(model,q,v_eps);
// forwardKinematics(model,data_plus,q_plus,v);
//
// partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
// partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
// v_eps[k] -= alpha;
// }
//
// BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));
//
// BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));
//
//// computeJacobiansTimeVariation(model,data_ref,q,v);
//// Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.setZero();
//// getJacobianTimeVariation<WORLD>(model,data_ref,jointId,dJ_ref);
//// BOOST_CHECK(partial_dq.isApprox(dJ_ref));
////
//// std::cout << "partial_dq\n" << partial_dq << std::endl;
//// std::cout << "dJ_ref\n" << dJ_ref << std::endl;
//
//}
//
//BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
//{
// using namespace Eigen;
// using namespace se3;
//
// Model model;
// buildModels::humanoidSimple(model);
//
// Data data(model), data_ref(model);
//
// model.lowerPositionLimit.head<3>().fill(-1.);
// model.upperPositionLimit.head<3>().fill(1.);
// VectorXd q = randomConfiguration(model);
// VectorXd v(VectorXd::Random(model.nv));
// VectorXd a(VectorXd::Random(model.nv));
//
// computeForwardKinematicsDerivatives(model,data,q,v,a);
//
//
// const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
// Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
// Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
// Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
// Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
// Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
// Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
// Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
// Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();
//
// getJointAccelerationDerivatives<WORLD>(model,data,jointId,
// v_partial_dq,
// a_partial_dq,a_partial_dv,a_partial_da);
//
// getJointAccelerationDerivatives<LOCAL>(model,data,jointId,
// v_partial_dq_local,
// a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
//
// // Check v_partial_dq against getJointVelocityDerivatives
// {
// Data data_v(model);
// computeForwardKinematicsDerivatives(model,data_v,q,v,a);
//
// Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
// Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();