[dcm-controller] [dcm-com-controller] set ZMP height to zero

src/dcm-com-controller.cpp

@@ -138,10 +138,11 @@ namespace dynamicgraph
 
         Vector dcmError = dcmDes - dcm;
 
-        m_dcmIntegralError += ( dcmError - decayFactor*m_dcmIntegralError ) * m_dt;
-
         Vector ddcomRef = ddcomDes + omega * Kp.cwiseProduct(dcmError) + omega * Ki.cwiseProduct(m_dcmIntegralError);
 
+        // update the integrator (AFTER using its value)
+        m_dcmIntegralError += ( dcmError - decayFactor*m_dcmIntegralError ) * m_dt;
+
         s = ddcomRef;
 
         getProfiler().stop(PROFILE_DCMCOMCONTROLLER_DDCOMREF_COMPUTATION);
@@ -167,6 +168,7 @@ namespace dynamicgraph
         assert(comDes.size()==3 && "Unexpected size of signal comDes");
 
         Vector zmpRef = comDes - ddcomRef/(omega*omega);
+        zmpRef[2] = 0.0; // maybe needs better way
 
         s = zmpRef;
 

src/dcm-controller.cpp

@@ -138,6 +138,7 @@ namespace dynamicgraph
         Vector dcmError = dcmDes - dcm;
 
         Vector zmpRef = zmpDes - (Vector::Constant(3,1,1.0) + Kp/omega).cwiseProduct(dcmError) - Ki.cwiseProduct(m_dcmIntegralError)/omega;
+        zmpRef[2] = 0.0; // maybe needs better way
 
         // update the integrator (AFTER using its value)
         m_dcmIntegralError += ( dcmError - decayFactor*m_dcmIntegralError ) * m_dt;