Commit b35c281c by jcarpent

[Algo] Add algo to compute the inverse of the joint space inertia matrix

parent 6db2169c
 // // Copyright (c) 2016 CNRS // Copyright (c) 2016-2018 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it ... ... @@ -60,6 +60,20 @@ namespace se3 const Eigen::VectorXd & v, const Eigen::VectorXd & tau, const container::aligned_vector & fext); /// /// \brief Computes the inverse of the joint space inertia matrix using Articulated Body formulation. /// /// \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). /// /// \return The inverse of the joint space inertia matrix stored in data.ddq. /// inline const Data::RowMatrixXd & computeMinverse(const Model & model, Data & data, const Eigen::VectorXd & q); DEFINE_ALGO_CHECKER(ABA); ... ...
 ... ... @@ -18,6 +18,7 @@ #ifndef __se3_aba_hxx__ #define __se3_aba_hxx__ #include "pinocchio/spatial/act-on-set.hpp" #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/algorithm/check.hpp" ... ... @@ -257,8 +258,174 @@ namespace se3 return data.ddq; } struct computeMinverseForwardStep1 : public fusion::JointVisitor { typedef boost::fusion::vector ArgsType; JOINT_VISITOR_INIT(computeMinverseForwardStep1); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data, const Eigen::VectorXd & q) { const Model::JointIndex & i = jmodel.id(); jmodel.calc(jdata.derived(),q); const Model::Index & parent = model.parents[i]; 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]; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock J_cols = jmodel.jointCols(data.J); J_cols = data.oMi[i].act(jdata.S()); data.Yaba[i] = model.inertias[i].matrix(); } }; struct computeMinverseBackwardStep : public fusion::JointVisitor { typedef boost::fusion::vector ArgsType; JOINT_VISITOR_INIT(computeMinverseBackwardStep); template static void algo(const JointModelBase & jmodel, JointDataBase & jdata, const Model & model, Data & data) { const Model::JointIndex & i = jmodel.id(); const Model::Index & parent = model.parents[i]; Inertia::Matrix6 & Ia = data.Yaba[i]; Data::RowMatrixXd & Minv = data.Minv; Data::Matrix6x & Fcrb = data.Fcrb[0]; Data::Matrix6x & FcrbTmp = data.Fcrb.back(); Data::RowMatrix6 & M6tmpR = data.M6tmpR; jmodel.calc_aba(jdata.derived(), Ia, parent > 0); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; typedef typename SizeDepType::template ColsReturn::Type RowColsBlock; // RowColsBlock U_cols = M6tmpR.leftCols(jmodel.nv()); ColsBlock U_cols = jmodel.jointCols(data.IS); forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv(); const int nv_children = data.nvSubtree[i] - jmodel.nv(); if(nv_children > 0) { ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(),jmodel.idx_v()+jmodel.nv(),jmodel.nv(),nv_children).noalias() = -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v()+jmodel.nv(),nv_children); if(parent > 0) { FcrbTmp.leftCols(data.nvSubtree[i]).noalias() = U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]); // Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) // += U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); } } else { Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() = U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); } if(parent > 0) data.Yaba[parent] += AbaBackwardStep::SE3actOn(data.liMi[i], Ia); } }; struct computeMinverseForwardStep2 : public fusion::JointVisitor { typedef boost::fusion::vector ArgsType; JOINT_VISITOR_INIT(computeMinverseForwardStep2); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data) { const Model::JointIndex & i = jmodel.id(); const Model::Index & parent = model.parents[i]; Data::RowMatrixXd & Minv = data.Minv; Data::Matrix6x & Fcrb = data.Fcrb[0]; Data::Matrix6x & FcrbTmp = data.Fcrb.back(); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv); forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame ColsBlock J_cols = jmodel.jointCols(data.J); if(parent > 0) { FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() = UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()) -= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); // Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()) // -= UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); } data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = J_cols * Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); if(parent > 0) data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); } }; inline const Data::RowMatrixXd & computeMinverse(const Model & model, Data & data, const Eigen::VectorXd & q) { assert(model.check(data) && "data is not consistent with model."); for(Model::Index i=1;i<(Model::Index)model.njoints;++i) { computeMinverseForwardStep1::run(model.joints[i],data.joints[i], computeMinverseForwardStep1::ArgsType(model,data,q)); } data.Fcrb[0].setZero(); for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i ) { computeMinverseBackwardStep::run(model.joints[i],data.joints[i], computeMinverseBackwardStep::ArgsType(model,data)); } for(Model::Index i=1;i<(Model::Index)model.njoints;++i) { computeMinverseForwardStep2::run(model.joints[i],data.joints[i], computeMinverseForwardStep2::ArgsType(model,data)); } return data.Minv; } // --- CHECKER --------------------------------------------------------------- ... ...
 ... ... @@ -360,6 +360,8 @@ namespace se3 typedef SE3::Vector3 Vector3; typedef Eigen::Matrix RowMatrix6; typedef Eigen::Matrix RowMatrixXd; /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, /// encapsulated in JointDataAccessor. JointDataVector joints; ... ... @@ -425,6 +427,9 @@ namespace se3 /// \brief The joint space inertia matrix (a square matrix of dim model.nv). Eigen::MatrixXd M; /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv). RowMatrixXd Minv; /// \brief The Coriolis matrix (a square matrix of dim model.nv). Eigen::MatrixXd C; ... ... @@ -437,6 +442,15 @@ namespace se3 /// \brief Variation of the forceset with respect to the joint acceleration. Matrix6x dFda; /// \brief Used in computeMinverse Matrix6x SDinv; /// \brief Used in computeMinverse Matrix6x UDinv; /// \brief Used in computeMinverse Matrix6x IS; /// \brief Right variation of the inertia matrix container::aligned_vector vxI; ... ...
 ... ... @@ -252,10 +252,14 @@ namespace se3 ,Ycrb((std::size_t)model.njoints) ,dYcrb((std::size_t)model.njoints) ,M(model.nv,model.nv) ,Minv(model.nv,model.nv) ,C(model.nv,model.nv) ,dFdq(6,model.nv) ,dFdv(6,model.nv) ,dFda(6,model.nv) ,SDinv(6,model.nv) ,UDinv(6,model.nv) ,IS(6,model.nv) ,vxI((std::size_t)model.njoints) ,Ivx((std::size_t)model.njoints) ,oYcrb((std::size_t)model.njoints) ... ... @@ -301,7 +305,7 @@ namespace se3 joints.push_back(CreateJointData::run(model.joints[i])); /* Init for CRBA */ M.fill(0); M.fill(0); Minv.setZero(); for(Model::Index i=0;i<(Model::Index)(model.njoints);++i ) { Fcrb[i].resize(6,model.nv); } computeLastChild(model); ... ...
 // // Copyright (c) 2016 CNRS // Copyright (c) 2016-2018 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it ... ... @@ -22,6 +22,7 @@ #include "pinocchio/algorithm/aba.hpp" #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/crba.hpp" #include "pinocchio/parsers/sample-models.hpp" ... ... @@ -125,5 +126,45 @@ BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea ) BOOST_CHECK(data.ddq.isApprox(a, 1e-12)); } BOOST_AUTO_TEST_CASE ( test_computeMinverse ) { using namespace Eigen; using namespace se3; se3::Model model; buildModels::humanoidSimple(model); model.gravity.setZero(); se3::Data data(model); se3::Data data_ref(model); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); VectorXd q = randomConfiguration(model); VectorXd v = VectorXd::Random(model.nv); crba(model, data_ref, q); data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); MatrixXd Minv_ref(data_ref.M.inverse()); computeMinverse(model, data, q); BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>())); data.Minv.triangularView() = data.Minv.transpose().triangularView(); BOOST_CHECK(data.Minv.isApprox(Minv_ref)); // std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl; // std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl; // // std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl; // std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << 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