[Naveau2015] Fix a problem with the orientation.

src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp

@@ -53,9 +53,9 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
     FootAbsolutePosition &LeftFootPosition,
     FootAbsolutePosition &RightFootPosition, Eigen::VectorXd &ZMPRefPos,
     COMState &finalCOMPosition,
-    Eigen::VectorXd &, // CurrentConfiguration,
-    Eigen::VectorXd &, // CurrentVelocity,
-    Eigen::VectorXd &) // CurrentAcceleration)
+    Eigen::VectorXd & CurrentConfiguration,
+    Eigen::VectorXd & CurrentVelocity,
+    Eigen::VectorXd & CurrentAcceleration)
 {
   ODEBUG("Begin OneGlobalStepOfControl "
          << m_LeftFootPositions->size() << " " << m_RightFootPositions->size()
@@ -87,6 +87,18 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(
     return -4;
   }
 
+  CurrentConfiguration[3] = finalCOMPosition.roll[0];
+  CurrentConfiguration[4] = finalCOMPosition.pitch[0];
+  CurrentConfiguration[5] = finalCOMPosition.yaw[0];
+
+  CurrentVelocity[3] = finalCOMPosition.roll[1];
+  CurrentVelocity[4] = finalCOMPosition.pitch[1];
+  CurrentVelocity[5] = finalCOMPosition.yaw[1];
+
+  CurrentAcceleration[3] = finalCOMPosition.roll[2];
+  CurrentAcceleration[4] = finalCOMPosition.pitch[2];
+  CurrentAcceleration[5] = finalCOMPosition.yaw[2];
+
   if (m_ZMPPositions->size() > 0) {
     ZMPPosition aZMPPosition = (*m_ZMPPositions)[0];
     ZMPRefPos(0) = aZMPPosition.px;