Unverified Commit d78d951d authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1311 from jcarpent/topic/coriolis

Add getCoriolisMatrix
parents 1b10650f 0ca05e2f
Pipeline #11625 passed with stage
in 215 minutes and 34 seconds
......@@ -82,6 +82,15 @@ namespace pinocchio
"\tq: the joint configuration vector (size model.nq)\n"
"\tv: the joint velocity vector (size model.nv)\n",
bp::return_value_policy<bp::return_by_value>());
bp::def("getCoriolisMatrix",
&getCoriolisMatrix<double,0,JointCollectionDefaultTpl>,
bp::args("model","Data"),
"Retrives the Coriolis Matrix C(q,v) of the Lagrangian dynamics after calling one of the derivative algorithms, store the result in data.C and return it.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n",
bp::return_value_policy<bp::return_by_value>());
}
......
Subproject commit e4f507fb7e688c5d2dcf8f2d5c1af388596ba91c
Subproject commit 95917116c0f1b0b0690a22e12d3871b3bb7cae27
......@@ -55,7 +55,7 @@ namespace pinocchio
ov = data.oMi[i].act(data.v[i]);
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.Yaba[i] = model.inertias[i].matrix();
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
data.oh[i] = data.oYcrb[i] * ov;
data.of[i] = ov.cross(data.oh[i]);
......
//
// Copyright (c) 2017-2019 CNRS INRIA
// Copyright (c) 2017-2020 CNRS INRIA
//
#ifndef __pinocchio_rnea_derivatives_hxx__
......@@ -46,7 +46,7 @@ namespace pinocchio
else
data.oMi[i] = data.liMi[i];
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
data.of[i] = data.oYcrb[i] * minus_gravity;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
......@@ -242,7 +242,7 @@ namespace pinocchio
data.a[i] += data.liMi[i].actInv(data.a[parent]);
}
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
ov = data.oMi[i].act(data.v[i]);
oa = data.oMi[i].act(data.a[i]);
oa_gf = oa - model.gravity; // add gravity contribution
......
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2020 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__
......@@ -183,10 +183,13 @@ namespace pinocchio
/// \brief Left variation of the inertia matrix
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
/// \brief Inertia quantities expressed in the world frame
/// \brief Rigid Body Inertia supported by the joint expressed in the world frame
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
/// \brief Composite Rigid Body Inertia expressed in the world frame
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
/// \brief Time variation of the inertia quantities expressed in the world frame
/// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
/// \brief Temporary for derivative algorithms
......
......@@ -53,6 +53,7 @@ namespace pinocchio
, IS(MatrixXs::Zero(6,model.nv))
, vxI((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, Ivx((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, oinertias((std::size_t)model.njoints,Inertia::Zero())
, oYcrb((std::size_t)model.njoints,Inertia::Zero())
, doYcrb((std::size_t)model.njoints,Inertia::Matrix6::Zero())
, ddq(VectorXs::Zero(model.nv))
......@@ -248,6 +249,7 @@ namespace pinocchio
&& data1.IS == data2.IS
&& data1.vxI == data2.vxI
&& data1.Ivx == data2.Ivx
&& data1.oinertias == data2.oinertias
&& data1.oYcrb == data2.oYcrb
&& data1.doYcrb == data2.doYcrb
&& data1.ddq == data2.ddq
......
......@@ -419,7 +419,7 @@ namespace pinocchio
{
typedef typename ConfigVectorLike::Scalar Scalar;
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
typedef typename Eigen::Quaternion<Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
ConstQuaternionMap quat(q_joint.derived().data());
......
......@@ -57,6 +57,7 @@ namespace boost
PINOCCHIO_MAKE_DATA_NVP(ar,data,IS);
PINOCCHIO_MAKE_DATA_NVP(ar,data,vxI);
PINOCCHIO_MAKE_DATA_NVP(ar,data,Ivx);
PINOCCHIO_MAKE_DATA_NVP(ar,data,oinertias);
PINOCCHIO_MAKE_DATA_NVP(ar,data,oYcrb);
PINOCCHIO_MAKE_DATA_NVP(ar,data,doYcrb);
PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq);
......
......@@ -471,4 +471,39 @@ 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;
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