Allow to introduce the actual CoM offset.

Similar to isri-aist/BaselineWalkingController@6349ec5
isri-aist · May 23, 2024 · ee61d99 · ee61d99
1 parent df3e032
commit ee61d99
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git a/etc/MultiContactController.in.yaml b/etc/MultiContactController.in.yaml
@@ -193,6 +193,7 @@ CentroidalManager:
   enableCentroidalFeedback: true
   useTargetPoseForControlRobotAnchorFrame: true
   useActualComForWrenchDist: false
+  actualComOffset: [0.0, 0.0, 0.0]
   wrenchDistConfig:
     wrenchWeight:
       linear: [1.0, 1.0, 1.0]

diff --git a/include/MultiContactController/CentroidalManager.h b/include/MultiContactController/CentroidalManager.h
@@ -73,6 +73,9 @@ class CentroidalManager
     //! Whether to use actual CoM for wrench distribution
     bool useActualComForWrenchDist = false;
 
+    //! Actual CoM offset
+    Eigen::Vector3d actualComOffset = Eigen::Vector3d::Zero();
+
     //! Configuration for wrench distribution
     mc_rtc::Configuration wrenchDistConfig;
 
@@ -295,6 +298,9 @@ class CentroidalManager
    */
   sva::PTransformd calcAnchorFrame(const mc_rbdyn::Robot & robot) const;
 
+  /** \brief Get actual CoM. */
+  Eigen::Vector3d actualCom() const;
+
 protected:
   //! Pointer to controller
   MultiContactController * ctlPtr_ = nullptr;

diff --git a/src/CentroidalManager.cpp b/src/CentroidalManager.cpp
@@ -59,6 +59,7 @@ void CentroidalManager::Configuration::load(const mc_rtc::Configuration & mcRtcC
   mcRtcConfig("enableCentroidalFeedback", enableCentroidalFeedback);
   mcRtcConfig("useTargetPoseForControlRobotAnchorFrame", useTargetPoseForControlRobotAnchorFrame);
   mcRtcConfig("useActualComForWrenchDist", useActualComForWrenchDist);
+  mcRtcConfig("actualComOffset", actualComOffset);
   mcRtcConfig("wrenchDistConfig", wrenchDistConfig);
 }
 
@@ -75,6 +76,7 @@ void CentroidalManager::Configuration::addToLogger(const std::string & baseEntry
   MC_RTC_LOG_HELPER(baseEntry + "_enableCentroidalFeedback", enableCentroidalFeedback);
   MC_RTC_LOG_HELPER(baseEntry + "_useTargetPoseForControlRobotAnchorFrame", useTargetPoseForControlRobotAnchorFrame);
   MC_RTC_LOG_HELPER(baseEntry + "_useActualComForWrenchDist", useActualComForWrenchDist);
+  MC_RTC_LOG_HELPER(baseEntry + "_actualComOffset", actualComOffset);
 }
 
 void CentroidalManager::Configuration::removeFromLogger(mc_rtc::Logger & logger)
@@ -181,7 +183,7 @@ void CentroidalManager::update()
   refData_ = calcRefData(ctl().t());
   {
     const auto & baseOriLinkName = ctl().baseOriTask_->frame_->body();
-    controlData_.actualCentroidalPose.translation() = ctl().realRobot().com();
+    controlData_.actualCentroidalPose.translation() = actualCom();
     controlData_.actualCentroidalPose.rotation() = ctl().realRobot().bodyPosW(baseOriLinkName).rotation();
     if(lowPass_.cutoffPeriod() != config().lowPassCutoffPeriod)
     {
@@ -369,7 +371,10 @@ void CentroidalManager::addToGUI(mc_rtc::gui::StateBuilder & gui)
                      }),
                  mc_rtc::gui::Checkbox(
                      "useActualComForWrenchDist", [this]() { return config().useActualComForWrenchDist; },
-                     [this]() { config().useActualComForWrenchDist = !config().useActualComForWrenchDist; }));
+                     [this]() { config().useActualComForWrenchDist = !config().useActualComForWrenchDist; }),
+                 mc_rtc::gui::ArrayInput(
+                     "actualComOffset", {"x", "y", "z"}, [this]() -> const Eigen::Vector3d & { return config().actualComOffset; },
+                     [this](const Eigen::Vector3d & v) { config().actualComOffset = v; }));
 }
 
 void CentroidalManager::removeFromGUI(mc_rtc::gui::StateBuilder & gui)
@@ -611,3 +616,8 @@ sva::PTransformd CentroidalManager::calcAnchorFrame(const mc_rbdyn::Robot & robo
   }
   return calcWeightedAveragePose(weightPoseList);
 }
+
+Eigen::Vector3d CentroidalManager::actualCom() const
+{
+  return ctl().realRobot().com() + config().actualComOffset;
+}