Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow to change the reference CoM Z position #17

Merged
merged 1 commit into from
Aug 23, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions .github/workflows/config/WalkingOnPlane.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
3 changes: 3 additions & 0 deletions etc/BaselineWalkingController.in.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,7 @@ CentroidalManager:
method: PreviewControlZmp
horizonDuration: 2.0 # [sec]
horizonDt: 0.005 # [sec]
reinitForRefComZ: true

# # DdpZmp
# method: DdpZmp
Expand All @@ -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]
Expand Down
22 changes: 22 additions & 0 deletions include/BaselineWalkingController/CentroidalManager.h
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 <TrajColl/CubicInterpolator.h>

#include <BaselineWalkingController/FootTypes.h>

namespace mc_rbdyn
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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
*/
Expand All @@ -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;

Expand Down Expand Up @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down Expand Up @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down Expand Up @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down Expand Up @@ -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
51 changes: 45 additions & 6 deletions src/CentroidalManager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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; },
Expand Down Expand Up @@ -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();
Expand All @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion src/centroidal/CentroidalManagerDdpZmp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
17 changes: 15 additions & 2 deletions src/centroidal/CentroidalManagerFootGuidedControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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());
Expand Down
17 changes: 14 additions & 3 deletions src/centroidal/CentroidalManagerIntrinsicallyStableMpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ void CentroidalManagerIntrinsicallyStableMpc::Configuration::load(const mc_rtc::
{
qpSolverType = QpSolverCollection::strToQpSolverType(mcRtcConfig("qpSolverType"));
}
mcRtcConfig("reinitForRefComZ", reinitForRefComZ);
}

CentroidalManagerIntrinsicallyStableMpc::CentroidalManagerIntrinsicallyStableMpc(
Expand All @@ -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;
}

Expand All @@ -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>();
Expand Down
15 changes: 13 additions & 2 deletions src/centroidal/CentroidalManagerPreviewControlZmp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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>();
Expand All @@ -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 =
Expand Down
12 changes: 12 additions & 0 deletions src/states/ConfigWalkState.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <BaselineWalkingController/BaselineWalkingController.h>
#include <BaselineWalkingController/CentroidalManager.h>
#include <BaselineWalkingController/FootManager.h>
#include <BaselineWalkingController/states/ConfigWalkState.h>

Expand Down Expand Up @@ -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");
}

Expand Down