From bcee681fbfe71dd1156878eaaadfa72d71e741b6 Mon Sep 17 00:00:00 2001
From: Iori Kumagai <iori-kumagai@aist.go.jp>
Date: Thu, 19 Oct 2023 12:47:51 +0900
Subject: [PATCH 1/3] Check limb swing motion has been finished at the end of
 ConfigMotion if exitWhenLimbSwingFinished is true

---
 include/MultiContactController/LimbManagerSet.h    |  3 +++
 .../states/ConfigMotionState.h                     |  3 +++
 src/LimbManagerSet.cpp                             | 13 +++++++++++++
 src/states/ConfigMotionState.cpp                   | 14 +++++++++++++-
 4 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/include/MultiContactController/LimbManagerSet.h b/include/MultiContactController/LimbManagerSet.h
index 24fd7ce..8d6ce9f 100644
--- a/include/MultiContactController/LimbManagerSet.h
+++ b/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
    */
diff --git a/include/MultiContactController/states/ConfigMotionState.h b/include/MultiContactController/states/ConfigMotionState.h
index 46bdf72..628404b 100644
--- a/include/MultiContactController/states/ConfigMotionState.h
+++ b/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
diff --git a/src/LimbManagerSet.cpp b/src/LimbManagerSet.cpp
index a6d52e2..6fb3c8e 100644
--- a/src/LimbManagerSet.cpp
+++ b/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)
diff --git a/src/states/ConfigMotionState.cpp b/src/states/ConfigMotionState.cpp
index 712f67d..cb40edd 100644
--- a/src/states/ConfigMotionState.cpp
+++ b/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,13 @@ bool ConfigMotionState::run(mc_control::fsm::Controller &)
     }
   }
 
-  return !ctl().limbManagerSet_->contactCommandStacked() && taskConfigList_.empty() && collisionConfigList_.empty();
+  bool limbCofingsFinshed = !ctl().limbManagerSet_->contactCommandStacked();
+  if(exitWhenLimbSwingFinished_)
+  {
+    limbCofingsFinshed = limbCofingsFinshed && !ctl().limbManagerSet_->isExecutingLimbSwing();
+  }
+
+  return limbCofingsFinshed && taskConfigList_.empty() && collisionConfigList_.empty();
 }
 
 void ConfigMotionState::teardown(mc_control::fsm::Controller &) {}

From 1f8a24865bb8e6c6051ba7cd18c164eb80f12f3e Mon Sep 17 00:00:00 2001
From: Iori Kumagai <iori-kumagai@aist.go.jp>
Date: Mon, 23 Oct 2023 11:49:07 +0900
Subject: [PATCH 2/3] Refactor the check for finishing limb motion

---
 src/states/ConfigMotionState.cpp | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/states/ConfigMotionState.cpp b/src/states/ConfigMotionState.cpp
index cb40edd..97bc5d0 100644
--- a/src/states/ConfigMotionState.cpp
+++ b/src/states/ConfigMotionState.cpp
@@ -167,13 +167,8 @@ bool ConfigMotionState::run(mc_control::fsm::Controller &)
     }
   }
 
-  bool limbCofingsFinshed = !ctl().limbManagerSet_->contactCommandStacked();
-  if(exitWhenLimbSwingFinished_)
-  {
-    limbCofingsFinshed = limbCofingsFinshed && !ctl().limbManagerSet_->isExecutingLimbSwing();
-  }
-
-  return limbCofingsFinshed && taskConfigList_.empty() && collisionConfigList_.empty();
+  return !ctl().limbManagerSet_->contactCommandStacked() && taskConfigList_.empty() && collisionConfigList_.empty()
+    && (!exitWhenLimbSwingFinished_ || !ctl().limbManagerSet_->isExecutingLimbSwing());
 }
 
 void ConfigMotionState::teardown(mc_control::fsm::Controller &) {}

From 1d06a9defb93bd1ee12de48809f7fab82457f1c7 Mon Sep 17 00:00:00 2001
From: Iori Kumagai <iori-kumagai@aist.go.jp>
Date: Fri, 27 Oct 2023 12:34:07 +0900
Subject: [PATCH 3/3] Fix clang-format

---
 src/states/ConfigMotionState.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/states/ConfigMotionState.cpp b/src/states/ConfigMotionState.cpp
index 97bc5d0..fb27149 100644
--- a/src/states/ConfigMotionState.cpp
+++ b/src/states/ConfigMotionState.cpp
@@ -168,7 +168,7 @@ bool ConfigMotionState::run(mc_control::fsm::Controller &)
   }
 
   return !ctl().limbManagerSet_->contactCommandStacked() && taskConfigList_.empty() && collisionConfigList_.empty()
-    && (!exitWhenLimbSwingFinished_ || !ctl().limbManagerSet_->isExecutingLimbSwing());
+         && (!exitWhenLimbSwingFinished_ || !ctl().limbManagerSet_->isExecutingLimbSwing());
 }
 
 void ConfigMotionState::teardown(mc_control::fsm::Controller &) {}