[algorithm] computeSubtreeMasses

bindings/python/algorithm/expose-com.cpp

@@ -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<double,0,JointCollectionDefaultTpl>,
+              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)"),

src/algorithm/center-of-mass.hpp

@@ -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<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
   inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                  DataTpl<Scalar,Options,JointCollectionTpl> & 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<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                   DataTpl<Scalar,Options,JointCollectionTpl> & 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).

src/algorithm/center-of-mass.hxx

@@ -32,6 +32,24 @@ namespace pinocchio
     return data.mass[0];
   }
 
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                   DataTpl<Scalar,Options,JointCollectionTpl> & 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<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
   inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
   centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,

src/multibody/data.hpp

@@ -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<Vector3> 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<Scalar> mass;
     
     /// \brief Jacobien of center of mass.

unittest/com.cpp

@@ -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;

unittest/python/bindings_com.py

@@ -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())