Check changes in refComZ in a robust way.

isri-aist · Nov 3, 2023 · 88da866 · 88da866
1 parent c80b062
commit 88da866
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/src/centroidal/CentroidalManagerPreviewControlZmp.cpp b/src/centroidal/CentroidalManagerPreviewControlZmp.cpp
@@ -36,7 +36,8 @@ void CentroidalManagerPreviewControlZmp::reset()
 void CentroidalManagerPreviewControlZmp::runMpc()
 {
   double refComZ = calcRefComZ(ctl().t());
-  if(refComZ != lastRefComZ_)
+  constexpr double threRefComZ = 1e-3; // [m]
+  if(std::abs(refComZ - lastRefComZ_) > threRefComZ)
   {
     if(config_.reinitForRefComZ)
     {