mode: add the notion of subtrees

Will be useful for certain algorithms dealing with second order derivatives

src/multibody/model.hpp

@@ -128,6 +128,11 @@ namespace pinocchio
     /// \brief Vector of operational frames registered on the model.
     FrameVector frames;
     
+    /// \brief Vector of supports.
+    /// supports[j] corresponds to the collection of all joints located on the path between body *j*  and the world.
+    /// The last element of supports[j] is the index of the joint *j* itself.
+    std::vector<IndexVector> supports;
+    
     /// \brief Vector of subtrees.
     /// subtree[j] corresponds to the subtree supported by the joint *j*.
     /// The first element of subtree[j] is the index of the joint *j* itself.
@@ -154,6 +159,7 @@ namespace pinocchio
     , joints(1)
     , parents(1, 0)
     , names(1)
+    , supports(1,IndexVector(1,0))
     , subtrees(1)
     , gravity(gravity981,Vector3::Zero())
     {

src/multibody/model.hxx

@@ -122,6 +122,11 @@ namespace pinocchio
     subtrees.push_back(IndexVector(1));
     subtrees[idx][0] = idx;
     addJointIndexToParentSubtrees(idx);
+    
+    // Init and add joint index to the supports
+    supports.push_back(supports[parent]);
+    supports[idx].push_back(idx);
+    
     return idx;
   }
 

unittest/model.cpp

@@ -43,6 +43,30 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
     }
   }
 
+  BOOST_AUTO_TEST_CASE(test_model_support)
+  {
+    Model model;
+    buildModels::humanoidRandom(model);
+    const Model::IndexVector support0_ref(1,0);
+    BOOST_CHECK(model.supports[0] == support0_ref);
+
+    // Check that i ends supports[i]
+    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+    {
+      BOOST_CHECK(model.supports[joint_id].back() == joint_id);
+      Model::IndexVector & support = model.supports[joint_id];
+      
+      size_t current_id = support.size()-2;
+      for(JointIndex parent_id = model.parents[joint_id];
+          parent_id > 0;
+          parent_id = model.parents[parent_id],
+          current_id--)
+      {
+        BOOST_CHECK(parent_id == support[current_id]);
+      }
+    }
+  }
+
   BOOST_AUTO_TEST_CASE(comparison)
   {
     Model model;