Check limb swing motion has been finished at the end of ConfigMotion if exitWhenLimbSwingFinished is true

include/MultiContactController/LimbManagerSet.h

@@ -101,6 +101,9 @@ class LimbManagerSet : public std::unordered_map<Limb, std::shared_ptr<LimbManag
   /** \brief Get whether future contact command is stacked. */
   bool contactCommandStacked() const;
 
+  /** \brief Get whether any limbs are executing swing motion. */
+  bool isExecutingLimbSwing() const;
+
   /** \brief Get limbs of the specified group.
       \param group limb group
    */

include/MultiContactController/states/ConfigMotionState.h

@@ -25,5 +25,8 @@ struct ConfigMotionState : State
 
   //! Task configuration list
   std::multimap<double, mc_rtc::Configuration> taskConfigList_;
+
+  //! Option to select whether this state should wait for finishing swing motion or not
+  bool exitWhenLimbSwingFinished_ = false;
 };
 } // namespace MCC

src/LimbManagerSet.cpp

@@ -114,6 +114,19 @@ bool LimbManagerSet::contactCommandStacked() const
   return false;
 }
 
+bool LimbManagerSet::isExecutingLimbSwing() const
+{
+  for(const auto & limbManagerKV : *this)
+  {
+    const auto & contactCommandList = limbManagerKV.second->contactCommandList();
+    if(limbManagerKV.second->currentSwingCommand_ != nullptr)
+    {
+      return true;
+    }
+  }
+  return false;
+}
+
 void LimbManagerSet::addToGUI(mc_rtc::gui::StateBuilder & gui)
 {
   for(const auto & limbManagerKV : *this)

src/states/ConfigMotionState.cpp

@@ -89,6 +89,12 @@ void ConfigMotionState::start(mc_control::fsm::Controller & _ctl)
     }
   }
 
+  // Set option to wait for finishing swing motion
+  if(config_.has("configs") && config_("configs").has("exitWhenLimbSwingFinished"))
+  {
+    exitWhenLimbSwingFinished_ = static_cast<bool>(config_("configs")("exitWhenLimbSwingFinished"));
+  }
+
   output("OK");
 }
 
@@ -161,7 +167,8 @@ bool ConfigMotionState::run(mc_control::fsm::Controller &)
     }
   }
 
-  return !ctl().limbManagerSet_->contactCommandStacked() && taskConfigList_.empty() && collisionConfigList_.empty();
+  return !ctl().limbManagerSet_->contactCommandStacked() && taskConfigList_.empty() && collisionConfigList_.empty()
+         && (!exitWhenLimbSwingFinished_ || !ctl().limbManagerSet_->isExecutingLimbSwing());
 }
 
 void ConfigMotionState::teardown(mc_control::fsm::Controller &) {}