data: add starting_subspace_idx_fromRow field

src/multibody/data.hpp

@@ -233,6 +233,9 @@ namespace pinocchio
     
     /// \brief Dimension of the subtree motion space (for CRBA)
     std::vector<int> nvSubtree;
+    
+    /// \brief Starting index of the Joint motion subspace
+    std::vector<int> starting_subspace_idx_fromRow;
 
     /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
     MatrixXs U;

src/multibody/data.hxx

@@ -63,6 +63,7 @@ namespace pinocchio
   , Fcrb((std::size_t)model.njoints)
   , lastChild((std::size_t)model.njoints)
   , nvSubtree((std::size_t)model.njoints)
+  , starting_subspace_idx_fromRow((std::size_t)model.nv)
   , U(model.nv,model.nv)
   , D(model.nv)
   , Dinv(model.nv)
@@ -170,14 +171,16 @@ namespace pinocchio
       
       assert(idx_vj >= 0 && idx_vj < model.nv);
       
-      if(parent>0) parents_fromRow[(Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
-      else         parents_fromRow[(Index)idx_vj] = -1;
-      nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint];
+      if(parent>0) parents_fromRow[(size_t)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
+      else         parents_fromRow[(size_t)idx_vj] = -1;
+      nvSubtree_fromRow[(size_t)idx_vj] = nvSubtree[joint];
       
+      starting_subspace_idx_fromRow[(size_t)idx_vj] = idx_vj;
       for(int row=1;row<nvj;++row)
       {
-        parents_fromRow[(Index)(idx_vj+row)] = idx_vj+row-1;
-        nvSubtree_fromRow[(Index)(idx_vj+row)] = nvSubtree[joint]-row;
+        parents_fromRow[(size_t)(idx_vj+row)] = idx_vj+row-1;
+        nvSubtree_fromRow[(size_t)(idx_vj+row)] = nvSubtree[joint]-row;
+        starting_subspace_idx_fromRow[(size_t)(idx_vj+row)] = idx_vj;
       }
     }
   }

unittest/data.cpp

@@ -13,6 +13,24 @@ using namespace pinocchio;
 
 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
 
+
+  BOOST_AUTO_TEST_CASE(test_data_starting_subspace_idx_fromRow)
+   {
+     Model model;
+     buildModels::humanoidRandom(model);
+     
+     Data data(model);
+     
+     for(Model::JointIndex joint_id = 1; joint_id < (Model::JointIndex)model.njoints; ++joint_id)
+     {
+       const int nv_joint = model.joints[joint_id].nv();
+       const int idx_joint = model.joints[joint_id].idx_v();
+       
+       for(int k = 0; k < nv_joint; ++k)
+         BOOST_CHECK(data.starting_subspace_idx_fromRow[(size_t)(idx_joint+k)] == idx_joint);
+     }
+   }
+
   BOOST_AUTO_TEST_CASE(test_data_supports_fromRow)
   {
     Model model;