Skip to content

Commit

Permalink
some clean up of Parametrized and added relevant test
Browse files Browse the repository at this point in the history
  • Loading branch information
@mebbaid
mebbaid committed Jul 12, 2023
1 parent 130056d commit 486c8d7
Show file tree
Hide file tree
Showing 7 changed files with 441 additions and 79 deletions.
9 changes: 0 additions & 9 deletions ...cedModelControllers/include/BipedalLocomotion/ReducedModelControllers/BaseCentroidalMPC.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,6 @@ struct CentroidalMPCOutput
std::map<std::string, Contacts::PlannedContact> nextPlannedContact;
};

struct ParametrizedCentroidalMPCState
{
std::map<std::string, Contacts::DiscreteGeometryContact> contacts;
std::map<std::string, Contacts::PlannedContact> nextPlannedContact;
double computationalTime{0};
Eigen::Vector3d externalWrench;
};

class BaseCentroidalMPC : public BipedalLocomotion::System::Source<CentroidalMPCOutput>
{
Expand All @@ -120,8 +113,6 @@ class BaseCentroidalMPC : public BipedalLocomotion::System::Source<CentroidalMPC

virtual const CentroidalMPCOutput& getOutput() const = 0;

virtual const ParametrizedCentroidalMPCState& getParametrizedOutput() const = 0;

virtual bool isOutputValid() const = 0;

virtual bool advance() = 0;
Expand Down
2 changes: 0 additions & 2 deletions ...ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,8 +146,6 @@ class CentroidalMPC : public BaseCentroidalMPC
*/
const CentroidalMPCOutput& getOutput() const override;

const ParametrizedCentroidalMPCState& getParametrizedOutput() const override;

/**
* Determines the validity of the object retrieved with getOutput()
* @return True if the object is valid, false otherwise.
Expand Down
2 changes: 0 additions & 2 deletions ...Controllers/include/BipedalLocomotion/ReducedModelControllers/ParametrizedCentroidalMPC.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,6 @@ class ParametrizedCentroidalMPC : public BaseCentroidalMPC
bool setReferenceTrajectory(const std::vector<Eigen::Vector3d>& com,
const std::vector<Eigen::Vector3d>& angularMomentum) override;

const ParametrizedCentroidalMPCState& getParametrizedOutput() const override;

const CentroidalMPCOutput& getOutput() const override;

bool isOutputValid() const override;
Expand Down
5 changes: 0 additions & 5 deletions src/ReducedModelControllers/src/CentroidalMPC.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ struct CentroidalMPC::Impl
std::chrono::nanoseconds currentTime{std::chrono::nanoseconds::zero()};

CentroidalMPCOutput output;
ParametrizedCentroidalMPCState Invalidoutput;
Contacts::ContactPhaseList contactPhaseList;
Math::LinearizedFrictionCone frictionCone;

Expand Down Expand Up @@ -954,10 +953,6 @@ const CentroidalMPCOutput& CentroidalMPC::getOutput() const
return m_pimpl->output;
}

const ParametrizedCentroidalMPCState& CentroidalMPC::getParametrizedOutput() const
{
return m_pimpl->Invalidoutput;
}


bool CentroidalMPC::isOutputValid() const
Expand Down
99 changes: 40 additions & 59 deletions src/ReducedModelControllers/src/ParametrizedCentroidalMPC.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,7 @@ struct ParametrizedCentroidalMPC::Impl
casadi::Function controller;
std::chrono::nanoseconds currentTime{std::chrono::nanoseconds::zero()};

ParametrizedCentroidalMPCState output;
CentroidalMPCOutput Invalidoutput;
CentroidalMPCOutput output;
Contacts::ContactPhaseList contactPhaseList;

enum class FSM
Expand All @@ -44,9 +43,6 @@ struct ParametrizedCentroidalMPC::Impl
double staticFrictionCoefficient;
double torsionalFrictionCoefficient;
const double fZmin = 0.0;
const double k1
= 10.0; // see
// https://github.com/ami-iit/element_walking-with-payload/issues/37#issuecomment-1505225623
};
ParametrizationConstants parameterizationConstants; /**< constants for computing phi and its
inverse */
Expand Down Expand Up @@ -78,7 +74,7 @@ struct ParametrizedCentroidalMPC::Impl
this->position(1, 0) = other.position(1);
this->position(2, 0) = other.position(2);

this->xi_parameter = casadi::MX::sym(cornerName + "xi", other.force.size(), 1);
this->xi_parameter = casadi::MX::sym(cornerName + "xi", other.force.size(), 1); // we start with parameter and force coinciding

return *this;
}
Expand Down Expand Up @@ -176,12 +172,6 @@ struct ParametrizedCentroidalMPC::Impl
casadi::MX externalForce;
casadi::MX externalTorque;

