Commit a50e7131 by jcarpent

### [Algo] Add algo to compute the time derivative of the Centroidal Momentum Matrix

parent 31562102
 ... ... @@ -62,23 +62,19 @@ namespace se3 const Eigen::VectorXd & v); /// /// \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 through data.M. /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors. /// /// \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(); /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$\dot{h} = A_g \ddot{q} + \dot{A_g}(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 configuration vector (dim model.nv). /// /// \return The joint space inertia matrix with only the upper triangular part computed. /// \return The Centroidal Momentum Matrix time derivative dAg /// inline const Data::Matrix6x & ccrba(const Model & model, dccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v); ... ...
 ... ... @@ -179,25 +179,25 @@ namespace se3 forceSet::se3Action(data.oMi[i],jdata.U(),jF); } static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data) { typedef SizeDepType::ColsReturn::Type ColsBlock; const Model::JointIndex & i = (Model::JointIndex) jmodel.id(); const Model::Index & parent = model.parents[i]; data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); jdata.U() = data.Ycrb[i] * jdata.S(); ColsBlock jF = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv()); forceSet::se3Action(data.oMi[i],jdata.U(),jF); } // static void algo(const se3::JointModelBase & jmodel, // se3::JointDataBase & jdata, // const se3::Model & model, // se3::Data & data) // { // typedef SizeDepType::ColsReturn::Type ColsBlock; // // const Model::JointIndex & i = (Model::JointIndex) jmodel.id(); // const Model::Index & parent = model.parents[i]; // // data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); // // jdata.U() = data.Ycrb[i] * jdata.S(); // // ColsBlock jF // = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv()); // // forceSet::se3Action(data.oMi[i],jdata.U(),jF); // } }; // struct CcrbaBackwardStep ... ... @@ -235,6 +235,174 @@ namespace se3 return data.Ag; } struct DCcrbaForwardStep : public fusion::JointVisitor { typedef boost::fusion::vector< const se3::Model &, se3::Data &, const Eigen::VectorXd &, const Eigen::VectorXd & > ArgsType; JOINT_VISITOR_INIT(DCcrbaForwardStep); 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 = (Model::JointIndex) jmodel.id(); const Model::Index & parent = model.parents[i]; jmodel.calc(jdata.derived(),q,v); data.liMi[i] = model.jointPlacements[i]*jdata.M(); data.Ycrb[i] = model.inertias[i]; data.v[i] = jdata.v(); if (parent>0) { data.oMi[i] = data.oMi[parent]*data.liMi[i]; data.v[i] += data.liMi[i].actInv(data.v[parent]); } else data.oMi[i] = data.liMi[i]; } }; // struct DCcrbaForwardStep struct DCcrbaBackwardStep : public fusion::JointVisitor { typedef boost::fusion::vector< const se3::Model &, se3::Data & > ArgsType; JOINT_VISITOR_INIT(DCcrbaBackwardStep); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data) { typedef typename SizeDepType::template ColsReturn::Type ColsBlock; typedef typename JointModel::JointDataDerived JointData; typedef typename JointModel::Constraint_t Constraint_t; const Model::JointIndex & i = (Model::JointIndex) jmodel.id(); const Model::Index & parent = model.parents[i]; const Motion & v = data.v[i]; const Inertia & Y = data.Ycrb[i]; const Inertia::Matrix6 & dY = data.dYcrb[i]; data.Ycrb[parent] += data.liMi[i].act(Y); data.dYcrb[parent] += SE3actOn(data.liMi[i],dY); // Calc Ag ColsBlock Ag_cols = jmodel.jointCols(data.Ag); jdata.U() = Y * jdata.S(); forceSet::se3Action(data.oMi[i],jdata.U(),Ag_cols); // Calc dAg = oXi* dU typename JointData::U_t dU(dY * jdata.S()); // TODO: add dU inside jdata. typename Constraint_t::DenseBase dS = jdata.S().variation(v); dU.noalias() += Y.matrix()*dS; ColsBlock dAg_cols = jmodel.jointCols(data.dAg); forceSet::se3Action(data.oMi[i],dU,dAg_cols); } inline static Inertia::Matrix6 SE3actOn(const SE3 & M, const Inertia::Matrix6 & I) { typedef Inertia::Matrix6 Matrix6; typedef SE3::Matrix3 Matrix3; typedef SE3::Vector3 Vector3; typedef Eigen::Block constBlock3; typedef Eigen::Block Block3; const constBlock3 & Ai = I.block<3,3> (Inertia::LINEAR, Inertia::LINEAR); const constBlock3 & Bi = I.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR); const constBlock3 & Di = I.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR); const Matrix3 & R = M.rotation(); const Vector3 & t = M.translation(); Matrix6 res; Block3 Ao = res.block<3,3> (Inertia::LINEAR, Inertia::LINEAR); Block3 Bo = res.block<3,3> (Inertia::LINEAR, Inertia::ANGULAR); Block3 Co = res.block<3,3> (Inertia::ANGULAR, Inertia::LINEAR); Block3 Do = res.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR); Ao = R*Ai*R.transpose(); Bo = R*Bi*R.transpose(); Do.row(0) = t.cross(Bo.col(0)); Do.row(1) = t.cross(Bo.col(1)); Do.row(2) = t.cross(Bo.col(2)); Co.col(0) = t.cross(Ao.col(0)); Co.col(1) = t.cross(Ao.col(1)); Co.col(2) = t.cross(Ao.col(2)); Co += Bo.transpose(); Bo = Co.transpose(); Do.col(0) += t.cross(Bo.col(0)); Do.col(1) += t.cross(Bo.col(1)); Do.col(2) += t.cross(Bo.col(2)); Do += R*Di*R.transpose(); return res; } }; // struct DCcrbaBackwardStep inline const Data::Matrix6x & dccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) { assert(model.check(data) && "data is not consistent with model."); typedef Eigen::Block Block3x; forwardKinematics(model,data,q,v); data.Ycrb[0].setZero(); for(Model::Index i=1;i<(Model::Index)(model.njoints);++i) { data.Ycrb[i] = model.inertias[i]; data.dYcrb[i] = model.inertias[i].variation(data.v[i]); // (vx* I - Ivx) } for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i) { DCcrbaBackwardStep::run(model.joints[i],data.joints[i], DCcrbaBackwardStep::ArgsType(model,data)); } 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(Force::LINEAR); Block3x dAg_ang = data.dAg.middleRows<3>(Force::ANGULAR); for (long i = 0; i
 ... ... @@ -402,6 +402,9 @@ namespace se3 /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint. container::aligned_vector Ycrb; /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$\dot{Y}_{crb}$\f. See Data::Ycrb for more details. container::aligned_vector dYcrb; // TODO: change with dense symmetric matrix6 /// \brief The joint space inertia matrix (a square matrix of dim model.nv). Eigen::MatrixXd M; ... ... @@ -410,7 +413,7 @@ namespace se3 // ABA internal data /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA] container::aligned_vector Yaba; container::aligned_vector Yaba; // TODO: change with dense symmetric matrix6 /// \brief Intermediate quantity corresponding to apparent torque [ABA] Eigen::VectorXd u; // Joint Inertia ... ... @@ -420,6 +423,11 @@ namespace se3 /// \note \f$hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum. Matrix6x Ag; // dCCRBA return quantities /// \brief Centroidal Momentum Matrix Time Variation /// \note \f$\dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum. Matrix6x dAg; /// \brief Centroidal momentum quantity. /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame). /// ... ...
 ... ... @@ -238,11 +238,13 @@ namespace se3 ,nle(model.nv) ,oMf((std::size_t)model.nframes) ,Ycrb((std::size_t)model.njoints) ,dYcrb((std::size_t)model.njoints) ,M(model.nv,model.nv) ,ddq(model.nv) ,Yaba((std::size_t)model.njoints) ,u(model.nv) ,Ag(6,model.nv) ,dAg(6,model.nv) ,Fcrb((std::size_t)model.njoints) ,lastChild((std::size_t)model.njoints) ,nvSubtree((std::size_t)model.njoints) ... ...
 // // Copyright (c) 2015-2016 CNRS // Copyright (c) 2015-2017 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it ... ... @@ -127,6 +127,56 @@ BOOST_AUTO_TEST_CASE (test_ccrb) se3::Data::Matrix6x Ag_ref (cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows <6> ()); BOOST_CHECK(data.Ag.isApprox(Ag_ref,1e-12)); } BOOST_AUTO_TEST_CASE (test_dccrb) { using namespace se3; Model model; buildModels::humanoidSimple(model); Data data(model), data_ref(model); Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq); q.segment <4> (3).normalize(); Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv); Eigen::VectorXd a = Eigen::VectorXd::Ones(model.nv); const Eigen::VectorXd g = rnea(model,data_ref,q,0*v,0*a); rnea(model,data_ref,q,v,a); crba(model,data_ref,q); data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); SE3::Vector3 com = data_ref.Ycrb[1].lever(); SE3 cMo(SE3::Identity()); cMo.translation() = -getComFromCrba(model, data_ref); SE3 oM1 (data_ref.liMi[1]); SE3 cM1 (cMo * oM1); Data::Matrix6x Ag_ref (cM1.toDualActionMatrix() * data_ref.M.topRows <6> ()); Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>()))); ccrba(model,data_ref,q,v); dccrba(model,data,q,0*v); BOOST_CHECK(data.Ag.isApprox(Ag_ref)); BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); BOOST_CHECK(data.dAg.isZero()); centerOfMass(model,data_ref,q,v,a); BOOST_CHECK(data_ref.vcom[0].isApprox(data_ref.hg.linear()/data_ref.M(0,0))); BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear()/data_ref.M(0,0))); dccrba(model,data,q,v); Force::Vector6 test1(data.Ag * a); Force::Vector6 test2(data.dAg * v); Force hdot(data.Ag * a + data.dAg * v); BOOST_CHECK(hdot.isApprox(hdot_ref)); } 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!