Allow to stop object motion from GUI.

isri-aist · Nov 23, 2023 · 98019d2 · 98019d2
1 parent 655fd61
commit 98019d2
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 2 deletions.
diff --git a/.github/workflows/config/PushCartWaypoint.yaml b/.github/workflows/config/PushCartWaypoint.yaml
@@ -1,5 +1,5 @@
 transitions:
-  - [LMC::Initial_, OK, LMC::ConfigManip_, Auto]
+  - [LMC::Initial_, OK, LMC::Main_, Auto]
 
 states:
   LMC::Initial_:
@@ -24,6 +24,10 @@ states:
           relPose:
             translation: [-1.0, 0.0, 0.0]
 
+  LMC::Main_:
+    base: Parallel
+    states: [LMC::GuiManip_, LMC::ConfigManip_]
+
 ManipManager:
   objPoseTopic: /cnoid/object/pose
   objVelTopic: /cnoid/object/vel
diff --git a/include/LocomanipController/ManipManager.h b/include/LocomanipController/ManipManager.h
@@ -241,6 +241,12 @@ class ManipManager
   */
   bool appendWaypoint(const Waypoint & newWaypoint);
 
+  /** \brief Clear waypoint queue.
+
+      The object is stopped at the timing when the ongoing foot swing ends.
+  */
+  void clearWaypointQueue();
+
   /** \brief Reach hand to object. */
   void reachHandToObj();
 

diff --git a/src/ManipManager.cpp b/src/ManipManager.cpp
@@ -338,6 +338,26 @@ bool ManipManager::appendWaypoint(const Waypoint & newWaypoint)
   return true;
 }
 
+void ManipManager::clearWaypointQueue()
+{
+  ctl().footManager_->clearFootstepQueue();
+  const auto & footstepQueue = ctl().footManager_->footstepQueue();
+  double stopTime;
+  if(footstepQueue.empty())
+  {
+    constexpr double stopDuration = 0.2; // [sec]
+    stopTime = ctl().t() + stopDuration;
+  }
+  else
+  {
+    stopTime = footstepQueue.back().transitEndTime;
+  }
+  waypointQueue_.clear();
+  // \todo Avoid discontinuous changes in object velocity
+  waypointQueue_.push_back(Waypoint(ctl().t(), stopTime, calcRefObjPose(stopTime)));
+  lastWaypointPose_ = calcRefObjPose(ctl().t());
+}
+
 void ManipManager::reachHandToObj()
 {
   for(const auto & hand : Hands::Both)

diff --git a/src/states/GuiManipState.cpp b/src/states/GuiManipState.cpp
@@ -18,7 +18,8 @@ void GuiManipState::start(mc_control::fsm::Controller & _ctl)
   // Setup GUI
   ctl().gui()->addElement({ctl().name(), "GuiManip"},
                           mc_rtc::gui::Button("Reach", [this]() { ctl().manipManager_->reachHandToObj(); }),
-                          mc_rtc::gui::Button("Release", [this]() { ctl().manipManager_->releaseHandFromObj(); }));
+                          mc_rtc::gui::Button("Release", [this]() { ctl().manipManager_->releaseHandFromObj(); }),
+                          mc_rtc::gui::Button("StopManip", [this]() { ctl().manipManager_->clearWaypointQueue(); }));
   ctl().gui()->addElement(
       {ctl().name(), "GuiManip", "WalkToObj"},
       mc_rtc::gui::Form(