[StabilizerTask] Fix disabling when the robot is in the air

arntanguy · Nov 24, 2023 · 5be33b6 · 5be33b6
1 parent 3763fef
commit 5be33b6
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 0 deletions.
diff --git a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h
@@ -953,6 +953,8 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask
   mc_filter::ExponentialMovingAverage<Eigen::Vector3d> dcmIntegrator_;
   mc_filter::LowPassCompose<Eigen::Vector3d> dcmDerivator_;
   bool inTheAir_ = false; /**< Is the robot in the air? */
+  bool wasInTheAir_ = false; /**< Whether the robot was in the air at the previous iteration */
+  bool wasEnabled_ = true; /**< Whether the stabilizer was enabled before the robot was in the air */
   Eigen::Vector3d dfForceError_ = Eigen::Vector3d::Zero(); /**< Force error in foot force difference control */
   Eigen::Vector3d dfError_ = Eigen::Vector3d::Zero(); /**< Height error in foot force difference control */
   double dt_ = 0.005; /**< Controller cycle in [s] */

diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp
@@ -274,6 +274,7 @@ void StabilizerTask::enable()
   configure(lastConfig_);
   zmpcc_.enabled(true);
   enabled_ = true;
+  wasEnabled_ = true;
 }
 
 void StabilizerTask::disable()
@@ -523,6 +524,25 @@ void StabilizerTask::checkInTheAir()
   {
     inTheAir_ = inTheAir_ && footT.second->measuredWrench().force().z() < c_.safetyThresholds.MIN_DS_PRESSURE;
   }
+
+  if(!wasInTheAir_ && inTheAir_)
+  {
+    wasInTheAir_ = true;
+    if(enabled_)
+    {
+      mc_rtc::log::warning("[{}] Robot is in the air, disabling stabilizer", name());
+      disable();
+    }
+  }
+  else if(!inTheAir_ && wasInTheAir_)
+  {
+    wasInTheAir_ = false;
+    if(wasEnabled_)
+    {
+      mc_rtc::log::warning("[{}] Robot is no longer in the air, re-enabling stabilizer", name());
+      enable();
+    }
+  }
 }
 
 void StabilizerTask::computeLeftFootRatio()