Skip to content

Commit

Permalink
Merge pull request #27 from mmurooka/dcm-bias-estimator
Browse files Browse the repository at this point in the history 
Integrate DCM Estimator
  • Loading branch information
@mmurooka
mmurooka authored May 20, 2024
2 parents 674fc41 + c233a62 commit 87f2f52
Show file tree
Hide file tree
Showing 5 changed files with 210 additions and 17 deletions.
4 changes: 4 additions & 0 deletions .github/workflows/config/WalkingOnPlaneWaypoint.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,3 +15,7 @@ states:
waypointList:
- [0.5, 0.0, 0.0]
- [0.5, 0.3, 0.0]

CentroidalManager:
dcmEstimator:
enableDcmEstimator: true
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ https://user-images.githubusercontent.com/6636600/184788709-fcb55fa8-fd93-4be3-b

## Features
- Completely open source! (controller framework: mc_rtc, simulator: Choreonoid, sample robot model: JVRC1)
- Full capabilities, including 3D walking and integration with a footstep planner.
- Full capabilities, including 3D walking, mass property error compensation of robot model, and integration with a footstep planner.
- Easy to switch between various methods of centroidal trajectory generation for walking implemented in [CentroidalControlCollection](https://github.com/isri-aist/CentroidalControlCollection).
- Easy to switch between the two frameworks for centroidal trajectory generation for walking: (1) closed-loop MPC and (2) open-loop MPC + stabilizer.
- Support for a virtual robot whose model is publicly available so you can try out the controller right away.
Expand Down
9 changes: 9 additions & 0 deletions etc/BaselineWalkingController.in.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -199,6 +199,15 @@ CentroidalManager:
angular: [1.0, 1.0, 1.0]
regularWeight: 1e-8
ridgeForceMinMax: [3, 1000] # [N]
dcmEstimator:
enableDcmEstimator: false
dcmCorrectionMode: Bias # "Bias", "Filter", or "None"
biasDriftPerSecondStd: 0.002
dcmMeasureErrorStd: 0.01
zmpMeasureErrorStd: 0.005
biasLimit: [0.03, 0.03]
initDcmUncertainty: [0.01, 0.01]
initBiasUncertainty: [0.01, 0.01]

# PreviewControlZmp
method: PreviewControlZmp
Expand Down
51 changes: 49 additions & 2 deletions include/BaselineWalkingController/CentroidalManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include <mc_rtc/gui/StateBuilder.h>
#include <mc_rtc/log/Logger.h>

#include <state-observation/dynamics-estimators/lipm-dcm-estimator.hpp>

#include <TrajColl/CubicInterpolator.h>

#include <BaselineWalkingController/FootTypes.h>
Expand Down Expand Up @@ -30,6 +32,39 @@ class BaselineWalkingController;
class CentroidalManager
{
public:
/** \brief Configuration for DCM estimator. */
struct DcmEstimatorConfiguration
{
//! Whether to enable DCM estimator
bool enableDcmEstimator = false;

//! DCM correction mode ("Bias", "Filter", or "None")
std::string dcmCorrectionMode = "Bias";

//! The standard deviation of the DCM bias drift [m/s]
double biasDriftPerSecondStd = stateObservation::LipmDcmEstimator::defaultBiasDriftSecond;

//! The standard deviation of the DCM estimation error, NOT including the DCM bias [m]
double dcmMeasureErrorStd = stateObservation::LipmDcmEstimator::defaultDcmErrorStd;

//! The standard deviaiton of the ZMP estimation error [m]
double zmpMeasureErrorStd = stateObservation::LipmDcmEstimator::defaultZmpErrorStd;

//! The largest accepted absolute value of the DCM bias in local frame [m] (negative value for no limit)
Eigen::Vector2d biasLimit = Eigen::Vector2d::Constant(0.03);

//! The uncertainty in the DCM initial value [m]
Eigen::Vector2d initDcmUncertainty =
Eigen::Vector2d::Constant(stateObservation::LipmDcmEstimator::defaultDCMUncertainty);

//! The uncertainty in the DCM bias initial value [m]
Eigen::Vector2d initBiasUncertainty =
Eigen::Vector2d::Constant(stateObservation::LipmDcmEstimator::defaultBiasUncertainty);

/** \brief Load mc_rtc configuration. */
virtual void load(const mc_rtc::Configuration & mcRtcConfig);
};

/** \brief Configuration. */
struct Configuration
{
Expand Down Expand Up @@ -70,14 +105,17 @@ class CentroidalManager
bool useTargetPoseForControlRobotAnchorFrame = true;

//! Whether to use actual CoM for wrench distribution
bool useActualComForWrenchDist = true;
bool useActualComForWrenchDist = false;

//! Actual CoM offset
//! Actual CoM offset in world frame [m]
Eigen::Vector3d actualComOffset = Eigen::Vector3d::Zero();

//! Configuration for wrench distribution
mc_rtc::Configuration wrenchDistConfig;

//! Configuration for DCM estimator
DcmEstimatorConfiguration dcmEstimatorConfig;

/** \brief Load mc_rtc configuration. */
virtual void load(const mc_rtc::Configuration & mcRtcConfig);
};
Expand Down Expand Up @@ -172,6 +210,9 @@ class CentroidalManager
/** \brief Get actual CoM. */
Eigen::Vector3d actualCom() const;

/** \brief Get actual unbiased CoM. */
Eigen::Vector3d actualComUnbiased() const;

/** \brief Calculate ZMP from wrench list.
\param wrenchList wrench list
\param zmpPlaneHeight height of ZMP plane
Expand Down Expand Up @@ -232,5 +273,11 @@ class CentroidalManager

//! Interpolation function of reference CoM Z position
std::shared_ptr<TrajColl::CubicInterpolator<double>> refComZFunc_;

//! DCM estimator
std::shared_ptr<stateObservation::LipmDcmEstimator> dcmEstimator_;

//! Whether to require to reset DCM estimator
bool requireDcmEstimatorReset_ = true;
};
} // namespace BWC
161 changes: 147 additions & 14 deletions src/CentroidalManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <mc_rtc/gui/Button.h>
#include <mc_rtc/gui/Checkbox.h>
#include <mc_rtc/gui/ComboInput.h>
#include <mc_rtc/gui/Label.h>
#include <mc_rtc/gui/NumberInput.h>
#include <mc_rtc/gui/plot.h>
Expand All @@ -17,6 +18,24 @@

using namespace BWC;

void CentroidalManager::DcmEstimatorConfiguration::load(const mc_rtc::Configuration & mcRtcConfig)
{
mcRtcConfig("enableDcmEstimator", enableDcmEstimator);
mcRtcConfig("dcmCorrectionMode", dcmCorrectionMode);
const std::vector<std::string> dcmCorrectionModeList = {"Bias", "Filter", "None"};
if(std::find(dcmCorrectionModeList.begin(), dcmCorrectionModeList.end(), dcmCorrectionMode)
== dcmCorrectionModeList.end())
{
mc_rtc::log::error_and_throw("[CentroidalManager] Invalid dcmCorrectionMode: {}", dcmCorrectionMode);
}
mcRtcConfig("biasDriftPerSecondStd", biasDriftPerSecondStd);
mcRtcConfig("dcmMeasureErrorStd", dcmMeasureErrorStd);
mcRtcConfig("zmpMeasureErrorStd", zmpMeasureErrorStd);
mcRtcConfig("biasLimit", biasLimit);
mcRtcConfig("initDcmUncertainty", initDcmUncertainty);
mcRtcConfig("initBiasUncertainty", initBiasUncertainty);
}

void CentroidalManager::Configuration::load(const mc_rtc::Configuration & mcRtcConfig)
{
mcRtcConfig("name", name);
Expand All @@ -33,6 +52,7 @@ void CentroidalManager::Configuration::load(const mc_rtc::Configuration & mcRtcC
mcRtcConfig("useActualComForWrenchDist", useActualComForWrenchDist);
mcRtcConfig("actualComOffset", actualComOffset);
mcRtcConfig("wrenchDistConfig", wrenchDistConfig);
dcmEstimatorConfig.load(mcRtcConfig("dcmEstimator", mc_rtc::Configuration()));
}

CentroidalManager::CentroidalManager(BaselineWalkingController * ctlPtr, const mc_rtc::Configuration & // mcRtcConfig
Expand All @@ -49,6 +69,15 @@ void CentroidalManager::reset()
refComZFunc_->appendPoint(std::make_pair(ctl().t(), config().refComZ));
refComZFunc_->appendPoint(std::make_pair(interpMaxTime_, config().refComZ));
refComZFunc_->calcCoeff();

const DcmEstimatorConfiguration & dcmEstimatorConfig = config().dcmEstimatorConfig;
double nominalOmega = std::sqrt(mc_rtc::constants::GRAVITY / config().refComZ);
dcmEstimator_ = std::make_shared<stateObservation::LipmDcmEstimator>(
ctl().dt(), nominalOmega, dcmEstimatorConfig.biasDriftPerSecondStd, dcmEstimatorConfig.dcmMeasureErrorStd,
dcmEstimatorConfig.zmpMeasureErrorStd, dcmEstimatorConfig.biasLimit, Eigen::Vector2d::Zero(),
Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero(), dcmEstimatorConfig.initDcmUncertainty,
dcmEstimatorConfig.initBiasUncertainty);
requireDcmEstimatorReset_ = true;
}

void CentroidalManager::update()
Expand All @@ -70,6 +99,20 @@ void CentroidalManager::update()
// Run MPC
runMpc();

// Calculate measured ZMP
{
std::unordered_map<Foot, sva::ForceVecd> sensorWrenchList;
for(const auto & foot : ctl().footManager_->getCurrentContactFeet())
{
const auto & surfaceName = ctl().footManager_->surfaceName(foot);
const auto & sensorName = ctl().robot().indirectSurfaceForceSensor(surfaceName).name();
const auto & sensor = ctl().robot().forceSensor(sensorName);
const auto & sensorWrench = sensor.worldWrenchWithoutGravity(ctl().robot());
sensorWrenchList.emplace(foot, sensorWrench);
}
measuredZMP_ = calcZmp(sensorWrenchList, refZmp_.z());
}

// Calculate target wrench
{
controlZmp_ = plannedZmp_;
Expand All @@ -86,7 +129,48 @@ void CentroidalManager::update()
double omega = std::sqrt(plannedForceZ_ / (robotMass_ * (mpcCom_.z() - refZmp_.z())));
Eigen::Vector3d plannedDcm = ctl().comTask_->com() + ctl().comTask_->refVel() / omega;
Eigen::Vector3d actualDcm = actualCom() + ctl().realRobot().comVelocity() / omega;
controlZmp_.head<2>() += config().dcmGainP * (actualDcm - plannedDcm).head<2>();
Eigen::Vector3d dcmError = actualDcm - plannedDcm;

// Estimate and compensate for DCM bias
if(config().dcmEstimatorConfig.enableDcmEstimator)
{
if(omega != dcmEstimator_->getLipmNaturalFrequency())
{
dcmEstimator_->setLipmNaturalFrequency(omega);
}

const Eigen::Matrix3d & baseOrientation = ctl().robot().posW().rotation().transpose();
if(requireDcmEstimatorReset_)
{
dcmEstimator_->resetWithMeasurements(actualDcm.head<2>(), measuredZMP_.head<2>(), baseOrientation, true);
requireDcmEstimatorReset_ = false;
}
else
{
dcmEstimator_->setInputs(actualDcm.head<2>(), measuredZMP_.head<2>(), baseOrientation);
}

dcmEstimator_->update();

if(config().dcmEstimatorConfig.dcmCorrectionMode == "Bias")
{
dcmError.head<2>() -= dcmEstimator_->getBias();
}
else if(config().dcmEstimatorConfig.dcmCorrectionMode == "Filter")
{
dcmError.head<2>() = dcmEstimator_->getUnbiasedDCM();
}
// else if(config().dcmEstimatorConfig.dcmCorrectionMode == "None")
// {
// }
}
else
{
requireDcmEstimatorReset_ = true;
dcmEstimator_->setBias(Eigen::Vector2d::Zero());
}

controlZmp_.head<2>() += config().dcmGainP * dcmError.head<2>();
}

// Apply ForceZ feedback
Expand All @@ -104,7 +188,8 @@ void CentroidalManager::update()
contactList_ = ctl().footManager_->calcCurrentContactList();
wrenchDist_ = std::make_shared<ForceColl::WrenchDistribution>(ForceColl::getContactVecFromMap(contactList_),
config().wrenchDistConfig);
Eigen::Vector3d comForWrenchDist = (config().useActualComForWrenchDist ? actualCom() : ctl().comTask_->com());
Eigen::Vector3d comForWrenchDist =
(config().useActualComForWrenchDist ? actualComUnbiased() : ctl().comTask_->com());
sva::ForceVecd controlWrench;
controlWrench.force() << controlForceZ_ / (comForWrenchDist.z() - refZmp_.z())
* (comForWrenchDist.head<2>() - controlZmp_.head<2>()),
Expand Down Expand Up @@ -143,19 +228,8 @@ void CentroidalManager::update()
}
}

// Calculate ZMP for log
// Calculate support region for log
{
std::unordered_map<Foot, sva::ForceVecd> sensorWrenchList;
for(const auto & foot : ctl().footManager_->getCurrentContactFeet())
{
const auto & surfaceName = ctl().footManager_->surfaceName(foot);
const auto & sensorName = ctl().robot().indirectSurfaceForceSensor(surfaceName).name();
const auto & sensor = ctl().robot().forceSensor(sensorName);
const auto & sensorWrench = sensor.worldWrenchWithoutGravity(ctl().robot());
sensorWrenchList.emplace(foot, sensorWrench);
}
measuredZMP_ = calcZmp(sensorWrenchList, refZmp_.z());

supportRegion_[0].setConstant(std::numeric_limits<double>::max());
supportRegion_[1].setConstant(std::numeric_limits<double>::lowest());
for(const auto & contactKV : contactList_)
Expand Down Expand Up @@ -215,6 +289,44 @@ void CentroidalManager::addToGUI(mc_rtc::gui::StateBuilder & gui)
"actualComOffset", {"x", "y", "z"}, [this]() -> const Eigen::Vector3d & { return config().actualComOffset; },
[this](const Eigen::Vector3d & v) { config().actualComOffset = v; }));

gui.addElement({ctl().name(), config().name, "DcmEstimator"},
mc_rtc::gui::Checkbox(
"enableDcmEstimator", [this]() { return config().dcmEstimatorConfig.enableDcmEstimator; },
[this]() {
config().dcmEstimatorConfig.enableDcmEstimator = !config().dcmEstimatorConfig.enableDcmEstimator;
}),
mc_rtc::gui::ComboInput(
"dcmCorrectionMode", {"Bias", "Filter", "None"},
[this]() -> const std::string & { return config().dcmEstimatorConfig.dcmCorrectionMode; },
[this](const std::string & v) { config().dcmEstimatorConfig.dcmCorrectionMode = v; }),
mc_rtc::gui::NumberInput(
"biasDriftPerSecondStd", [this]() { return config().dcmEstimatorConfig.biasDriftPerSecondStd; },
[this](double v) {
config().dcmEstimatorConfig.biasDriftPerSecondStd = v;
dcmEstimator_->setBiasDriftPerSecond(v);
}),
mc_rtc::gui::NumberInput(
"dcmMeasureErrorStd", [this]() { return config().dcmEstimatorConfig.dcmMeasureErrorStd; },
[this](double v) {
config().dcmEstimatorConfig.dcmMeasureErrorStd = v;
dcmEstimator_->setDcmMeasureErrorStd(v);
}),
mc_rtc::gui::NumberInput(
"zmpMeasureErrorStd", [this]() { return config().dcmEstimatorConfig.zmpMeasureErrorStd; },
[this](double v) {
config().dcmEstimatorConfig.zmpMeasureErrorStd = v;
dcmEstimator_->setZmpMeasureErrorStd(v);
}),
mc_rtc::gui::ArrayInput(
"biasLimit", {"local X", "local Y"},
[this]() -> const Eigen::Vector2d & { return config().dcmEstimatorConfig.biasLimit; },
[this](const Eigen::Vector2d & v) {
config().dcmEstimatorConfig.biasLimit = v;
dcmEstimator_->setBiasLimit(v);
}),
mc_rtc::gui::ArrayLabel("estimatedLocalBias", {"local X", "local Y"},
[this]() { return dcmEstimator_->getLocalBias(); }));

gui.addElement(
{ctl().name(), config().name, "Plot"}, mc_rtc::gui::ElementsStacking::Horizontal,
mc_rtc::gui::Button(
Expand All @@ -230,6 +342,9 @@ void CentroidalManager::addToGUI(mc_rtc::gui::StateBuilder & gui)
plot::Style::Dotted),
plot::Y(
"CoM_realRobot", [this]() { return actualCom().x(); }, Color::Red, plot::Style::Dotted),
plot::Y(
"CoM_realRobotUnbiased", [this]() { return actualComUnbiased().x(); },
mc_rtc::gui::Color(1.0, 0.5, 0.0), plot::Style::Dotted),
plot::Y(
"ZMP_ref", [this]() { return refZmp_.x(); }, Color::Blue),
plot::Y(
Expand Down Expand Up @@ -259,6 +374,9 @@ void CentroidalManager::addToGUI(mc_rtc::gui::StateBuilder & gui)
plot::Style::Dotted),
plot::Y(
"CoM_realRobot", [this]() { return actualCom().y(); }, Color::Red, plot::Style::Dotted),
plot::Y(
"CoM_realRobotUnbiased", [this]() { return actualComUnbiased().y(); },
mc_rtc::gui::Color(1.0, 0.5, 0.0), plot::Style::Dotted),
plot::Y(
"ZMP_ref", [this]() { return refZmp_.y(); }, Color::Blue),
plot::Y(
Expand Down Expand Up @@ -304,6 +422,7 @@ void CentroidalManager::addToLogger(mc_rtc::Logger & logger)
logger.addLogEntry(config().name + "_CoM_planned", this, [this]() { return ctl().comTask_->com(); });
logger.addLogEntry(config().name + "_CoM_controlRobot", this, [this]() { return ctl().robot().com(); });
logger.addLogEntry(config().name + "_CoM_realRobot", this, [this]() { return actualCom(); });
logger.addLogEntry(config().name + "_CoM_realRobotUnbiased", this, [this]() { return actualComUnbiased(); });

MC_RTC_LOG_HELPER(config().name + "_forceZ_planned", plannedForceZ_);
MC_RTC_LOG_HELPER(config().name + "_forceZ_control", controlForceZ_);
Expand All @@ -322,6 +441,13 @@ void CentroidalManager::addToLogger(mc_rtc::Logger & logger)
logger.addLogEntry(config().name + "_CentroidalMomentum_controlRobot", this, [this]() {
return rbd::computeCentroidalMomentum(ctl().robot().mb(), ctl().robot().mbc(), ctl().robot().com());
});

logger.addLogEntry(config().name + "_DcmEstimator_enableDcmEstimator", this,
[this]() { return config().dcmEstimatorConfig.enableDcmEstimator; });
logger.addLogEntry(config().name + "_DcmEstimator_dcmCorrectionMode", this,
[this]() { return config().dcmEstimatorConfig.dcmCorrectionMode; });
logger.addLogEntry(config().name + "_DcmEstimator_localBias", this,
[this]() { return dcmEstimator_->getLocalBias(); });
}

void CentroidalManager::removeFromLogger(mc_rtc::Logger & logger)
Expand Down Expand Up @@ -397,6 +523,13 @@ Eigen::Vector3d CentroidalManager::actualCom() const
return ctl().realRobot().com() + config().actualComOffset;
}

Eigen::Vector3d CentroidalManager::actualComUnbiased() const
{
Eigen::Vector3d com = actualCom();
com.head<2>() -= dcmEstimator_->getBias();
return com;
}

Eigen::Vector3d CentroidalManager::calcZmp(const std::unordered_map<Foot, sva::ForceVecd> & wrenchList,
double zmpPlaneHeight,
const Eigen::Vector3d & zmpPlaneNormal) const
Expand Down

0 comments on commit 87f2f52

Please sign in to comment.