[C++] Add subtree info

src/multibody/model.hpp

@@ -101,6 +101,7 @@ namespace se3
     
     /// \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.
     std::vector<IndexVector> subtrees;
 
     /// \brief Spatial gravity of the model.
@@ -121,6 +122,7 @@ namespace se3
       , joints(1)
       , parents(1)
       , names(1)
+      , subtrees(1)
       , gravity( gravity981,Eigen::Vector3d::Zero() )
     {
       names[0]     = "universe";     // Should be "universe joint (trivial)"
@@ -340,6 +342,13 @@ namespace se3
     ///
     bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_FRAME);
 
+  protected:
+    
+    /// \brief Add the joint_id to its parent subtrees.
+    ///
+    /// \param[in] joint_id The id of the joint to add to the subtrees
+    ///
+    void addJointIndexToParentSubtrees(const JointIndex joint_id);
   };
 
   class Data

src/multibody/model.hxx

@@ -83,6 +83,10 @@ namespace se3
     // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
     addFrame(names[idx],idx,SE3::Identity(),JOINT);
     
+    // Init and add joint index to its parent subtrees.
+    subtrees.push_back(IndexVector(1));
+    subtrees[idx][0] = idx;
+    addJointIndexToParentSubtrees(idx);
     return idx;
   }
 
@@ -225,6 +229,12 @@ namespace se3
     else
       return false;
   }
+  
+  inline void Model::addJointIndexToParentSubtrees(const JointIndex joint_id)
+  {
+    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
+      subtrees[parent].push_back(joint_id);
+  }
 
 
   inline Data::Data (const Model & ref)
@@ -244,7 +254,7 @@ namespace se3
     ,ddq(ref.nv)
     ,Yaba((std::size_t)ref.nbody)
     ,u(ref.nv)
-    ,Ag(6, ref.nv)
+    ,Ag(6,ref.nv)
     ,Fcrb((std::size_t)ref.nbody)
     ,lastChild((std::size_t)ref.nbody)
     ,nvSubtree((std::size_t)ref.nbody)
@@ -282,7 +292,8 @@ namespace se3
     computeParents_fromRow(ref);
 
     /* Init Jacobian */
-    J.fill(0);
+    J.setZero();
+    Ag.setZero();
     
     /* Init universe states relatively to itself */