Put PRs for multi-contact locomotion together for easy merge

include/MultiContactController/CentroidalManager.h

@@ -186,6 +186,20 @@ class CentroidalManager
    */
   CentroidalManager(MultiContactController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig = {});
 
+  /** \brief Reset.
+      \param constraintSetConfig mc_rtc configuration that has nominalCentroidalPose
+
+      This method should be called once when controller is reset.
+      The validity of nominalCentroidalPoseConfig should be checked before calling this function.
+
+      An example of \p nominalCentroidalPoseConfig is as follows.
+      @code
+      nominalCentroidalPose:
+        translation: [0.0, 0.0, 0.966]
+      @endcode
+  */
+  virtual void reset(const mc_rtc::Configuration & nominalCentroidalPoseConfig);
+
   /** \brief Reset.
 
       This method should be called once when controller is reset.
@@ -229,6 +243,11 @@ class CentroidalManager
   /** \brief Set anchor frame. */
   void setAnchorFrame();
 
+  /** \brief Check whether reference CoM trajectory is completed at given time
+      \param t time
+  */
+  bool isFinished(const double t) const;
+
 protected:
   /** \brief Const accessor to the controller. */
   inline const MultiContactController & ctl() const

include/MultiContactController/LimbManager.h

@@ -258,5 +258,8 @@ class LimbManager
 
   //! Whether to require updating impedance gains for limb tasks
   bool requireImpGainUpdate_ = true;
+
+  //! Whether to require updating target pose for contact constraint
+  bool requireTouchDownPoseUpdate_ = false;
 };
 } // namespace MCC

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/MultiContactController.h

@@ -20,17 +20,26 @@ namespace MCC
 {
 class LimbManagerSet;
 class CentroidalManager;
+class PostureManager;
 
 /** \brief Humanoid multi-contact motion controller. */
 struct MultiContactController : public mc_control::fsm::Controller
 {
 public:
-  /** \brief Constructor. */
+  /** \brief Constructor.
+
+      If the pose is stored in the "basePose" key of the controller configuration, the pose will be registered in the
+     "MCC::ResetBasePose" key of the datastore and applied to the baselink poses of the control and real robots in the
+     reset function.
+   */
   MultiContactController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & _config);
 
   /** \brief Reset a controller.
 
       This method is called when starting the controller.
+
+      If the pose is registered in the "MCC::ResetBasePose" key of the datastore, the pose will be applied to the
+     baselink poses of the control and real robots in the reset function.
    */
   void reset(const mc_control::ControllerResetData & resetData) override;
 
@@ -43,6 +52,10 @@ struct MultiContactController : public mc_control::fsm::Controller
   /** \brief Stop a controller.
 
       This method is called when stopping the controller.
+
+       If the "saveLastBasePose" key in the controller configuration is true, the baselink pose of the current control
+     robot is registered in the "MCC::ResetBasePose" key of the datastore. This is intended to be used in subsequent
+     controller initializations.
    */
   void stop() override;
 
@@ -86,9 +99,15 @@ struct MultiContactController : public mc_control::fsm::Controller
   //! Centroidal manager
   std::shared_ptr<CentroidalManager> centroidalManager_;
 
+  //! Posture manager
+  std::shared_ptr<PostureManager> postureManager_;
+
   //! Whether to enable manager update
   bool enableManagerUpdate_ = false;
 
+  //! Whether to save last base pose when stopping this controller.
+  bool saveLastBasePose_ = false;
+
 protected:
   //! Controller name
   std::string name_ = "MCC";

include/MultiContactController/PostureManager.h

@@ -0,0 +1,122 @@
+#pragma once
+
+#include <mc_rtc/gui/StateBuilder.h>
+#include <mc_rtc/log/Logger.h>
+#include <mc_tasks/PostureTask.h>
+
+namespace mc_rbdyn
+{
+class Robot;
+}
+
+namespace MCC
+{
+class MultiContactController;
+
+/** \brief Posture manager.
+
+    Posture manager calculates the target of robot posture.
+ */
+class PostureManager
+{
+public:
+  typedef std::map<std::string, std::vector<double>> PostureMap;
+
+  /** \brief Configuration. */
+  struct Configuration
+  {
+    //! Name
+    std::string name = "PostureManager";
+
+    /** \brief Load mc_rtc configuration. */
+    virtual void load(const mc_rtc::Configuration & mcRtcConfig);
+  };
+
+public:
+  /** \brief Constructor.
+      \param ctlPtr pointer to controller
+      \param mcRtcConfig mc_rtc configuration
+   */
+  PostureManager(MultiContactController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig = {});
+
+  /** \brief Reset.
+      \param initialPosture map of joint names and angles for the initial posture
+
+      This method should be called once when controller is reset.
+   */
+  virtual void reset(const PostureManager::PostureMap & initialPosture = {});
+
+  /** \brief Update.
+
+      This method should be called once every control cycle.
+   */
+  virtual void update();
+
+  /** \brief Stop.
+
+      This method should be called once when stopping the controller.
+  */
+  virtual void stop();
+
+  /** \brief Add entries to the GUI. */
+  virtual void addToGUI(mc_rtc::gui::StateBuilder & gui);
+
+  /** \brief Remove entries from the GUI. */
+  virtual void removeFromGUI(mc_rtc::gui::StateBuilder & gui);
+
+  /** \brief Add entries to the logger. */
+  virtual void addToLogger(mc_rtc::Logger & logger);
+
+  /** \brief Remove entries from the logger. */
+  virtual void removeFromLogger(mc_rtc::Logger & logger);
+
+  /** \brief Append a nominal postures of the robot
+      \param t time
+      \param PostureMap map from joint names to joint angle [rad]
+      \return whether nominalCentroidalPose is appended
+  */
+  virtual bool appendNominalPosture(double t, const PostureMap & nominalPosture);
+
+  /** \brief Get nominal posture.
+      \param t time
+  */
+  virtual PostureMap getNominalPosture(double t) const;
+
+  /** \brief Check whether reference postures are completed at given time
+      \param t time
+  */
+  virtual bool isFinished(double t) const;
+
+protected:
+  /** \brief Const accessor to the controller. */
+  inline const MultiContactController & ctl() const
+  {
+    return *ctlPtr_;
+  }
+
+  /** \brief Accessor to the controller. */
+  inline MultiContactController & ctl()
+  {
+    return *ctlPtr_;
+  }
+
+  /** \brief Const accessor to the configuration. */
+  inline virtual const Configuration & config() const
+  {
+    return config_;
+  }
+
+protected:
+  //! Configuration
+  Configuration config_;
+
+  //! Pointer to controller
+  MultiContactController * ctlPtr_ = nullptr;
+
+  //! Pointer to posture task in controller
+  std::shared_ptr<mc_tasks::PostureTask> postureTask_;
+
+  //! Nominal posture list
+  std::map<double, PostureMap> nominalPostureList_;
+};
+} // namespace MCC

include/MultiContactController/states/ConfigMotionState.h

@@ -25,5 +25,14 @@ 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;
+
+  //! Option to select whether this state should wait for finishing reference CoM trajectory or not
+  bool exitWhenCentroidalManagerFinished_ = false;
+
+  //! Option to select whether this state should wait for finishing reference postures or not
+  bool exitWhenPostureManagerFinished_ = false;
 };
 } // namespace MCC