// robust adaptive variables see
// https://github.com/ami-iit/element_walking-with-payload/issues/37#issuecomment-1505225623
casadi::MX z1; /**< CoM reference tracking error */
casadi::MX z2; /**< Adaptive coordinates change */
// casadi::MX eta; /**< zero dynamics- angular momentum */
casadi::MX k2; /**< second adaptive gain */
};
OptimizationVariables optiVariables; /**< Optimization variables */

Expand Down Expand Up @@ -242,7 +232,7 @@ struct ParametrizedCentroidalMPC::Impl
bool loadContactCorners(std::shared_ptr<const ParametersHandler::IParametersHandler> ptr,
DiscreteGeometryContact& contact)
{
constexpr auto errorPrefix = "[CentroidalMPC::Impl::loadContactCorners]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::Impl::loadContactCorners]";

int numberOfCorners;
if (!ptr->getParameter("number_of_corners", numberOfCorners))
Expand Down Expand Up @@ -362,13 +352,12 @@ struct ParametrizedCentroidalMPC::Impl

ok = ok && getParameter(ptr, "com_weight", this->weights.com);
ok = ok && getParameter(ptr, "contact_position_weight", this->weights.contactPosition);
ok = ok
&& getParameter(ptr, "force_rate_of_change_weight", this->weights.forceRateOfChange);
ok = ok && getParameter(ptr, "force_rate_of_change_weight", this->weights.forceRateOfChange);
ok = ok && getParameter(ptr, "angular_momentum_weight", this->weights.angularMomentum);
ok = ok
&& getParameter(ptr,
"contact_force_symmetry_weight",
this->weights.contactForceSymmetry);
ok = ok && getParameter(ptr, "contact_force_symmetry_weight", this->weights.contactForceSymmetry);
ok = ok && getParameter(ptr, "parameter_regularization_weight", this->weights.parameterRegularization);
ok = ok && getParameter(ptr, "payload_weight", this->weights.payloadWeight);


// initialize the friction cone

Expand Down Expand Up @@ -426,7 +415,7 @@ struct ParametrizedCentroidalMPC::Impl

casadi::Function ode()
{
constexpr auto errorPrefix = "[CentroidalMPC::Impl::ode]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::Impl::ode]";
std::map<std::string, ParametrizedCasadiContact> casadiContacts;

for (const auto& [key, contact] : this->output.contacts)
Expand Down Expand Up @@ -473,7 +462,7 @@ struct ParametrizedCentroidalMPC::Impl
for (const auto& corner : contact.corners)
{
using namespace casadi;
casadi::MX xi = casadi::MX::sym("xi", 3);
//casadi::MX xi = casadi::MX::sym("xi", 3);

casadi::MX force = HorizonParametrization(this->parameterizationConstants
.staticFrictionCoefficient,
Expand Down Expand Up @@ -657,11 +646,6 @@ struct ParametrizedCentroidalMPC::Impl
this->optiVariables.dcom = this->opti.variable(vector3Size, stateHorizon);
this->optiVariables.angularMomentum = this->opti.variable(vector3Size, stateHorizon);

// create varibales for coordinate change and adaptive gain
this->optiVariables.z1 = this->opti.variable(vector3Size, stateHorizon);
this->optiVariables.z2 = this->opti.variable(vector3Size, stateHorizon);
this->optiVariables.k2 = this->opti.variable(vector3Size, vector3Size); /** < 3 x 3 fixed gain over the horizon */

// the casadi contacts depends on the maximum number of contacts
for (const auto& [key, contact] : this->output.contacts)
{
Expand Down Expand Up @@ -763,10 +747,6 @@ struct ParametrizedCentroidalMPC::Impl
auto& externalForce = this->optiVariables.externalForce;
auto& externalTorque = this->optiVariables.externalTorque;

auto& z1 = this->optiVariables.z1;
auto& z2 = this->optiVariables.z2;
auto& k2 = this->optiVariables.k2;

// prepare the input of the ode
std::vector<casadi::MX> odeInput;
odeInput.push_back(externalForce);
Expand All @@ -783,27 +763,24 @@ struct ParametrizedCentroidalMPC::Impl

for (const auto& corner : contact.corners)
{

/*
force = Parametrization(this->parameterizationConstants.staticFrictionCoefficient,
this->parameterizationConstants.staticFrictionCoefficient,
contactBoundingBoxes[key].lowerLimit(0),
contactBoundingBoxes[key].upperLimit(0), contactBoundingBoxes[key].lowerLimit(1),
contactBoundingBoxes[key].upperLimit(1), this->parameterizationConstants.fZmin,
corner.xi_parameter);
casadi::MX force
= HorizonParametrization(this->parameterizationConstants.staticFrictionCoefficient,
this->parameterizationConstants.staticFrictionCoefficient,
contactBoundingBoxes[key].lowerLimit(0),
contactBoundingBoxes[key].upperLimit(0),
contactBoundingBoxes[key].lowerLimit(1),
contactBoundingBoxes[key].upperLimit(1),
this->parameterizationConstants.fZmin,
corner.xi_parameter);
*/
odeInput.push_back(corner.xi_parameter);
}
}

// set the feedback
this->opti.subject_to(this->optiVariables.comCurrent == com(Sl(), 0));
this->opti.subject_to(z1(Sl(), 0)
== this->optiVariables.comCurrent
- this->optiVariables.comReference); // adaptive change of
// coordinates
this->opti.subject_to(z2(Sl(), 0)
== this->parameterizationConstants.k1 * casadi::MX::eye(3)
+ this->optiVariables.angularMomentumCurrent);
this->opti.subject_to(this->optiVariables.dcomCurrent == dcom(Sl(), 0));
this->opti.subject_to(this->optiVariables.angularMomentumCurrent
== angularMomentum(Sl(), 0));
Expand Down Expand Up @@ -856,14 +833,15 @@ struct ParametrizedCentroidalMPC::Impl
weightCoMZ(Sl(), i) = (this->weights.com(2) - min) * std::exp(-i) + min;
}


casadi::MX cost
= this->weights.angularMomentum
* casadi::MX::sumsqr(angularMomentum - angularMomentumReference)
+ this->weights.com(0) * casadi::MX::sumsqr(com(0, Sl()) - comReference(0, Sl()))
+ this->weights.com(1) * casadi::MX::sumsqr(com(1, Sl()) - comReference(1, Sl()))
+ casadi::MX::sumsqr(weightCoMZ * (com(2, Sl()) - comReference(2, Sl())))
+ this->weights.com(0) * casadi::MX::sumsqr(z1(1, Sl()));

+ casadi::MX::sumsqr(weightCoMZ * (com(2, Sl()) - comReference(2, Sl())));

casadi::MX averageForce;
for (const auto& [key, contact] : this->optiVariables.parametrizedContacts)
{
Expand Down Expand Up @@ -921,7 +899,7 @@ struct ParametrizedCentroidalMPC::Impl
auto forceRateOfChange = casadi::MX::diff(force.T()).T();

cost += this->weights.contactForceSymmetry
* casadi::MX::sumsqr(force - averageForce);
* casadi::MX::sumsqr(force - averageForce);

cost += this->weights.forceRateOfChange(0)
* casadi::MX::sumsqr(forceRateOfChange(0, Sl()));
Expand All @@ -931,9 +909,16 @@ struct ParametrizedCentroidalMPC::Impl
* casadi::MX::sumsqr(forceRateOfChange(2, Sl()));

// parameter regularization task

casadi::DM gravity = casadi::DM::zeros(3);
gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation;

cost += this->weights.payloadWeight * casadi::MX::sumsqr(force - gravity - 1/(8) * externalForce);

cost += this->weights.parameterRegularization
* casadi::MX::mtimes(corner.xi_parameter(Sl(), 0).T(),
corner.xi_parameter(Sl(), 0));

}
}

Expand Down Expand Up @@ -1021,7 +1006,7 @@ struct ParametrizedCentroidalMPC::Impl
bool ParametrizedCentroidalMPC::initialize(
std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
{
constexpr auto errorPrefix = "[CentroidalMPC::initialize]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::initialize]";
auto ptr = handler.lock();

if (ptr == nullptr)
Expand Down Expand Up @@ -1050,14 +1035,9 @@ ParametrizedCentroidalMPC::ParametrizedCentroidalMPC()
m_pimpl = std::make_unique<Impl>();
}

const ParametrizedCentroidalMPCState& ParametrizedCentroidalMPC::getParametrizedOutput() const
{
return m_pimpl->output;
}

const CentroidalMPCOutput& ParametrizedCentroidalMPC::getOutput() const
{
return m_pimpl->Invalidoutput;
return m_pimpl->output;
}

bool ParametrizedCentroidalMPC::isOutputValid() const
Expand All @@ -1067,7 +1047,7 @@ bool ParametrizedCentroidalMPC::isOutputValid() const

bool ParametrizedCentroidalMPC::advance()
{
constexpr auto errorPrefix = "[CentroidalMPC::advance]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::advance]";
assert(m_pimpl);

using Sl = casadi::Slice;
Expand Down Expand Up @@ -1177,7 +1157,8 @@ bool ParametrizedCentroidalMPC::advance()
m_pimpl->parameterizationConstants.fZmin,
casadi::MX(*it));

corner.force = toEigen(*it).leftCols<1>();
corner.force = toEigen(casadi::DM(casadi::MX::evalf(force))).leftCols<1>();
//log()->info("{} Obtained contact forces {}", errorPrefix, corner.force);
} else
{
corner.force.setZero();
Expand All @@ -1198,7 +1179,7 @@ bool ParametrizedCentroidalMPC::advance()
bool ParametrizedCentroidalMPC::setReferenceTrajectory(
const std::vector<Eigen::Vector3d>& com, const std::vector<Eigen::Vector3d>& angularMomentum)
{
constexpr auto errorPrefix = "[CentroidalMPC::setReferenceTrajectory]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::setReferenceTrajectory]";

const int stateHorizon = m_pimpl->optiSettings.horizon + 1;

Expand Down Expand Up @@ -1265,7 +1246,7 @@ bool ParametrizedCentroidalMPC::setState(Eigen::Ref<const Eigen::Vector3d> com,
Eigen::Ref<const Eigen::Vector3d> angularMomentum,
const Math::Wrenchd& externalWrench)
{
constexpr auto errorPrefix = "[CentroidalMPC::setState]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::setState]";
assert(m_pimpl);

if (m_pimpl->fsm == Impl::FSM::Idle)
Expand Down Expand Up @@ -1294,7 +1275,7 @@ bool ParametrizedCentroidalMPC::setState(Eigen::Ref<const Eigen::Vector3d> com,
bool ParametrizedCentroidalMPC::setContactPhaseList(
const Contacts::ContactPhaseList& contactPhaseList)
{
constexpr auto errorPrefix = "[CentroidalMPC::setContactPhaseList]";
constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::setContactPhaseList]";
assert(m_pimpl);

if (m_pimpl->fsm == Impl::FSM::Idle)
Expand Down
15 changes: 13 additions & 2 deletions src/ReducedModelControllers/tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,22 @@
# GNU Lesser General Public License v2.1 or any later version.

if(FRAMEWORK_COMPILE_Planners)

add_bipedal_test(
NAME CentroidalMPC
SOURCES CentroidalMPCTest.cpp
LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math
BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners)
BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners)

add_bipedal_test(
NAME ParametrizedCentroidalMPC
SOURCES ParametrizedCentroidalMPCTest.cpp
LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math
BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners)

# add_bipedal_test(
# NAME AdaptiveCentroidalMPC
# SOURCES AdaptiveCentroidalMPCTest.cpp
# LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math
# BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners)

endif()
Loading

0 comments on commit 486c8d7

Please sign in to comment.