From 486c8d714294bbc347f5d803baf97b1997807d5d Mon Sep 17 00:00:00 2001 From: mebbaid Date: Wed, 12 Jul 2023 17:32:37 +0200 Subject: [PATCH] some clean up of Parametrized and added relevant test --- .../BaseCentroidalMPC.h | 9 - .../ReducedModelControllers/CentroidalMPC.h | 2 - .../ParametrizedCentroidalMPC.h | 2 - .../src/CentroidalMPC.cpp | 5 - .../src/ParametrizedCentroidalMPC.cpp | 99 ++--- .../tests/CMakeLists.txt | 15 +- .../tests/ParametrizedCentroidalMPCTest.cpp | 388 ++++++++++++++++++ 7 files changed, 441 insertions(+), 79 deletions(-) create mode 100644 src/ReducedModelControllers/tests/ParametrizedCentroidalMPCTest.cpp diff --git a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/BaseCentroidalMPC.h b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/BaseCentroidalMPC.h index e41ab0e6e6..8b0165c662 100644 --- a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/BaseCentroidalMPC.h +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/BaseCentroidalMPC.h @@ -87,13 +87,6 @@ struct CentroidalMPCOutput std::map nextPlannedContact; }; -struct ParametrizedCentroidalMPCState -{ - std::map contacts; - std::map nextPlannedContact; - double computationalTime{0}; - Eigen::Vector3d externalWrench; -}; class BaseCentroidalMPC : public BipedalLocomotion::System::Source { @@ -120,8 +113,6 @@ class BaseCentroidalMPC : public BipedalLocomotion::System::Source& com, const std::vector& angularMomentum) override; - const ParametrizedCentroidalMPCState& getParametrizedOutput() const override; - const CentroidalMPCOutput& getOutput() const override; bool isOutputValid() const override; diff --git a/src/ReducedModelControllers/src/CentroidalMPC.cpp b/src/ReducedModelControllers/src/CentroidalMPC.cpp index 049abc5ed3..e5e82576f9 100644 --- a/src/ReducedModelControllers/src/CentroidalMPC.cpp +++ b/src/ReducedModelControllers/src/CentroidalMPC.cpp @@ -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; @@ -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 diff --git a/src/ReducedModelControllers/src/ParametrizedCentroidalMPC.cpp b/src/ReducedModelControllers/src/ParametrizedCentroidalMPC.cpp index 83911cdcd4..8a1447f356 100644 --- a/src/ReducedModelControllers/src/ParametrizedCentroidalMPC.cpp +++ b/src/ReducedModelControllers/src/ParametrizedCentroidalMPC.cpp @@ -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 @@ -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 */ @@ -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; } @@ -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 */ @@ -242,7 +232,7 @@ struct ParametrizedCentroidalMPC::Impl bool loadContactCorners(std::shared_ptr ptr, DiscreteGeometryContact& contact) { - constexpr auto errorPrefix = "[CentroidalMPC::Impl::loadContactCorners]"; + constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::Impl::loadContactCorners]"; int numberOfCorners; if (!ptr->getParameter("number_of_corners", numberOfCorners)) @@ -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 @@ -426,7 +415,7 @@ struct ParametrizedCentroidalMPC::Impl casadi::Function ode() { - constexpr auto errorPrefix = "[CentroidalMPC::Impl::ode]"; + constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::Impl::ode]"; std::map casadiContacts; for (const auto& [key, contact] : this->output.contacts) @@ -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, @@ -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) { @@ -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 odeInput; odeInput.push_back(externalForce); @@ -783,13 +763,17 @@ 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); } @@ -797,13 +781,6 @@ struct ParametrizedCentroidalMPC::Impl // 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)); @@ -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) { @@ -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())); @@ -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)); + } } @@ -1021,7 +1006,7 @@ struct ParametrizedCentroidalMPC::Impl bool ParametrizedCentroidalMPC::initialize( std::weak_ptr handler) { - constexpr auto errorPrefix = "[CentroidalMPC::initialize]"; + constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::initialize]"; auto ptr = handler.lock(); if (ptr == nullptr) @@ -1050,14 +1035,9 @@ ParametrizedCentroidalMPC::ParametrizedCentroidalMPC() m_pimpl = std::make_unique(); } -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 @@ -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; @@ -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(); @@ -1198,7 +1179,7 @@ bool ParametrizedCentroidalMPC::advance() bool ParametrizedCentroidalMPC::setReferenceTrajectory( const std::vector& com, const std::vector& angularMomentum) { - constexpr auto errorPrefix = "[CentroidalMPC::setReferenceTrajectory]"; + constexpr auto errorPrefix = "[ParametrizedCentroidalMPC::setReferenceTrajectory]"; const int stateHorizon = m_pimpl->optiSettings.horizon + 1; @@ -1265,7 +1246,7 @@ bool ParametrizedCentroidalMPC::setState(Eigen::Ref com, Eigen::Ref 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) @@ -1294,7 +1275,7 @@ bool ParametrizedCentroidalMPC::setState(Eigen::Ref 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) diff --git a/src/ReducedModelControllers/tests/CMakeLists.txt b/src/ReducedModelControllers/tests/CMakeLists.txt index 036b9796ce..325613381f 100644 --- a/src/ReducedModelControllers/tests/CMakeLists.txt +++ b/src/ReducedModelControllers/tests/CMakeLists.txt @@ -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() diff --git a/src/ReducedModelControllers/tests/ParametrizedCentroidalMPCTest.cpp b/src/ReducedModelControllers/tests/ParametrizedCentroidalMPCTest.cpp new file mode 100644 index 0000000000..4a2b04d7a7 --- /dev/null +++ b/src/ReducedModelControllers/tests/ParametrizedCentroidalMPCTest.cpp @@ -0,0 +1,388 @@ + +#include +#include +#include + +// Catch2 +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::ReducedModelControllers; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +void updateContactPhaseList( + const std::map& nextPlannedContacts, + BipedalLocomotion::Contacts::ContactPhaseList& phaseList) +{ + auto newList = phaseList.lists(); + for (const auto& [key, contact] : nextPlannedContacts) + { + auto it = newList.at(key).getPresentContact(contact.activationTime); + newList.at(key).editContact(it, contact); + } + + phaseList.setLists(newList); +} + +void writeHeaderOfFile( + const std::map& contacts, + std::ostream& centroidalMPCData) +{ + for (const auto& [key, contact] : contacts) + { + centroidalMPCData << key << "_pos_x " << key << "_pos_y " << key << "_pos_z "; + for (int j = 0; j < contact.corners.size(); j++) + { + centroidalMPCData << key << "_" << j << "_x" + << " " << key << "_" << j << "_y" + << " " << key << "_" << j << "_z "; + } + + centroidalMPCData << key << "_next_pos_x " << key << "_next_pos_y " << key + << "_next_pos_z "; + } + centroidalMPCData << "com_x com_y com_z des_com_x des_com_y des_com_z ang_x ang_y ang_z " + "elapsed_time" + << std::endl; +} + +void writeResultsToFile(const ParametrizedCentroidalMPC& mpc, + const Eigen::Vector3d& com, + const Eigen::Vector3d& angularMomentum, + const Eigen::Vector3d& comDes, + const std::chrono::nanoseconds& elapsedTime, + std::ostream& centroidalMPCData) +{ + for (const auto& [key, contact] : mpc.getOutput().contacts) + { + centroidalMPCData << contact.pose.translation().transpose() << " "; + for (const auto& corner : contact.corners) + { + centroidalMPCData << corner.force.transpose() << " "; + } + + auto nextPlannedContact = mpc.getOutput().nextPlannedContact.find(key); + if (nextPlannedContact == mpc.getOutput().nextPlannedContact.end()) + { + centroidalMPCData << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } else + { + centroidalMPCData << nextPlannedContact->second.pose.translation().transpose() << " "; + } + } + centroidalMPCData << com.transpose() << " " << comDes.transpose() << " " + << angularMomentum.transpose() << " " << elapsedTime.count() << std::endl; +} + +TEST_CASE("ParametrizedCentroidalMPC") +{ + + constexpr bool saveDataset = false; + + using namespace std::chrono_literals; + constexpr std::chrono::nanoseconds dT = 100ms; + + std::shared_ptr handler = std::make_shared(); + handler->setParameter("sampling_time", dT); + handler->setParameter("time_horizon", 1s + 250ms); + handler->setParameter("number_of_maximum_contacts", 2); + handler->setParameter("number_of_slices", 1); + handler->setParameter("static_friction_coefficient", 0.33); + handler->setParameter("solver_verbosity", 0); + handler->setParameter("linear_solver", "ma97"); + handler->setParameter("is_warm_start_enabled", true); + + auto contact0Handler = std::make_shared(); + contact0Handler->setParameter("number_of_corners", 4); + contact0Handler->setParameter("contact_name", "left_foot"); + contact0Handler->setParameter("corner_0", std::vector{0.1, 0.05, 0}); + contact0Handler->setParameter("corner_1", std::vector{0.1, -0.05, 0}); + contact0Handler->setParameter("corner_2", std::vector{-0.1, -0.05, 0}); + contact0Handler->setParameter("corner_3", std::vector{-0.1, 0.05, 0}); + contact0Handler->setParameter("bounding_box_lower_limit", std::vector{0, 0, 0}); + contact0Handler->setParameter("bounding_box_upper_limit", std::vector{0, 0, 0}); + + auto contact1Handler = std::make_shared(); + contact1Handler->setParameter("number_of_corners", 4); + contact1Handler->setParameter("contact_name", "right_foot"); + contact1Handler->setParameter("corner_0", std::vector{0.1, 0.05, 0}); + contact1Handler->setParameter("corner_1", std::vector{0.1, -0.05, 0}); + contact1Handler->setParameter("corner_2", std::vector{-0.1, -0.05, 0}); + contact1Handler->setParameter("corner_3", std::vector{-0.1, 0.05, 0}); + contact1Handler->setParameter("bounding_box_lower_limit", std::vector{0, 0, 0}); + contact1Handler->setParameter("bounding_box_upper_limit", std::vector{0, 0, 0}); + + handler->setGroup("CONTACT_0", contact0Handler); + handler->setGroup("CONTACT_1", contact1Handler); + + handler->setParameter("com_weight", std::vector{1, 1, 1000}); + handler->setParameter("contact_position_weight", 1e3); + handler->setParameter("force_rate_of_change_weight", std::vector{10, 10, 10}); + handler->setParameter("angular_momentum_weight", 1e2); + handler->setParameter("contact_force_symmetry_weight", 10.0); + handler->setParameter("parameter_regularization_weight", 10.0); + handler->setParameter("payload_weight", 0.0); + + + ParametrizedCentroidalMPC mpc; + + REQUIRE(mpc.initialize(handler)); + + BipedalLocomotion::Contacts::ContactPhaseList phaseList; + BipedalLocomotion::Contacts::ContactListMap contactListMap; + + BipedalLocomotion::Planners::QuinticSpline comSpline; + + constexpr int scaling = 1; + constexpr double scalingPos = 4.0; + constexpr double scalingPosY = 12; + + // // t 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 + // 22 23 24 25 26 27 + // // L + // |+++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|++++++++++|---|+++| + // // R + // |+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++| + + Eigen::Vector3d leftPosition; + leftPosition << 0, 0.08, 0; + manif::SE3d leftTransform(leftPosition, manif::SO3d::Identity()); + contactListMap["left_foot"].addContact(leftTransform, 0s, scaling * 1s); + + leftPosition(0) += 0.05 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 2s * scaling, 5s * scaling); + + leftPosition(0) += 0.1 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 6s * scaling, 9s * scaling); + + leftPosition(0) += 0.05 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 10s * scaling, 13s * scaling); + + leftPosition(0) += 0.15 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 14s * scaling, 17s * scaling); + + leftPosition(1) -= 0.01 * scalingPosY; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 18s * scaling, 21s * scaling); + + leftPosition(1) -= 0.01 * scalingPosY; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 22s * scaling, 25s * scaling); + + leftPosition(1) -= 0.01 * scalingPosY; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 26s * scaling, 29s * scaling); + + // // t 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 + // 22 23 24 25 26 27 + // // L + // |+++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|++++++++++|---|+++| + // // R + // |+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++| + + // right foot + // first footstep + Eigen::Vector3d rightPosition; + rightPosition << 0, -0.08, 0; + manif::SE3d rightTransform(rightPosition, manif::SO3d::Identity()); + + contactListMap["right_foot"].addContact(rightTransform, 0s, 3s * scaling); + + rightPosition(0) += 0.1 * scalingPos; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 4s * scaling, 7s * scaling); + + rightPosition(0) += 0.1 * scalingPos; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 8s * scaling, 11s * scaling); + + rightPosition(0) += 0.1 * scalingPos; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 12s * scaling, 15s * scaling); + + rightPosition(0) += 0.05 * scalingPos; + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 16s * scaling, 19s * scaling); + + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 20s * scaling, 23s * scaling); + + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 24s * scaling, 27s * scaling); + + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 28s * scaling, 29s * scaling); + phaseList.setLists(contactListMap); + + std::vector comKnots; + std::vector timeKnots; + + timeKnots.push_back(phaseList.cbegin()->beginTime); + Eigen::Vector3d com0; + com0 << 0, 0, 0.53; + comKnots.push_back(com0); + for (auto it = phaseList.begin(); it != phaseList.end(); std::advance(it, 1)) + { + if (it->activeContacts.size() == 2 && it != phaseList.begin() + && it != phaseList.lastPhase()) + { + timeKnots.emplace_back((it->endTime + it->beginTime) / 2); + + auto contactIt = it->activeContacts.cbegin(); + const Eigen::Vector3d p1 = contactIt->second->pose.translation(); + std::advance(contactIt, 1); + const Eigen::Vector3d p2 = contactIt->second->pose.translation(); + + Eigen::Vector3d desiredCoMPosition = (p1 + p2) / 2.0; + desiredCoMPosition(2) += com0(2); + + comKnots.emplace_back(desiredCoMPosition); + } + + else if (it->activeContacts.size() == 2 && it == phaseList.lastPhase()) + { + timeKnots.push_back(it->endTime); + auto contactIt = it->activeContacts.cbegin(); + const Eigen::Vector3d p1 = contactIt->second->pose.translation(); + std::advance(contactIt, 1); + const Eigen::Vector3d p2 = contactIt->second->pose.translation(); + + Eigen::Vector3d desiredCoMPosition = (p1 + p2) / 2.0; + desiredCoMPosition(2) += com0(2); + + comKnots.emplace_back(desiredCoMPosition); + } + } + + comSpline.setInitialConditions(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + comSpline.setFinalConditions(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + comSpline.setKnots(comKnots, timeKnots); + + Eigen::Vector3d velocity, acceleration; + std::vector comTraj(700, Eigen::Vector3d::Zero()); + std::vector angularMomentumTraj(700, Eigen::Vector3d::Zero()); + + int tempInt = 500; + for (int i = 0; i < tempInt; i++) + { + comSpline.evaluatePoint(i * dT, comTraj[i], velocity, acceleration); + } + + for (int i = tempInt; i < comTraj.size(); i++) + { + comTraj[i] = comTraj[tempInt - 1]; + } + + // initialize the dynamical system + auto system = std::make_shared(); + system->setState({comTraj[0], Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); + + constexpr std::chrono::nanoseconds integratorStepTime = dT; + ForwardEuler integrator; + integrator.setIntegrationStep(integratorStepTime); + REQUIRE(integrator.setDynamicalSystem(system)); + + std::ofstream centroidalMPCData; + if (saveDataset) + { + centroidalMPCData.open("ParametrizedCentroidalMPCUnitTest.txt"); + } + + int controllerIndex = 0; + int index = 0; + + std::chrono::nanoseconds elapsedTime = 0s; + std::chrono::nanoseconds currentTime = 0s; + auto phaseIt = phaseList.getPresentPhase(currentTime); + + constexpr int simulationHorizon = 50; + std::vector comTrajectoryRecedingHorizon; + for (int i = 0; i < simulationHorizon; i++) + { + const auto& [com, dcom, angularMomentum] = system->getState(); + + if (controllerIndex == 0) + { + // update the phaseList this happens only when a new contact should be established + auto newPhaseIt = phaseList.getPresentPhase(currentTime); + if (newPhaseIt != phaseIt) + { + // check if new contact is established + if (phaseIt->activeContacts.size() == 1 && newPhaseIt->activeContacts.size() == 2) + { + updateContactPhaseList(mpc.getOutput().nextPlannedContact, phaseList); + + // the iterators have been modified we have to compute the new one + phaseIt = phaseList.getPresentPhase(currentTime); + } else + { + // the iterators did not change no need to get the present phase again + phaseIt = newPhaseIt; + } + } + + comTrajectoryRecedingHorizon = {comTraj.begin() + i, comTraj.end() - 1}; + + REQUIRE(mpc.setState(com, dcom, angularMomentum)); + REQUIRE(mpc.setReferenceTrajectory(comTrajectoryRecedingHorizon, angularMomentumTraj)); + REQUIRE(mpc.setContactPhaseList(phaseList)); + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + REQUIRE(mpc.advance()); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + elapsedTime = std::chrono::duration_cast(end - begin); + index++; + currentTime += dT; + } + + if (i == 0 && saveDataset) + { + writeHeaderOfFile(mpc.getOutput().contacts, centroidalMPCData); + } + if (saveDataset) + { + writeResultsToFile(mpc, + com, + angularMomentum, + comTrajectoryRecedingHorizon[0], + elapsedTime, + centroidalMPCData); + } + + system->setControlInput({mpc.getOutput().contacts, Eigen::Vector3d::Zero()}); + REQUIRE(integrator.integrate(0s, integratorStepTime)); + + controllerIndex++; + if (controllerIndex == dT / integratorStepTime) + { + controllerIndex = 0; + } + } + + if (saveDataset) + { + centroidalMPCData.close(); + } + + const auto& [com, dcom, angularMomentum] = system->getState(); + + // We check that the robot walked forward keeping the CoM height almost constant + REQUIRE(com(0) > 0.25); + REQUIRE(std::abs(com(1) - com0(1)) < 0.1); + REQUIRE(std::abs(com(2) - com0(2)) < 0.005); +}