Commit 79251600 by Justin Carpentier Committed by GitHub

Merge pull request #405 from jcarpent/topic/ag-variation

Implement time derivative of the centroidal momemtum matrix
parents 7700dee4 5e4ca72a
 ... ... @@ -47,6 +47,13 @@ namespace se3 "Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite Rigid Body Inertia, puts the result in Data and returns the centroidal mapping.", bp::return_value_policy()); bp::def("dccrba",dccrba, bp::args("Model","Data", "Joint configuration q (size Model::nq)", "Joint velocity v (size Model::nv)"), "Computes the time derivative of the centroidal momentum matrix Ag in terms of q and v. It computes also the same information than ccrtba for the same price.", bp::return_value_policy()); } } // namespace python ... ...
 ... ... @@ -88,6 +88,8 @@ namespace se3 .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag, "Centroidal matrix which maps from joint velocity to the centroidal momentum.") .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dAg, "Time derivative of the centroidal momentum matrix Ag.") .ADD_DATA_PROPERTY_READONLY(Force,hg, "Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).") .ADD_DATA_PROPERTY_READONLY(Inertia,Ig, ... ...
 ... ... @@ -74,6 +74,9 @@ namespace se3 .def("se3ActionInverse",&Motion::se3ActionInverse, bp::args("M"),"Returns the result of the action of the inverse of M on *this.") .add_property("action",&Motion::toActionMatrix,"Returns the action matrix of *this (acting on Motion).") .add_property("dualAction",&Motion::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).") .def("setZero",&MotionPythonVisitor::setZero, "Set the linear and angular components of *this to zero.") .def("setRandom",&MotionPythonVisitor::setRandom, ... ...
 ... ... @@ -44,26 +44,40 @@ namespace se3 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 through data.M. /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta /// according to the current joint configuration and velocity. /// /// \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). /// \param[in] v The joint velocity vector (dim model.nv). /// /// \return The joint space inertia matrix with only the upper triangular part computed. /// \return The Centroidal Momentum Matrix Ag. /// inline const Data::Matrix6x & ccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v); /// /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors. /// /// \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 Centroidal Momentum Matrix time derivative dAg /// inline const Data::Matrix6x & dccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v); DEFINE_ALGO_CHECKER(CRBA); ... ...
 ... ... @@ -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
 ... ... @@ -189,6 +189,13 @@ namespace se3 { return (m.inverse().toActionMatrix()*S).eval(); } DenseBase variation(const Motion & v) const { DenseBase res(v.toActionMatrix() * S); return res; } void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;} ... ...
 ... ... @@ -84,6 +84,8 @@ namespace se3 TransposeConst transpose() const { return TransposeConst(); } operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); } DenseBase variation(const Motion & m) const { return m.toActionMatrix(); } }; // struct ConstraintIdentity template ... ...
 ... ... @@ -189,6 +189,20 @@ namespace se3 return X_subspace; } DenseBase variation(const Motion & m) const { const Motion::ConstLinear_t v = m.linear(); const Motion::ConstAngular_t w = m.angular(); DenseBase res(DenseBase::Zero()); res(0,1) = -w[2]; res(0,2) = v[1]; res(1,0) = w[2]; res(1,2) = -v[0]; res(2,0) = -w[1]; res(2,1) = w[0]; res(3,2) = w[1]; res(4,2) = -w[0]; return res; } }; // struct ConstraintPlanar template ... ...
 ... ... @@ -191,6 +191,14 @@ namespace se3 return ConstraintXd(S); } DenseBase variation(const Motion & m) const { DenseBase res; res << m.angular().cross(axis), Vector3::Zero(); return res; } }; // struct ConstraintPrismaticUnaligned ... ...
 ... ... @@ -196,6 +196,14 @@ namespace se3 S << prismatic::CartesianVector3(1).vector(), Eigen::Vector3d::Zero(); return ConstraintXd(S); } DenseBase variation(const Motion & m) const { DenseBase res; res << m.angular().cross(prismatic::CartesianVector3(1).vector()), Vector3::Zero(); return res; } }; // struct ConstraintPrismatic ... ...
 ... ... @@ -192,6 +192,17 @@ namespace se3 return ConstraintXd(S); } DenseBase variation(const Motion & m) const { const Motion::ConstLinear_t v = m.linear(); const Motion::ConstAngular_t w = m.angular(); DenseBase res; res << v.cross(axis), w.cross(axis); return res; } }; // struct ConstraintRevoluteUnaligned ... ...
 ... ... @@ -198,6 +198,18 @@ namespace se3 S << Eigen::Vector3d::Zero(), revolute::CartesianVector3(1).vector(); return ConstraintXd(S); } DenseBase variation(const Motion & m) const { const typename Motion::ConstLinear_t v = m.linear(); const typename Motion::ConstAngular_t w = m.angular(); const Vector3 a(revolute::CartesianVector3(1).vector()); DenseBase res; res << v.cross(a), w.cross(a); return res; } }; // struct ConstraintRevolute template ... ...
 ... ... @@ -97,6 +97,7 @@ namespace se3 typedef Eigen::Matrix <_Scalar,3,3,_Options> Matrix3; typedef Eigen::Matrix <_Scalar,3,1,_Options> Vector3; typedef Eigen::Matrix <_Scalar,6,3,_Options> ConstraintDense; typedef Eigen::Matrix <_Scalar,6,3,_Options> DenseBase; Matrix3 S_minimal; ... ... @@ -187,6 +188,18 @@ namespace se3 return result; } DenseBase variation(const Motion & m) const { const typename Motion::ConstLinear_t v = m.linear(); const typename Motion::ConstAngular_t w = m.angular(); DenseBase res; res.template middleRows<3>(Motion::LINEAR) = cross(v,S_minimal); res.template middleRows<3>(Motion::ANGULAR) = cross(w,S_minimal); return res; } }; // struct ConstraintRotationalSubspace ... ... @@ -246,6 +259,13 @@ namespace se3 { return Y.template block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal; } inline Eigen::Matrix operator*(const Inertia::Matrix6 & Y, const JointSphericalZYX::ConstraintRotationalSubspace & S) { return Y.block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal; } namespace internal { ... ...
 ... ... @@ -156,11 +156,23 @@ namespace se3 Eigen::Matrix se3Action (const SE3 & m) const { Eigen::Matrix X_subspace; X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation (); X_subspace.block <3,3> (Motion::LINEAR, 0) = cross(m.translation(),m.rotation ()); X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation (); return X_subspace; } DenseBase variation(const Motion & m) const { const Motion::ConstLinear_t v = m.linear(); const Motion::ConstAngular_t w = m.angular(); DenseBase res; res.middleRows<3>(LINEAR) = skew(v); res.middleRows<3>(ANGULAR) = skew(w); return res; } }; // struct ConstraintRotationalSubspace ... ...
 ... ... @@ -172,6 +172,17 @@ namespace se3 return M; } DenseBase variation(const Motion & m) const { const Motion::ConstAngular_t w = m.angular(); DenseBase res; res.middleRows<3>(LINEAR) = skew(w); res.middleRows<3>(ANGULAR).setZero(); return res; } }; // struct ConstraintTranslationSubspace ... ...
 ... ... @@ -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,18 +413,23 @@ 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 // CCRBA return quantities /// \brief Centroidal Momentum Matrix /// \note \f$hg = Ag \dot{q}\f$ maps the joint velocity set to the centroidal momentum. /// \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. /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame). /// Force hg; ... ...
 ... ... @@ -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 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // // This file is part of Pinocchio ... ... @@ -61,6 +61,7 @@ namespace se3 Force operator*(const Motion & v) const { return derived().__mult__(v); } Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); } Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); } void setZero() { derived().setZero(); } void setIdentity() { derived().setIdentity(); } ... ... @@ -119,6 +120,8 @@ namespace se3 SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl); EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; public: // Constructors InertiaTpl() : m(), c(), I() {} ... ... @@ -230,12 +233,12 @@ namespace se3 Matrix6 matrix_impl() const { Matrix6 M; const Matrix3 & c_cross = (skew(c)); M.template block<3,3>(LINEAR, LINEAR ).setZero (); M.template block<3,3>(LINEAR, LINEAR ).diagonal ().fill (m);