Commit 5f3f9f1d by jcarpent

### [C++] Add CCRBA for composition centroidal dynamics

parent f61413fb
 ... ... @@ -21,6 +21,7 @@ #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/multibody/model.hpp" #include "pinocchio/spatial/act-on-set.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include ... ... @@ -45,6 +46,28 @@ namespace se3 crba(const Model & model, Data & data, const Eigen::VectorXd & q); /// /// \brief Computes the upper triangular part of the joint space inertia matrix M by /// using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008). /// The result is accessible throw data.M. /// /// \note You can easly get data.M symetric by copying the stricly upper trinangular part /// in the stricly lower tringular part with /// data.M.triangularView() = data.M.transpose().triangularView(); /// /// \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 configuration vector (dim model.nv). /// /// \return The joint space inertia matrix with only the upper triangular part computed. /// inline const Data::Matrix6x & ccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v); } // namespace se3 ... ... @@ -107,10 +130,10 @@ namespace se3 /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) = jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]); = jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]); const Model::JointIndex & parent = model.parents[i]; if(parent>0) if(parent>0) { /* Yli += liXi Yi */ data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); ... ... @@ -148,6 +171,110 @@ namespace se3 return data.M; } struct CcrbaForwardStep : public fusion::JointVisitor { typedef boost::fusion::vector< const se3::Model &, se3::Data &, const Model::Index, const Eigen::VectorXd & > ArgsType; JOINT_VISITOR_INIT(CcrbaForwardStep); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data, const Model::Index i, const Eigen::VectorXd & q) { using namespace Eigen; using namespace se3; const Model::Index & parent = model.parents[i]; jmodel.calc(jdata.derived(),q); data.liMi[i] = model.jointPlacements[i]*jdata.M(); data.Ycrb[i] = model.inertias[i]; if (parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i]; else data.oMi[i] = data.liMi[i]; } }; // struct CcrbaForwardStep struct CcrbaBackwardStep : public fusion::JointVisitor { typedef boost::fusion::vector< const se3::Model &, se3::Data &, const Model::Index > ArgsType; JOINT_VISITOR_INIT(CcrbaBackwardStep); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data, const Model::Index i) { using namespace Eigen; using namespace se3; const Model::Index & parent = model.parents[i]; data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); jdata.U() = data.Ycrb[i] * jdata.S(); Eigen::Block jF = data.Ag.block(0,jmodel.idx_v(),6,JointModel::NV); Eigen::Block iF = jdata.U().block(0,0,6,JointModel::NV); forceSet::se3Action(data.oMi[i],iF,jF); } }; // struct CcrbaBackwardStep inline const Data::Matrix6x & ccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) { typedef Eigen::Block Block3x; forwardKinematics(model, data, q); for( Model::Index i=1;i<(Model::Index)(model.nbody);++i ) { // CcrbaForwardStep::run(model.joints[i],data.joints[i], // CcrbaForwardStep::ArgsType(model,data,i,q)); data.Ycrb[i] = model.inertias[i]; } data.Ycrb[0].setZero(); for(Model::Index i=(Model::Index)(model.nbody-1);i>0;--i) { CcrbaBackwardStep::run(model.joints[i],data.joints[i], CcrbaBackwardStep::ArgsType(model,data,i)); } data.com[0] = data.Ycrb[0].lever(); const Block3x Ag_lin = data.Ag.middleRows<3> (Force::LINEAR); Block3x Ag_ang = data.Ag.middleRows<3> (Force::ANGULAR); for (long i = 0; i
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!