Commit f52ee642 by Gabriele Buondonno

### [algorithm] computeSubtreeMasses

parent aa58d690
 ... ... @@ -57,6 +57,11 @@ namespace pinocchio bp::args("Model", "Data"), "Compute the total mass of the model, put it in data.mass[0] and return it."); bp::def("computeSubtreeMasses", (void (*)(const Model &, Data &))&computeSubtreeMasses, bp::args("Model", "Data"), "Compute the mass of each kinematic subtree and store it in data.mass."); bp::def("centerOfMass",com_0_proxy, bp::args("Model","Data", "Joint configuration q (size Model::nq)"), ... ...
 ... ... @@ -25,12 +25,23 @@ namespace pinocchio /// \param[in] data The data structure of the rigid body system. /// /// \warning This method does not fill the whole data.mass vector. Only data.mass[0] is updated. /// If you need the whole data.mass vector to be computed, use computeSubtreeMasses /// /// \return Total mass of the model. template class JointCollectionTpl> inline Scalar computeTotalMass(const ModelTpl & model, DataTpl & data); /// \brief Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds to the total mass of the model. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// /// \note If you are only interested in knowing the total mass of the model, computeTotalMass will probably be slightly faster. template class JointCollectionTpl> inline void computeSubtreeMasses(const ModelTpl & model, DataTpl & data); /// /// \brief Computes the center of mass position of a given model according to a particular joint configuration. /// The result is accessible through data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame). ... ...
 ... ... @@ -32,6 +32,24 @@ namespace pinocchio return data.mass[0]; } template class JointCollectionTpl> inline void computeSubtreeMasses(const ModelTpl & model, DataTpl & data) { // Forward Step for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) { data.mass[i] = model.inertias[i].mass(); } // Backward Step for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) { const JointIndex & parent = model.parents[i]; data.mass[parent] += data.mass[i]; } } template class JointCollectionTpl, typename ConfigVectorType> inline const typename DataTpl::Vector3 & centerOfMass(const ModelTpl & model, ... ...
 ... ... @@ -280,7 +280,7 @@ namespace pinocchio /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f\$ j \f\$ and expressed in the joint frame \f\$ j \f\$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame. container::aligned_vector acom; /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f\$ j \f\$. The element mass[0] corrresponds to the total mass of the model. /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f\$ j \f\$. The element mass[0] corresponds to the total mass of the model. std::vector mass; /// \brief Jacobien of center of mass. ... ...
 ... ... @@ -110,6 +110,29 @@ BOOST_AUTO_TEST_CASE ( test_mass ) BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12); } BOOST_AUTO_TEST_CASE ( test_subtree_masses ) { using namespace Eigen; using namespace pinocchio; pinocchio::Model model; pinocchio::buildModels::humanoidRandom(model); pinocchio::Data data1(model); computeSubtreeMasses(model,data1); pinocchio::Data data2(model); VectorXd q = VectorXd::Ones(model.nq); q.middleRows<4> (3).normalize(); centerOfMass(model,data2,q); for(size_t i=0; i<(size_t)(model.njoints);++i) { BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12); } } //BOOST_AUTO_TEST_CASE ( test_timings ) //{ // using namespace Eigen; ... ...
 ... ... @@ -36,6 +36,15 @@ class TestComBindings(TestCase): pin.centerOfMass(self.model,data2,self.q) self.assertApprox(mass,data2.mass[0]) def test_subtree_masses(self): pin.computeSubtreeMasses(self.model,self.data) data2 = self.model.createData() pin.centerOfMass(self.model,data2,self.q) for i in range(self.model.njoints): self.assertApprox(self.data.mass[i],data2.mass[i]) def test_Jcom_update3(self): Jcom = pin.jacobianCenterOfMass(self.model,self.data,self.q) self.assertFalse(np.isnan(Jcom).any()) ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!