From 88da86642a25ea21ccc8887edfc86e02a4fce5b4 Mon Sep 17 00:00:00 2001
From: Masaki Murooka <m-murooka@aist.go.jp>
Date: Fri, 3 Nov 2023 22:25:20 +0900
Subject: [PATCH] Check changes in refComZ in a robust way.

---
 src/centroidal/CentroidalManagerPreviewControlZmp.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/centroidal/CentroidalManagerPreviewControlZmp.cpp b/src/centroidal/CentroidalManagerPreviewControlZmp.cpp
index 6005b6f2..5843a1c8 100644
--- 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)
     {