include/MultiContactController/states/InterruptState.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#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/CMakeLists.txt

@@ -9,6 +9,7 @@ add_library(${CONTROLLER_NAME} SHARED
   LimbManager.cpp
   LimbManagerSet.cpp
   CentroidalManager.cpp
+  PostureManager.cpp
   swing/SwingTrajCubicSplineSimple.cpp
   centroidal/CentroidalManagerDDP.cpp
   centroidal/CentroidalManagerPC.cpp

src/CentroidalManager.cpp

@@ -145,6 +145,12 @@ CentroidalManager::CentroidalManager(MultiContactController * ctlPtr, const mc_r
 {
 }
 
+void CentroidalManager::reset(const mc_rtc::Configuration & nominalCentroidalPoseConfig)
+{
+  config().nominalCentroidalPose = static_cast<sva::PTransformd>(nominalCentroidalPoseConfig);
+  reset();
+}
+
 void CentroidalManager::reset()
 {
   refData_.reset();
@@ -426,6 +432,15 @@ void CentroidalManager::setAnchorFrame()
   ctl().datastore().make_call(anchorName, [this](const mc_rbdyn::Robot & robot) { return calcAnchorFrame(robot); });
 }
 
+bool CentroidalManager::isFinished(const double t) const
+{
+  if(nominalCentroidalPoseList_.empty())
+  {
+    return true;
+  }
+  return t > nominalCentroidalPoseList_.rbegin()->first;
+}
+
 CentroidalManager::RefData CentroidalManager::calcRefData(double t) const
 {
   RefData refData;

src/LimbManager.cpp

@@ -66,6 +66,8 @@ void LimbManager::reset(const mc_rtc::Configuration & _constraintConfig)
     impGainType_ = "Uninitialized";
 
     requireImpGainUpdate_ = false;
+
+    requireTouchDownPoseUpdate_ = false;
   }
 
   contactCommandList_.clear();
@@ -144,6 +146,7 @@ void LimbManager::update()
       {
         targetPose_ = swingTraj_->endPose_;
       }
+
       targetVel_ = sva::MotionVecd::Zero();
       targetAccel_ = sva::MotionVecd::Zero();
 
@@ -244,6 +247,11 @@ void LimbManager::update()
     {
       touchDown_ = true;
 
+      if(config_.keepPoseForTouchDownLimb)
+      {
+        requireTouchDownPoseUpdate_ = true;
+      }
+
       if(config_.stopSwingTrajForTouchDownLimb)
       {
         swingTraj_->touchDown(ctl().t());
@@ -274,6 +282,16 @@ void LimbManager::update()
 
   // Update currentContactCommand_ (this should be after setting swingTraj_)
   currentContactCommand_ = getContactCommand(ctl().t());
+  if(currentContactCommand_ && currentContactCommand_->constraint->type() != "Empty" && requireTouchDownPoseUpdate_)
+  {
+    // \todo In order to correctly update global vertices when transitioning from an EmptyContact with no vertices for
+    // wrench distribution to, for example, a GraspContact with vertices for wrench distribution, the condition here is
+    // skipped for EmptyContact and requireTouchDownPoseUpdate_ is retained. The current implementation works well for
+    // contact transitions where the hand grasps the environment (i.e., EmptyContact -> GraspContact), but it does not
+    // work well to perform other contact transitions in the future (e.g., SurfaceContact -> GraspContact).
+    currentContactCommand_->constraint->updateGlobalVertices(targetPose_);
+    requireTouchDownPoseUpdate_ = false;
+  }
 
   // Update phase_
   if(currentSwingCommand_)

src/LimbManagerSet.cpp

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