Skip to content

Commit

Permalink
Allow to introduce the actual CoM offset.
Browse files Browse the repository at this point in the history 
Similar to isri-aist/BaselineWalkingController@6349ec5
  • Loading branch information
@mmurooka
mmurooka committed May 23, 2024
1 parent df3e032 commit 77153c7
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 38 deletions.
1 change: 1 addition & 0 deletions etc/MultiContactController.in.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
6 changes: 6 additions & 0 deletions include/MultiContactController/CentroidalManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down
86 changes: 48 additions & 38 deletions src/CentroidalManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -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)
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -333,43 +335,46 @@ void CentroidalManager::addToGUI(mc_rtc::gui::StateBuilder & gui)
"plannedCentroidalPose", centroidMarkerSize,
[this]() -> const sva::PTransformd & { return controlData_.plannedCentroidalPose; },
mc_rtc::gui::Color(0.0, 1.0, 0.0, 0.8)));
gui.addElement({ctl().name(), config().name, "Config"},
mc_rtc::gui::Label("method", [this]() -> const std::string & { return config().method; }),
mc_rtc::gui::ComboInput(
"nominalCentroidalPoseBaseFrame", {"LimbAveragePose", "World"},
[this]() -> const std::string & { return config().nominalCentroidalPoseBaseFrame; },
[this](const std::string & v) { config().nominalCentroidalPoseBaseFrame = v; }),
mc_rtc::gui::ComboInput(
"refComZPolicy", {"Average", "Constant", "Min", "Max"},
[this]() -> const std::string & { return config().refComZPolicy; },
[this](const std::string & v) { config().refComZPolicy = v; }),
mc_rtc::gui::ArrayInput(
"Centroidal P-Gain", {"ax", "ay", "az", "lx", "ly", "lz"},
[this]() -> const sva::ImpedanceVecd & { return config().centroidalGainP; },
[this](const Eigen::Vector6d & v) { config().centroidalGainP = sva::ImpedanceVecd(v); }),
mc_rtc::gui::ArrayInput(
"Centroidal D-Gain", {"ax", "ay", "az", "lx", "ly", "lz"},
[this]() -> const sva::ImpedanceVecd & { return config().centroidalGainD; },
[this](const Eigen::Vector6d & v) { config().centroidalGainD = sva::ImpedanceVecd(v); }),
mc_rtc::gui::NumberInput(
"lowPassCutoffPeriod", [this]() { return config().lowPassCutoffPeriod; },
[this](double v) { config().lowPassCutoffPeriod = v; }),
mc_rtc::gui::Checkbox(
"useActualStateForMpc", [this]() { return config().useActualStateForMpc; },
[this]() { config().useActualStateForMpc = !config().useActualStateForMpc; }),
mc_rtc::gui::Checkbox(
"enableCentroidalFeedback", [this]() { return config().enableCentroidalFeedback; },
[this]() { config().enableCentroidalFeedback = !config().enableCentroidalFeedback; }),
mc_rtc::gui::Checkbox(
"useTargetPoseForControlRobotAnchorFrame",
[this]() { return config().useTargetPoseForControlRobotAnchorFrame; },
[this]() {
config().useTargetPoseForControlRobotAnchorFrame =
!config().useTargetPoseForControlRobotAnchorFrame;
}),
mc_rtc::gui::Checkbox(
"useActualComForWrenchDist", [this]() { return config().useActualComForWrenchDist; },
[this]() { config().useActualComForWrenchDist = !config().useActualComForWrenchDist; }));
gui.addElement(
{ctl().name(), config().name, "Config"},
mc_rtc::gui::Label("method", [this]() -> const std::string & { return config().method; }),
mc_rtc::gui::ComboInput(
"nominalCentroidalPoseBaseFrame", {"LimbAveragePose", "World"},
[this]() -> const std::string & { return config().nominalCentroidalPoseBaseFrame; },
[this](const std::string & v) { config().nominalCentroidalPoseBaseFrame = v; }),
mc_rtc::gui::ComboInput(
"refComZPolicy", {"Average", "Constant", "Min", "Max"},
[this]() -> const std::string & { return config().refComZPolicy; },
[this](const std::string & v) { config().refComZPolicy = v; }),
mc_rtc::gui::ArrayInput(
"Centroidal P-Gain", {"ax", "ay", "az", "lx", "ly", "lz"},
[this]() -> const sva::ImpedanceVecd & { return config().centroidalGainP; },
[this](const Eigen::Vector6d & v) { config().centroidalGainP = sva::ImpedanceVecd(v); }),
mc_rtc::gui::ArrayInput(
"Centroidal D-Gain", {"ax", "ay", "az", "lx", "ly", "lz"},
[this]() -> const sva::ImpedanceVecd & { return config().centroidalGainD; },
[this](const Eigen::Vector6d & v) { config().centroidalGainD = sva::ImpedanceVecd(v); }),
mc_rtc::gui::NumberInput(
"lowPassCutoffPeriod", [this]() { return config().lowPassCutoffPeriod; },
[this](double v) { config().lowPassCutoffPeriod = v; }),
mc_rtc::gui::Checkbox(
"useActualStateForMpc", [this]() { return config().useActualStateForMpc; },
[this]() { config().useActualStateForMpc = !config().useActualStateForMpc; }),
mc_rtc::gui::Checkbox(
"enableCentroidalFeedback", [this]() { return config().enableCentroidalFeedback; },
[this]() { config().enableCentroidalFeedback = !config().enableCentroidalFeedback; }),
mc_rtc::gui::Checkbox(
"useTargetPoseForControlRobotAnchorFrame",
[this]() { return config().useTargetPoseForControlRobotAnchorFrame; },
[this]() {
config().useTargetPoseForControlRobotAnchorFrame = !config().useTargetPoseForControlRobotAnchorFrame;
}),
mc_rtc::gui::Checkbox(
"useActualComForWrenchDist", [this]() { return 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)
Expand Down Expand Up @@ -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;
}

0 comments on commit 77153c7

Please sign in to comment.