diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 4703478822d857fd809cbc90f4fbf9a5feb07c7c..5021022a8b32ba0d63e3ff14f60b9d71826964e5 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -719,8 +719,9 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
       i);
   }
 
-  /// \brief rnea
-  /// -----------
+  /// \brief rnea, calculation of the multi body ZMP
+  /// ----------------------------------------------
+  double _1_mg = 1/(RobotMass_*9.81);
   for (unsigned int i = 0 ; i < N ; i++ ){
     // Apply the RNEA to the metapod multibody and print the result in a log file.
     for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++ )
@@ -732,19 +733,15 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
     metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
     getTorques(m_robot, m_torques);
     m_allTorques[i] = m_torques ;
-  }
 
-  /// \brief Projection of the Torques on the ground, the result is the ZMP Multi-Body
-  /// --------------------------------------------------------------------------------
-  double _1_mg = 1/(RobotMass_*9.81);
-//  m_deltaZMPMBPositions[0].px = 0.0 ;
-//  m_deltaZMPMBPositions[0].py = 0.0 ;
-//  m_deltaZMPMBPositions[0].pz = 0.0 ;
-//  m_deltaZMPMBPositions[0].theta = 0.0 ;
-//  m_deltaZMPMBPositions[0].time = m_CurrentTime ;
-//  m_deltaZMPMBPositions[0].stepType = ZMPPositions[0].stepType ;
-  for(unsigned int i = 0; i <  N ; i++ )
-  {
+    Node & node = boost::fusion::at_c<0>(Robot_Model.nodes);
+    metapod::Spatial::Force aforce = node.joint.f ;
+
+
+
+
+
+
     m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - m_allTorques[i](4,0) * _1_mg + FinalCOMTraj_deq[currentIndex+i].x[0] ) ;
     m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( m_allTorques[i](3,0) * _1_mg + FinalCOMTraj_deq[currentIndex+i].y[0] ) ;
     m_deltaZMPMBPositions[i].pz = 0.0 ;
@@ -753,6 +750,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
     m_deltaZMPMBPositions[i].stepType = ZMPPositions[currentIndex+i].stepType ;
   }
 
+
   /// \brief Debug Purpose
   /// --------------------
   ofstream aof5;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 3334c5f938b47d2dc541f1fe23ed519e4ed2d5f0..88e606ce00537d92b5494b7a8c9d3bc7abdff686 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -49,6 +49,7 @@
 #define METAPOD_TYPEDEF
 typedef double LocalFloatType;
 typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+typedef metapod::Nodes< Robot_Model, 0 >::type Node;
 #endif
 
 namespace PatternGeneratorJRL