data: add supports_fromRow field

src/multibody/data.hpp

@@ -230,6 +230,7 @@ namespace pinocchio
 
     /// \brief Index of the last child (for CRBA)
     std::vector<int> lastChild;
+    
     /// \brief Dimension of the subtree motion space (for CRBA)
     std::vector<int> nvSubtree;
 
@@ -248,6 +249,10 @@ namespace pinocchio
     /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
     std::vector<int> parents_fromRow;
     
+    /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tree of the
+    ///        given index at the row level. It may be helpful to retrieve the sparsity pattern through it.
+    std::vector< std::vector<int> > supports_fromRow;
+    
     /// \brief Subtree of the current row index (used in Cholesky Decomposition).
     std::vector<int> nvSubtree_fromRow;
     
@@ -347,6 +352,7 @@ namespace pinocchio
   private:
     void computeLastChild(const Model & model);
     void computeParents_fromRow(const Model & model);
+    void computeSupports_fromRow(const Model & model);
 
   };
 

src/multibody/data.hxx

@@ -68,6 +68,7 @@ namespace pinocchio
   , Dinv(model.nv)
   , tmp(model.nv)
   , parents_fromRow((std::size_t)model.nv)
+  , supports_fromRow((std::size_t)model.nv)
   , nvSubtree_fromRow((std::size_t)model.nv)
   , J(6,model.nv)
   , dJ(6,model.nv)
@@ -106,13 +107,14 @@ namespace pinocchio
     { Fcrb[i].resize(6,model.nv); }
     
     computeLastChild(model);
-    
+
     /* Init for Coriolis */
     C.setZero();
 
     /* Init for Cholesky */
     U.setIdentity();
     computeParents_fromRow(model);
+    computeSupports_fromRow(model);
 
     /* Init Jacobian */
     J.setZero();
@@ -180,6 +182,36 @@ namespace pinocchio
     }
   }
 
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline void DataTpl<Scalar,Options,JointCollectionTpl>
+  ::computeSupports_fromRow(const Model & model)
+  {
+    typedef typename Model::JointIndex JointIndex;
+    
+    for(JointIndex joint_id = 1;
+        joint_id < (JointIndex)(model.njoints);
+        joint_id++)
+    {
+      const int nvj    = nv   (model.joints[joint_id]);
+      const int idx_vj = idx_v(model.joints[joint_id]);
+      
+      assert(idx_vj >= 0 && idx_vj < model.nv);
+      
+      const int parent_fromRow = parents_fromRow[(size_t)idx_vj];
+      
+      if(parent_fromRow >= 0)
+        supports_fromRow[(size_t)idx_vj] = supports_fromRow[(size_t)parent_fromRow];
+      
+      supports_fromRow[(size_t)idx_vj].push_back(idx_vj);
+      
+      for(int row = 1; row < nvj; ++row)
+      {
+        supports_fromRow[(size_t)(idx_vj+row)] = supports_fromRow[(size_t)(idx_vj+row-1)];
+        supports_fromRow[(size_t)(idx_vj+row)].push_back(idx_vj+row);
+      }
+    }
+  }
+
 } // namespace pinocchio
 
 /// @endcond

unittest/CMakeLists.txt

@@ -96,6 +96,7 @@ IF(BUILD_PYTHON_INTERFACE)
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 ADD_PINOCCHIO_UNIT_TEST(model)
+ADD_PINOCCHIO_UNIT_TEST(data)
 ADD_PINOCCHIO_UNIT_TEST(constraint)
 ADD_PINOCCHIO_UNIT_TEST(joints)
 ADD_PINOCCHIO_UNIT_TEST(compute-all-terms)

unittest/data.cpp

+//
+// Copyright (c) 2019 INRIA
+//
+
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+using namespace pinocchio;
+
+BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
+
+  BOOST_AUTO_TEST_CASE(test_data_supports_fromRow)
+  {
+    Model model;
+    buildModels::humanoidRandom(model);
+    
+    Data data(model);
+    
+    for(size_t k = 0; k < (size_t)model.nv; ++k)
+    {
+      const std::vector<int> & support = data.supports_fromRow[k];
+      const int parent_id = data.parents_fromRow[k];
+      
+      if(parent_id >= 0)
+      {
+        const std::vector<int> & support_parent = data.supports_fromRow[(size_t)parent_id];
+        BOOST_CHECK(support.size() == support_parent.size()+1);
+        for(size_t j = 0; j < support_parent.size(); ++j)
+        {
+          BOOST_CHECK(support[j] == support_parent[j]);
+        }
+      }
+      
+      BOOST_CHECK(support.back() == (int)k);
+    }
+  }
+
+BOOST_AUTO_TEST_SUITE_END()