[Algo] Add a simple algo to update the global placement of the joints when...

[Algo] Add a simple algo to update the global placement of the joints when only relative placement are computed

src/algorithm/kinematics.hpp

@@ -33,6 +33,18 @@ namespace se3
   inline void emptyForwardPass(const Model & model,
                                Data & data);
   
+  ///
+  /// \brief Update the global placement of the joints oMi according to the relative
+  ///        placements of the joints.
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  ///
+  /// \remark This algorithm may be useful to call to update global joint placement
+  ///         after calling se3::rnea, se3::aba, etc for example.
+  ///
+  inline void updateGlobalPlacements(const Model & model, Data & data);
+  
   ///
   /// \brief Update the joint placement according to the current joint configuration.
   ///

src/algorithm/kinematics.hxx

@@ -53,6 +53,19 @@ namespace se3
     }
   }
   
+  inline void updateGlobalPlacements(const Model & model, Data & data)
+  {
+    for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
+    {
+      const Model::JointIndex & parent = model.parents[i];
+      
+      if (parent>0)
+        data.oMi[i] = data.oMi[parent] * data.liMi[i];
+      else
+        data.oMi[i] = data.liMi[i];
+    }
+  }
+  
   struct ForwardKinematicZeroStep : public fusion::JointVisitor<ForwardKinematicZeroStep>
   {
     typedef boost::fusion::vector<const se3::Model &,