Commit 69ccceac by jcarpent

### [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: ///
\f$\begin{eqnarray} M \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau \end{eqnarray} \f$

/// \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 { typedef boost::fusion::vector ArgsType; JOINT_VISITOR_INIT(CoriolisMatrixForwardStep); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & 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::template ColsReturn::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 { typedef boost::fusion::vector ArgsType; JOINT_VISITOR_INIT(CoriolisMatrixBackwardStep); template static void algo(const JointModelBase & jmodel, JointDataBase & 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::template ColsReturn::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 static void rhsInertiaMultVector(const Inertia & Y, const Eigen::MatrixBase & m, const Eigen::MatrixBase & f) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Min,6); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Mout,6); Mout & f_ = const_cast(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 static void rhsInertiaMult(const Inertia & Y, const Eigen::MatrixBase & J, const Eigen::MatrixBase & F) { assert(J.cols() == F.cols()); Mout & F_ = const_cast(F.derived()); for(int i = 0; i < J.cols(); ++i) { rhsInertiaMultVector(Y,J.col(i),F_.col(i)); } } template static void lhsInertiaMult(const Inertia & Y, const Eigen::MatrixBase & J, const Eigen::MatrixBase & F) { Mout & F_ = const_cast(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 vxI; /// \brief Left variation of the inertia matrix container::aligned_vector Ivx; /// \brief Inertia quantities expressed in the world frame container::aligned_vector 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() = data_ref.M.transpose().triangularView(); crba(model,data_ref_plus,q_plus); data_ref_plus.M.triangularView() = data_ref_plus.M.transpose().triangularView(); 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!