Commit 0b94ed14 by Justin Carpentier

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 & q, const Eigen::MatrixBase & v); /// /// \brief Retrives 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$

/// 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 class JointCollectionTpl> inline const typename DataTpl::MatrixXs & getCoriolisMatrix(const ModelTpl & model, DataTpl & 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::template ColsReturn::Type ColsBlock; ... ... @@ -543,9 +543,84 @@ namespace pinocchio return data.C; } template class JointCollectionTpl> struct GetCoriolisMatrixBackwardStep : public fusion::JointUnaryVisitorBase< GetCoriolisMatrixBackwardStep > { typedef ModelTpl Model; typedef DataTpl Data; typedef boost::fusion::vector ArgsType; template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { typedef typename Model::JointIndex JointIndex; typedef CoriolisMatrixBackwardStep EquivalentPass; const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; typedef typename SizeDepType::template ColsReturn::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 class JointCollectionTpl> inline const typename DataTpl::MatrixXs & getCoriolisMatrix(const ModelTpl & model, DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); typedef ModelTpl 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 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::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::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::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() = data_ref.M.transpose().triangularView(); crba(model,data_ref_plus,q_plus); data_ref_plus.M.triangularView() = data_ref_plus.M.transpose().triangularView(); 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() = 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; // } 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!