Verified Commit 0b94ed14 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: add getCoriolisMatrix

parent e5d52188
......@@ -2,8 +2,8 @@
// Copyright (c) 2015-2019 CNRS INRIA
//
#ifndef __pinocchio_rnea_hpp__
#define __pinocchio_rnea_hpp__
#ifndef __pinocchio_algorithm_rnea_hpp__
#define __pinocchio_algorithm_rnea_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
......@@ -154,9 +154,28 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v);
///
/// \brief Retrives 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>
/// after a call to the dynamics derivatives.
///
/// \note In the previous equation, \f$ c(q, \dot{q}) = C(q, \dot{q})\dot{q} \f$.
///
/// \tparam JointCollection Collection of Joint types.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \return The Coriolis matrix stored in data.C.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
getCoriolisMatrix(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data);
} // namespace pinocchio
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/rnea.hxx"
#endif // ifndef __pinocchio_rnea_hpp__
#endif // ifndef __pinocchio_algorithm_rnea_hpp__
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_rnea_hxx__
#define __pinocchio_rnea_hxx__
#ifndef __pinocchio_algorithm_rnea_hxx__
#define __pinocchio_algorithm_rnea_hxx__
/// @cond DEV
......@@ -473,8 +473,8 @@ namespace pinocchio
{
typedef typename Model::JointIndex JointIndex;
const JointIndex & i = jmodel.id();
const JointIndex & parent = model.parents[i];
const JointIndex i = jmodel.id();
const JointIndex parent = model.parents[i];
typename Data::RowMatrix6 & M6tmpR = data.M6tmpR;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
......@@ -543,9 +543,84 @@ namespace pinocchio
return data.C;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
struct GetCoriolisMatrixBackwardStep
: public fusion::JointUnaryVisitorBase< GetCoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> >
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &
> ArgsType;
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
const Model & model,
Data & data)
{
typedef typename Model::JointIndex JointIndex;
typedef CoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> EquivalentPass;
const JointIndex i = jmodel.id();
const JointIndex parent = model.parents[i];
typename Data::RowMatrix6 & M6tmpR = data.M6tmpR;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
ColsBlock J_cols = jmodel.jointCols(data.J);
typename Data::Matrix6x & dFdv = data.Fcrb[0];
ColsBlock dFdv_cols = jmodel.jointCols(dFdv);
motionSet::inertiaAction(data.oYcrb[i],dJ_cols,dFdv_cols);
dFdv_cols.noalias() += data.vxI[i] * J_cols;
data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
= J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
EquivalentPass::lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv()));
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j])
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j);
M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.vxI[i];
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j])
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() += M6tmpR.topRows(jmodel.nv()) * data.J.col(j);
if(parent>0)
{
data.vxI[parent] += data.vxI[i];
}
}
};
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
getCoriolisMatrix(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Inertia::vxi(data.ov[i],data.oinertias[i],data.vxI[i]);
}
typedef GetCoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
{
Pass2::run(model.joints[i],typename Pass2::ArgsType(model,data));
}
return data.C;
}
} // namespace pinocchio
/// @endcond
#endif // ifndef __pinocchio_rnea_hxx__
#endif // ifndef __pinocchio_algorithm_rnea_hxx__
......@@ -471,4 +471,41 @@ BOOST_AUTO_TEST_CASE(test_multiple_calls)
BOOST_CHECK(data1.M.isApprox(data2.M));
}
BOOST_AUTO_TEST_CASE(test_get_coriolis)
{
using namespace Eigen;
using namespace pinocchio;
const double prec = Eigen::NumTraits<double>::dummy_precision();
Model model;
buildModels::humanoidRandom(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill( 1.);
Data data_ref(model);
Data data(model);
VectorXd q = randomConfiguration(model);
VectorXd v = VectorXd::Random(model.nv);
VectorXd tau = VectorXd::Random(model.nv);
computeCoriolisMatrix(model,data_ref,q,v);
computeRNEADerivatives(model,data,q,v,tau);
getCoriolisMatrix(model,data);
BOOST_CHECK(data.J.isApprox(data_ref.J));
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdv));
for(JointIndex k = 1; k < model.joints.size(); ++k)
{
BOOST_CHECK(data.vxI[k].isApprox(data_ref.vxI[k]));
BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
}
BOOST_CHECK(data.C.isApprox(data_ref.C));
}
BOOST_AUTO_TEST_SUITE_END()
......@@ -243,90 +243,70 @@ BOOST_AUTO_TEST_CASE(test_compute_static_torque)
BOOST_CHECK(static_torque_ref.isApprox(data.tau));
}
BOOST_AUTO_TEST_CASE(test_compute_coriolis)
BOOST_AUTO_TEST_CASE(test_compute_coriolis)
{
using namespace Eigen;
using namespace pinocchio;
const double prec = Eigen::NumTraits<double>::dummy_precision();
Model model;
buildModels::humanoidRandom(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill( 1.);
Data data_ref(model);
Data data(model);
VectorXd q = randomConfiguration(model);
VectorXd v (VectorXd::Random(model.nv));
computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
BOOST_CHECK(data.C.isZero(prec));
model.gravity.setZero();
rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
computeJointJacobiansTimeVariation(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,prec));
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,prec));
Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
Force dh(data_ref.dAg * v);
BOOST_CHECK(dh.isApprox(dh_ref,prec));
{
using namespace Eigen;
using namespace pinocchio;
const double prec = Eigen::NumTraits<double>::dummy_precision();
Model model;
buildModels::humanoidRandom(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill( 1.);
Data data_ref(model);
Data data(model);
VectorXd q = randomConfiguration(model);
VectorXd v (VectorXd::Random(model.nv));
computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
BOOST_CHECK(data.C.isZero(prec));
model.gravity.setZero();
rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
computeJointJacobiansTimeVariation(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));
Data data_ref(model), data_ref_plus(model);
Eigen::MatrixXd dM(data.C + data.C.transpose());
VectorXd tau = data.C * v;
BOOST_CHECK(tau.isApprox(data_ref.tau,prec));
const double alpha = 1e-8;
Eigen::VectorXd q_plus(model.nq);
q_plus = integrate(model,q,alpha*v);
dccrba(model,data_ref,q,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>();
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,prec));
Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
Force dh(data_ref.dAg * v);
BOOST_CHECK(dh.isApprox(dh_ref,prec));
{
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;
// }
Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
}
BOOST_AUTO_TEST_SUITE_END ()
}
BOOST_AUTO_TEST_SUITE_END()
Markdown is supported
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