Add interrupt state

include/MultiContactController/states/InterruptState.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <TrajColl/CubicInterpolator.h>
+
+#include <MultiContactController/State.h>
+
+namespace MCC
+{
+/** \brief FSM state to initialize. */
+struct InterruptState : State
+{
+public:
+  /** \brief Start. */
+  void start(mc_control::fsm::Controller & ctl) override;
+
+  /** \brief Run. */
+  bool run(mc_control::fsm::Controller & ctl) override;
+
+  /** \brief Teardown. */
+  void teardown(mc_control::fsm::Controller & ctl) override;
+
+protected:
+  /** \brief Check whether state is completed. */
+  bool complete() const;
+
+protected:
+  //! Phase
+  int phase_ = 0;
+
+  //! BaseTime for automatic start
+  double baseTime_ = 0.0;
+};
+} // namespace MCC

src/states/CMakeLists.txt

@@ -6,6 +6,10 @@ add_fsm_state(ConfigMotionState ConfigMotionState.cpp)
 target_link_libraries(ConfigMotionState PUBLIC
   ${CONTROLLER_NAME})
 
+add_fsm_state(InterruptState InterruptState.cpp)
+target_link_libraries(InterruptState PUBLIC
+  ${CONTROLLER_NAME})
+
 add_fsm_state(GuiWalkState GuiWalkState.cpp)
 target_link_libraries(GuiWalkState PUBLIC
   ${CONTROLLER_NAME})

src/states/InterruptState.cpp

@@ -0,0 +1,66 @@
+#include <mc_rtc/gui/Button.h>
+#include <MultiContactController/MultiContactController.h>
+#include <MultiContactController/states/InterruptState.h>
+
+using namespace MCC;
+
+void InterruptState::start(mc_control::fsm::Controller & _ctl)
+{
+  State::start(_ctl);
+
+  phase_ = 0;
+
+  baseTime_ = 0.0;
+  if(config_.has("configs") && config_("configs").has("baseTime"))
+  {
+    if(config_("configs")("baseTime") == "Relative")
+    {
+      baseTime_ = ctl().t();
+    }
+    else
+    {
+      baseTime_ = static_cast<double>(config_("configs")("baseTime"));
+    }
+  }
+
+  // Setup GUI
+  ctl().gui()->addElement({ctl().name()}, mc_rtc::gui::Button("Start", [this]() { phase_ = 1; }));
+
+  output("OK");
+}
+
+bool InterruptState::run(mc_control::fsm::Controller &)
+{
+  if(phase_ == 0)
+  {
+    // Auto start
+    if(config_.has("configs") && config_("configs").has("autoStartTime")
+       && ctl().t() - baseTime_ > static_cast<double>(config_("configs")("autoStartTime")))
+    {
+      phase_ = 1;
+    }
+  }
+  if(phase_ == 1)
+  {
+    phase_ = 2;
+
+    // Clean up GUI
+    ctl().gui()->removeElement({ctl().name()}, "Start");
+
+    // Start updating references
+    // NOTE: enableManagerUpdate_ is not explicitly disabled by InterruptState
+    //       for feedback control to maintain balance while stopping in CentroidalManager
+    ctl().enableManagerUpdate_ = true;
+  }
+
+  return complete();
+}
+
+void InterruptState::teardown(mc_control::fsm::Controller &) {}
+
+bool InterruptState::complete() const
+{
+  return phase_ == 2;
+}
+
+EXPORT_SINGLE_STATE("MCC::Interrupt", InterruptState)