Allow to change the reference CoM Z posiiton via function or configur…

…ation.

.github/workflows/config/WalkingOnPlane.yaml

@@ -63,3 +63,10 @@ states:
         - footMidpose:
             translation: [1.34305922386, -0.409195670883, 0.0]
             rotation: [0.0, 0.0, -0.558996938995747]
+      refComZList:
+        - refComZ: 0.725
+          startTime: 8.0
+          interpDuration: 1.0
+        - refComZ: 0.825
+          startTime: 12.0
+          interpDuration: 1.0

etc/BaselineWalkingController.in.yaml

@@ -204,6 +204,7 @@ CentroidalManager:
   method: PreviewControlZmp
   horizonDuration: 2.0 # [sec]
   horizonDt: 0.005 # [sec]
+  reinitForRefComZ: true
 
   # # DdpZmp
   # method: DdpZmp
@@ -213,11 +214,13 @@ CentroidalManager:
 
   # # FootGuidedControl
   # method: FootGuidedControl
+  # reinitForRefComZ: true
 
   # # IntrinsicallyStableMpc
   # method: IntrinsicallyStableMpc
   # horizonDuration: 2.0 # [sec]
   # horizonDt: 0.02 # [sec]
+  # reinitForRefComZ: true
 
 
 # OverwriteConfigKeys: [NoSensors]

include/BaselineWalkingController/CentroidalManager.h

@@ -3,6 +3,8 @@
 #include <mc_rtc/gui/StateBuilder.h>
 #include <mc_rtc/log/Logger.h>
 
+#include <TrajColl/CubicInterpolator.h>
+
 #include <BaselineWalkingController/FootTypes.h>
 
 namespace mc_rbdyn
@@ -120,6 +122,14 @@ class CentroidalManager
   /** \brief Remove entries from the logger. */
   virtual void removeFromLogger(mc_rtc::Logger & logger);
 
+  /** \brief Set reference CoM Z position.
+      \param refComZ reference CoM Z position
+      \param startTime time to start interpolation
+      \param interpDuration duration to interpolate
+      \return whether refComZ is set correctly
+  */
+  bool setRefComZ(double refComZ, double startTime, double interpDuration);
+
   /** \brief Set anchor frame. */
   void setAnchorFrame();
 
@@ -148,6 +158,12 @@ class CentroidalManager
   /** \brief Whether to assume that CoM Z is constant. */
   virtual bool isConstantComZ() const = 0;
 
+  /** \brief Calculate reference CoM Z position.
+      \param t time
+      \param derivOrder derivative order (0 for original value, 1 for velocity)
+  */
+  double calcRefComZ(double t, int derivOrder = 0) const;
+
   /** \brief Calculate anchor frame.
       \param robot robot
    */
@@ -172,6 +188,9 @@ class CentroidalManager
   virtual Eigen::Vector3d calcPlannedComAccel() const;
 
 protected:
+  //! Maximum time of interpolation endpoint
+  const double interpMaxTime_ = 1e10;
+
   //! Pointer to controller
   BaselineWalkingController * ctlPtr_ = nullptr;
 
@@ -210,5 +229,8 @@ class CentroidalManager
 
   //! Contact list
   std::unordered_map<Foot, std::shared_ptr<ForceColl::Contact>> contactList_;
+
+  //! Interpolation function of reference CoM Z position
+  std::shared_ptr<TrajColl::CubicInterpolator<double>> refComZFunc_;
 };
 } // namespace BWC

include/BaselineWalkingController/centroidal/CentroidalManagerFootGuidedControl.h

@@ -17,6 +17,9 @@ class CentroidalManagerFootGuidedControl : public CentroidalManager
   /** \brief Configuration. */
   struct Configuration : public CentroidalManager::Configuration
   {
+    //! Whether to reinitialize MPC when reference CoM Z position is updated
+    bool reinitForRefComZ = true;
+
     /** \brief Load mc_rtc configuration. */
     virtual void load(const mc_rtc::Configuration & mcRtcConfig) override;
   };
@@ -72,5 +75,8 @@ class CentroidalManagerFootGuidedControl : public CentroidalManager
 
   //! Foot-guided control
   std::shared_ptr<CCC::FootGuidedControl> footGuided_;
+
+  //! Reference CoM Z position of the previous control step
+  double lastRefComZ_ = 0;
 };
 } // namespace BWC

include/BaselineWalkingController/centroidal/CentroidalManagerIntrinsicallyStableMpc.h

@@ -26,6 +26,9 @@ class CentroidalManagerIntrinsicallyStableMpc : public CentroidalManager
     //! QP solver type
     QpSolverCollection::QpSolverType qpSolverType = QpSolverCollection::QpSolverType::Any;
 
