Commit 69ccceac authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add algo to compute the Coriolis matrix

parent 3854dc4a
......@@ -89,6 +89,23 @@ namespace se3
inline const Eigen::VectorXd &
computeGeneralizedGravity(const Model & model, Data & data,
const Eigen::VectorXd & q);
///
/// \brief Computes the Coriolis Matrix \f$ C(q,\dot{q}) \f$ of the Lagrangian dynamics:
/// <CENTER> \f$ \begin{eqnarray} M \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau \end{eqnarray} \f$ </CENTER> <BR>
/// \note In the previous equation, \f$ c(q, \dot{q}) = C(q, \dot{q})\dot{q} \f$.
///
/// \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).
///
/// \return The Coriolis matrix stored in data.C.
///
inline const Eigen::MatrixXd &
computeCoriolisMatrix(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
} // namespace se3
......
......@@ -296,6 +296,167 @@ namespace se3
return data.g;
}
struct CoriolisMatrixForwardStep : public fusion::JointVisitor<CoriolisMatrixForwardStep>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(CoriolisMatrixForwardStep);
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 Model::JointIndex & i = jmodel.id();
const Model::JointIndex & parent = model.parents[i];
Inertia::Matrix6 & tmp = data.Itmp;
jmodel.calc(jdata.derived(),q,v);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
if(parent>0) data.oMi[i] = data.oMi[parent] * data.liMi[i];
else data.oMi[i] = data.liMi[i];
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
// computes S expressed at the world frame
jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame
// computes vxS expressed at the world frame
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
tmp.leftCols(jmodel.nv()) = data.v[i].cross(jdata.S());
ColsBlock dJcols = jmodel.jointCols(data.dJ);
motionSet::se3Action(data.oMi[i],tmp.leftCols(jmodel.nv()),dJcols);
// express quantities in the world frame
data.oYo[i] = data.oMi[i].act(model.inertias[i]);
// computes vxI
const Motion ov(data.oMi[i].act(data.v[i])); // v_i expressed in the world frame
Inertia::vxi(ov,data.oYo[i],data.vxI[i]);
}
};
struct CoriolisMatrixBackwardStep : public fusion::JointVisitor<CoriolisMatrixBackwardStep>
{
typedef boost::fusion::vector<const Model &,
Data &
> ArgsType;
JOINT_VISITOR_INIT(CoriolisMatrixBackwardStep);
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
JointDataBase<typename JointModel::JointDataDerived> & jdata,
const Model & model,
Data & data)
{
const Model::JointIndex & i = jmodel.id();
const Model::JointIndex & parent = model.parents[i];
Inertia::Matrix6 & tmp = data.Itmp;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
ColsBlock dJcols = jmodel.jointCols(data.dJ);
ColsBlock Jcols = jmodel.jointCols(data.J);
rhsInertiaMult(data.oYo[i],dJcols,jmodel.jointCols(data.dFdv));
jmodel.jointCols(data.dFdv) += data.vxI[i] * Jcols;
data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
= Jcols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
lhsInertiaMult(data.oYo[i],Jcols.transpose(),tmp.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) = tmp.topRows(jmodel.nv()) * data.dJ.col(j);
tmp.topRows(jmodel.nv()) = Jcols.transpose() * data.vxI[i];
for(int j = data.parents_fromRow[(Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(Model::Index)j])
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += tmp.topRows(jmodel.nv()) * data.J.col(j);
if(parent>0)
{
data.oYo[parent] += data.oYo[i];
data.vxI[parent] += data.vxI[i];
}
}
template<typename Min, typename Mout>
static void rhsInertiaMultVector(const Inertia & Y,
const Eigen::MatrixBase<Min> & m,
const Eigen::MatrixBase<Mout> & f)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Min,6);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Mout,6);
Mout & f_ = const_cast<Mout &>(f.derived());
f_.template segment<3>(Inertia::LINEAR) = -Y.mass() * Y.lever().cross(m.template segment<3>(Motion::ANGULAR));
f_.template segment<3>(Inertia::ANGULAR) = Y.inertia() * m.template segment<3>(Motion::ANGULAR);
f_.template segment<3>(Inertia::ANGULAR) += Y.lever().cross(f_.template segment<3>(Inertia::LINEAR));
f_.template segment<3>(Inertia::ANGULAR) += Y.mass() * Y.lever().cross(m.template segment<3>(Motion::LINEAR));
f_.template segment<3>(Inertia::LINEAR) += Y.mass() * m.template segment<3>(Motion::LINEAR);
}
template<typename Min, typename Mout>
static void rhsInertiaMult(const Inertia & Y,
const Eigen::MatrixBase<Min> & J,
const Eigen::MatrixBase<Mout> & F)
{
assert(J.cols() == F.cols());
Mout & F_ = const_cast<Mout &>(F.derived());
for(int i = 0; i < J.cols(); ++i)
{
rhsInertiaMultVector(Y,J.col(i),F_.col(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());
rhsInertiaMult(Y,J.transpose(),F_.transpose());
}
};
inline const Eigen::MatrixXd &
computeCoriolisMatrix(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
assert(model.check(data) && "data is not consistent with model.");
assert(q.size() == model.nq);
assert(v.size() == model.nv);
for(size_t i=1;i<(size_t) model.njoints;++i)
{
CoriolisMatrixForwardStep::run(model.joints[i],data.joints[i],
CoriolisMatrixForwardStep::ArgsType(model,data,q,v));
}
for(size_t i=(size_t) (model.njoints-1);i>0;--i)
{
CoriolisMatrixBackwardStep::run(model.joints[i],data.joints[i],
CoriolisMatrixBackwardStep::ArgsType(model,data));
}
return data.C;
}
} // namespace se3
/// @endcond
......
......@@ -411,6 +411,21 @@ namespace se3
/// \brief The Coriolis matrix (a square matrix of dim model.nv).
Eigen::MatrixXd C;
/// \brief Variation of the forceset with respect to qdot.
Matrix6x dFdv;
/// \brief Right variation of the inertia matrix
container::aligned_vector<Inertia::Matrix6> vxI;
/// \brief Left variation of the inertia matrix
container::aligned_vector<Inertia::Matrix6> Ivx;
/// \brief Inertia quantities expressed in the world frame
container::aligned_vector<Inertia> oYo;
/// \brief Temporary for derivative algorithms
Inertia::Matrix6 Itmp;
/// \brief The joint accelerations computed from ABA
Eigen::VectorXd ddq;
......
......@@ -248,6 +248,10 @@ namespace se3
,dYcrb((std::size_t)model.njoints)
,M(model.nv,model.nv)
,C(model.nv,model.nv)
,dFdv(6,model.nv)
,vxI((std::size_t)model.njoints)
,Ivx((std::size_t)model.njoints)
,oYo((std::size_t)model.njoints)
,ddq(model.nv)
,Yaba((std::size_t)model.njoints)
,u(model.nv)
......
......@@ -27,6 +27,7 @@
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
......@@ -195,4 +196,83 @@ BOOST_AUTO_TEST_CASE(test_compute_gravity)
BOOST_CHECK(g_ref.isApprox(data.g));
}
BOOST_AUTO_TEST_CASE(test_compute_coriolis)
{
using namespace Eigen;
using namespace se3;
Model model;
buildModels::humanoidSimple(model);
Data data_ref(model);
Data data(model);
VectorXd q (VectorXd::Random(model.nq));
q.segment<4>(3).normalize();
VectorXd v (VectorXd::Random(model.nv));
model.gravity.setZero();
rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
computeJacobiansTimeVariation(model,data_ref,q,v);
computeCoriolisMatrix(model,data,q,v);
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
BOOST_CHECK(data.J.isApprox(data_ref.J));
VectorXd tau = data.C * v;
BOOST_CHECK(tau.isApprox(data_ref.tau));
dccrba(model,data_ref,q,v);
crba(model,data_ref,q);
const Data::Vector3 & com = data_ref.com[0];
Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
SE3 cM1(data.oMi[1]); cM1.translation() -= com;
BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag));
Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
Force dh(data_ref.dAg * v);
BOOST_CHECK(dh.isApprox(dh_ref));
{
Data data_ref(model), data_ref_plus(model);
Eigen::MatrixXd dM(data.C + data.C.transpose());
const double alpha = 1e-8;
Eigen::VectorXd q_plus(model.nq);
q_plus = integrate(model,q,alpha*v);
crba(model,data_ref,q);
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
crba(model,data_ref_plus,q_plus);
data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
}
// {
// //v.setZero();
// v.setOnes();
// Eigen::VectorXd v_fd(v);
// Eigen::MatrixXd drnea_dv(model.nv,model.nv);
// Data data_fd(model);
//
// const VectorXd tau0 = rnea(model,data_fd,q,v,0*v);
// const double eps = 1e-8;
// for(int i = 0; i < model.nv; ++i)
// {
// v_fd[i] += eps;
// rnea(model,data_fd,q,v_fd,0*v);
// drnea_dv.col(i) = (data_fd.tau - tau0)/eps;
// v_fd[i] -= eps;
// }
// BOOST_CHECK(v_fd.isApprox(v));
// std::cout << "drnea_dv:\n" << drnea_dv.block<6,6>(0,0) << std::endl;
// std::cout << "C:\n" << data.C.block<6,6>(0,0) << std::endl;;
// }
}
BOOST_AUTO_TEST_SUITE_END ()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment