Merge pull request #411 from arntanguy/topic/FixInTheAir

include/mc_tasks/lipm_stabilizer/StabilizerTask.h

@@ -834,6 +834,7 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask
    * @param solver Solver to which this task has been added
    */
   void configure_(mc_solver::QPSolver & solver);
+  void disable_();
 
   /** Ensures that the configuration is valid */
   void checkConfiguration(const StabilizerConfiguration & config);
@@ -953,6 +954,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] */

src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp

@@ -274,11 +274,18 @@ void StabilizerTask::enable()
   configure(lastConfig_);
   zmpcc_.enabled(true);
   enabled_ = true;
+  wasEnabled_ = true;
 }
 
 void StabilizerTask::disable()
 {
   mc_rtc::log::info("[StabilizerTask] disabled");
+  disable_();
+  wasEnabled_ = false;
+}
+
+void StabilizerTask::disable_()
+{
   // Save current configuration to be reused when re-enabling
   lastConfig_ = c_;
   disableConfig_ = c_;
@@ -523,6 +530,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()