+    //! Whether to reinitialize MPC when reference CoM Z position is updated
+    bool reinitForRefComZ = true;
+
     /** \brief Load mc_rtc configuration. */
     virtual void load(const mc_rtc::Configuration & mcRtcConfig) override;
   };
@@ -84,5 +87,8 @@ class CentroidalManagerIntrinsicallyStableMpc : public CentroidalManager
 
   //! Whether it is the first iteration
   bool firstIter_ = true;
+
+  //! Reference CoM Z position of the previous control step
+  double lastRefComZ_ = 0;
 };
 } // namespace BWC

include/BaselineWalkingController/centroidal/CentroidalManagerPreviewControlZmp.h

@@ -23,6 +23,9 @@ class CentroidalManagerPreviewControlZmp : virtual public CentroidalManager
     //! Horizon dt [sec]
     double horizonDt = 0.005;
 
+    //! Whether to reinitialize MPC when reference CoM Z position is updated
+    bool reinitForRefComZ = true;
+
     /** \brief Load mc_rtc configuration. */
     virtual void load(const mc_rtc::Configuration & mcRtcConfig) override;
   };
@@ -78,5 +81,8 @@ class CentroidalManagerPreviewControlZmp : virtual public CentroidalManager
 
   //! Whether it is the first iteration
   bool firstIter_ = true;
+
+  //! Reference CoM Z position of the previous control step
+  double lastRefComZ_ = 0;
 };
 } // namespace BWC

src/CentroidalManager.cpp

@@ -37,13 +37,18 @@ void CentroidalManager::Configuration::load(const mc_rtc::Configuration & mcRtcC
 
 CentroidalManager::CentroidalManager(BaselineWalkingController * ctlPtr, const mc_rtc::Configuration & // mcRtcConfig
                                      )
-: ctlPtr_(ctlPtr)
+: ctlPtr_(ctlPtr), refComZFunc_(std::make_shared<TrajColl::CubicInterpolator<double>>())
 {
 }
 
 void CentroidalManager::reset()
 {
   robotMass_ = ctl().robot().mass();
+
+  refComZFunc_->clearPoints();
+  refComZFunc_->appendPoint(std::make_pair(ctl().t(), config().refComZ));
+  refComZFunc_->appendPoint(std::make_pair(interpMaxTime_, config().refComZ));
+  refComZFunc_->calcCoeff();
 }
 
 void CentroidalManager::update()
@@ -117,9 +122,9 @@ void CentroidalManager::update()
     Eigen::Vector3d nextPlannedComVel = mpcComVel_ + ctl().dt() * plannedComAccel;
     if(isConstantComZ())
     {
-      nextPlannedCom.z() = config().refComZ + ctl().footManager_->calcRefGroundPosZ(ctl().t());
-      nextPlannedComVel.z() = ctl().footManager_->calcRefGroundPosZ(ctl().t(), 1);
-      plannedComAccel.z() = ctl().footManager_->calcRefGroundPosZ(ctl().t(), 2);
+      nextPlannedCom.z() = calcRefComZ(ctl().t()) + ctl().footManager_->calcRefGroundPosZ(ctl().t());
+      nextPlannedComVel.z() = calcRefComZ(ctl().t(), 1) + ctl().footManager_->calcRefGroundPosZ(ctl().t(), 1);
+      plannedComAccel.z() = calcRefComZ(ctl().t(), 2) + ctl().footManager_->calcRefGroundPosZ(ctl().t(), 2);
     }
     ctl().comTask_->com(nextPlannedCom);
     ctl().comTask_->refVel(nextPlannedComVel);
@@ -197,8 +202,6 @@ void CentroidalManager::addToGUI(mc_rtc::gui::StateBuilder & gui)
           "comZGainP", [this]() { return config().comZGainP; }, [this](double v) { config().comZGainP = v; }),
       mc_rtc::gui::NumberInput(
           "comZGainD", [this]() { return config().comZGainD; }, [this](double v) { config().comZGainD = v; }),
-      mc_rtc::gui::NumberInput(
-          "refComZ", [this]() { return config().refComZ; }, [this](double v) { config().refComZ = v; }),
       mc_rtc::gui::Checkbox(
           "useTargetPoseForControlRobotAnchorFrame",
           [this]() { return config().useTargetPoseForControlRobotAnchorFrame; },
@@ -326,6 +329,30 @@ void CentroidalManager::removeFromLogger(mc_rtc::Logger & logger)
   logger.removeLogEntries(this);
 }
 
