data: add field end_idx_v_fromRow

src/multibody/data.hpp

@@ -236,6 +236,9 @@ namespace pinocchio
     
     /// \brief Starting index of the Joint motion subspace
     std::vector<int> start_idx_v_fromRow;
+    
+    /// \brief End index of the Joint motion subspace
+    std::vector<int> end_idx_v_fromRow;
 
     /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
     MatrixXs U;

src/multibody/data.hxx

@@ -64,6 +64,7 @@ namespace pinocchio
   , lastChild((std::size_t)model.njoints)
   , nvSubtree((std::size_t)model.njoints)
   , start_idx_v_fromRow((std::size_t)model.nv)
+  , end_idx_v_fromRow((std::size_t)model.nv)
   , U(model.nv,model.nv)
   , D(model.nv)
   , Dinv(model.nv)
@@ -176,11 +177,13 @@ namespace pinocchio
       nvSubtree_fromRow[(size_t)idx_vj] = nvSubtree[joint];
       
       start_idx_v_fromRow[(size_t)idx_vj] = idx_vj;
+      end_idx_v_fromRow[(size_t)idx_vj] = idx_vj+nvj-1;
       for(int row=1;row<nvj;++row)
       {
         parents_fromRow[(size_t)(idx_vj+row)] = idx_vj+row-1;
         nvSubtree_fromRow[(size_t)(idx_vj+row)] = nvSubtree[joint]-row;
         start_idx_v_fromRow[(size_t)(idx_vj+row)] = start_idx_v_fromRow[(size_t)idx_vj];
+        end_idx_v_fromRow[(size_t)(idx_vj+row)] = end_idx_v_fromRow[(size_t)idx_vj];
       }
     }
   }

unittest/data.cpp

@@ -27,7 +27,10 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
        const int idx_joint = model.joints[joint_id].idx_v();
        
        for(int k = 0; k < nv_joint; ++k)
+       {
          BOOST_CHECK(data.start_idx_v_fromRow[(size_t)(idx_joint+k)] == idx_joint);
+         BOOST_CHECK(data.end_idx_v_fromRow[(size_t)(idx_joint+k)] == idx_joint+nv_joint-1);
+       }
      }
    }