+bool CentroidalManager::setRefComZ(double refComZ, double startTime, double interpDuration)
+{
+  if(startTime < ctl().t())
+  {
+    mc_rtc::log::warning("[CentroidalManager] Ignore reference CoM Z position with past time: {} < {}", startTime,
+                         ctl().t());
+    return false;
+  }
+  auto it = std::next(refComZFunc_->points().rbegin());
+  if(startTime < it->first)
+  {
+    mc_rtc::log::warning("[CentroidalManager] Ignore reference CoM Z position with time before the existing "
+                         "interpolation points: {} < {}",
+                         startTime, it->first);
+    return false;
+  }
+
+  refComZFunc_->appendPoint(std::make_pair(startTime, it->second));
+  refComZFunc_->appendPoint(std::make_pair(startTime + interpDuration, refComZ));
+  refComZFunc_->calcCoeff();
+
+  return true;
+}
+
 void CentroidalManager::setAnchorFrame()
 {
   std::string anchorName = "KinematicAnchorFrame::" + ctl().robot().name();
@@ -336,6 +363,18 @@ void CentroidalManager::setAnchorFrame()
   ctl().datastore().make_call(anchorName, [this](const mc_rbdyn::Robot & robot) { return calcAnchorFrame(robot); });
 }
 
+double CentroidalManager::calcRefComZ(double t, int derivOrder) const
+{
+  if(derivOrder == 0)
+  {
+    return (*refComZFunc_)(t);
+  }
+  else
+  {
+    return refComZFunc_->derivative(t, derivOrder);
+  }
+}
+
 sva::PTransformd CentroidalManager::calcAnchorFrame(const mc_rbdyn::Robot & robot) const
 {
   double leftFootSupportRatio = ctl().footManager_->leftFootSupportRatio();

src/centroidal/CentroidalManagerDdpZmp.cpp

@@ -80,6 +80,6 @@ CCC::DdpZmp::RefData CentroidalManagerDdpZmp::calcRefData(double t) const
 {
   CCC::DdpZmp::RefData refData;
   refData.zmp = ctl().footManager_->calcRefZmp(t);
-  refData.com_z = config_.refComZ + ctl().footManager_->calcRefGroundPosZ(t);
+  refData.com_z = calcRefComZ(t) + ctl().footManager_->calcRefGroundPosZ(t);
   return refData;
 }

src/centroidal/CentroidalManagerFootGuidedControl.cpp

@@ -9,6 +9,8 @@ using namespace BWC;
 void CentroidalManagerFootGuidedControl::Configuration::load(const mc_rtc::Configuration & mcRtcConfig)
 {
   CentroidalManager::Configuration::load(mcRtcConfig);
+
+  mcRtcConfig("reinitForRefComZ", reinitForRefComZ);
 }
 
 CentroidalManagerFootGuidedControl::CentroidalManagerFootGuidedControl(BaselineWalkingController * ctlPtr,
@@ -23,21 +25,32 @@ void CentroidalManagerFootGuidedControl::reset()
   CentroidalManager::reset();
 
   footGuided_ = std::make_shared<CCC::FootGuidedControl>(config_.refComZ);
+  lastRefComZ_ = config_.refComZ;
 }
 
 void CentroidalManagerFootGuidedControl::addToLogger(mc_rtc::Logger & logger)
 {
   CentroidalManager::addToLogger(logger);
 
   logger.addLogEntry(config_.name + "_FootGuided_capturePoint", this, [this]() -> Eigen::Vector2d {
-    return mpcCom_.head<2>() + std::sqrt(config_.refComZ / CCC::constants::g) * mpcComVel_.head<2>();
+    return mpcCom_.head<2>() + std::sqrt(lastRefComZ_ / CCC::constants::g) * mpcComVel_.head<2>();
   });
 }
 
 void CentroidalManagerFootGuidedControl::runMpc()
 {
+  double refComZ = calcRefComZ(ctl().t());
+  if(refComZ != lastRefComZ_)
+  {
+    if(config_.reinitForRefComZ)
+    {
+      footGuided_ = std::make_shared<CCC::FootGuidedControl>(refComZ);
+    }
+    lastRefComZ_ = refComZ;
+  }
+
   CCC::FootGuidedControl::InitialParam initialParam =
-      mpcCom_.head<2>() + std::sqrt(config_.refComZ / CCC::constants::g) * mpcComVel_.head<2>();
+      mpcCom_.head<2>() + std::sqrt(refComZ / CCC::constants::g) * mpcComVel_.head<2>();
   CCC::FootGuidedControl::RefData refData = calcRefData();
 
   Eigen::Vector2d plannedData = footGuided_->planOnce(refData, initialParam, ctl().t());

src/centroidal/CentroidalManagerIntrinsicallyStableMpc.cpp

@@ -19,6 +19,7 @@ void CentroidalManagerIntrinsicallyStableMpc::Configuration::load(const mc_rtc::
   {
     qpSolverType = QpSolverCollection::strToQpSolverType(mcRtcConfig("qpSolverType"));
   }
+  mcRtcConfig("reinitForRefComZ", reinitForRefComZ);
 }
 
 CentroidalManagerIntrinsicallyStableMpc::CentroidalManagerIntrinsicallyStableMpc(
@@ -35,7 +36,7 @@ void CentroidalManagerIntrinsicallyStableMpc::reset()
 
   mpc_ = std::make_shared<CCC::IntrinsicallyStableMpc>(config_.refComZ, config_.horizonDuration, config_.horizonDt,
                                                        config_.qpSolverType);
-
+  lastRefComZ_ = config_.refComZ;
   firstIter_ = true;
 }
 
@@ -51,9 +52,19 @@ void CentroidalManagerIntrinsicallyStableMpc::addToLogger(mc_rtc::Logger & logge
 
 void CentroidalManagerIntrinsicallyStableMpc::runMpc()
 {
+  double refComZ = calcRefComZ(ctl().t());
+  if(refComZ != lastRefComZ_)
+  {
+    if(config_.reinitForRefComZ)
+    {
+      mpc_ = std::make_shared<CCC::IntrinsicallyStableMpc>(refComZ, config_.horizonDuration, config_.horizonDt,
+                                                           config_.qpSolverType);
+    }
+    lastRefComZ_ = refComZ;
+  }
+
   CCC::IntrinsicallyStableMpc::InitialParam initialParam;
-  initialParam.capture_point =
-      mpcCom_.head<2>() + std::sqrt(config_.refComZ / CCC::constants::g) * mpcComVel_.head<2>();
+  initialParam.capture_point = mpcCom_.head<2>() + std::sqrt(refComZ / CCC::constants::g) * mpcComVel_.head<2>();
   if(firstIter_)
   {
     initialParam.planned_zmp = mpcCom_.head<2>();

src/centroidal/CentroidalManagerPreviewControlZmp.cpp

@@ -14,6 +14,7 @@ void CentroidalManagerPreviewControlZmp::Configuration::load(const mc_rtc::Confi
 
   mcRtcConfig("horizonDuration", horizonDuration);
   mcRtcConfig("horizonDt", horizonDt);
+  mcRtcConfig("reinitForRefComZ", reinitForRefComZ);
 }
 
 CentroidalManagerPreviewControlZmp::CentroidalManagerPreviewControlZmp(BaselineWalkingController * ctlPtr,
@@ -28,12 +29,22 @@ void CentroidalManagerPreviewControlZmp::reset()
   CentroidalManager::reset();
 
   pc_ = std::make_shared<CCC::PreviewControlZmp>(config_.refComZ, config_.horizonDuration, config_.horizonDt);
-
+  lastRefComZ_ = config_.refComZ;
   firstIter_ = true;
 }
 
 void CentroidalManagerPreviewControlZmp::runMpc()
 {
+  double refComZ = calcRefComZ(ctl().t());
+  if(refComZ != lastRefComZ_)
+  {
+    if(config_.reinitForRefComZ)
+    {
+      pc_ = std::make_shared<CCC::PreviewControlZmp>(refComZ, config_.horizonDuration, config_.horizonDt);
+    }
+    lastRefComZ_ = refComZ;
+  }
+
   CCC::PreviewControlZmp::InitialParam initialParam;
   initialParam.pos = mpcCom_.head<2>();
   initialParam.vel = mpcComVel_.head<2>();
@@ -44,7 +55,7 @@ void CentroidalManagerPreviewControlZmp::runMpc()
   else
   {
     // Since the actual CoM acceleration cannot be obtained, the CoM acceleration is always calculated from LIPM dynamics
-    initialParam.acc = CCC::constants::g / config_.refComZ * (mpcCom_ - plannedZmp_).head<2>();
+    initialParam.acc = CCC::constants::g / refComZ * (mpcCom_ - plannedZmp_).head<2>();
   }
 
   Eigen::Vector2d plannedData =

src/states/ConfigWalkState.cpp

@@ -1,4 +1,5 @@
 #include <BaselineWalkingController/BaselineWalkingController.h>
+#include <BaselineWalkingController/CentroidalManager.h>
 #include <BaselineWalkingController/FootManager.h>
 #include <BaselineWalkingController/states/ConfigWalkState.h>
 
@@ -39,6 +40,17 @@ void ConfigWalkState::start(mc_control::fsm::Controller & _ctl)
     velModeEndTime_ = ctl().t() + static_cast<double>(config_("configs")("velocityMode")("duration"));
   }
 
+  // Set reference CoM Z position
+  if(config_.has("configs") && config_("configs").has("refComZList"))
+  {
+    for(const auto & refComZConfig : config_("configs")("refComZList"))
+    {
+      ctl().centroidalManager_->setRefComZ(refComZConfig("refComZ"),
+                                           ctl().t() + static_cast<double>(refComZConfig("startTime")),
+                                           refComZConfig("interpDuration"));
+    }
+  }
+
   output("OK");
 }