diff --git a/src/ReducedModelControllers/CMakeLists.txt b/src/ReducedModelControllers/CMakeLists.txt index 7c521e7529..19c2a5f68b 100644 --- a/src/ReducedModelControllers/CMakeLists.txt +++ b/src/ReducedModelControllers/CMakeLists.txt @@ -8,10 +8,11 @@ if(FRAMEWORK_COMPILE_ReducedModelControllers) add_bipedal_locomotion_library( NAME ReducedModelControllers - PUBLIC_HEADERS ${H_PREFIX}/CentroidalMPC.h - SOURCES src/CentroidalMPC.cpp + PUBLIC_HEADERS ${H_PREFIX}/ICentroidalMPC.h ${H_PREFIX}/CentroidalMPC.h ${H_PREFIX}/StableCentroidalMPC.h + SOURCES src/CentroidalMPC.cpp src/StableCentroidalMPC.cpp PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts PRIVATE_LINK_LIBRARIES casadi BipedalLocomotion::Math BipedalLocomotion::TextLogging BipedalLocomotion::CasadiConversions SUBDIRECTORIES tests) endif() + diff --git a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h index fbb0652438..40cb899187 100644 --- a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h @@ -18,25 +18,13 @@ #include #include #include +#include namespace BipedalLocomotion { namespace ReducedModelControllers { -/** - * CentroidalMPCOutput contains the output of the CentroidalMPC class - */ -struct CentroidalMPCOutput -{ - /** Map containing the contact name and the associated contact. */ - std::map contacts; - Contacts::ContactPhaseList contactPhaseList; /**< Contact phase list generated by the - CentroidalMPC. */ - std::vector comTrajectory; /**< Desired CoM trajectory generated by the - CentroidalMPC. */ -}; - /** * CentroidalMPC implements a Non-Linear Model Predictive Controller for humanoid robot locomotion * with online step adjustment capabilities. The proposed controller considers the Centroidal @@ -51,7 +39,7 @@ struct CentroidalMPCOutput * Locomotion with Step Adjustment," 2022 International Conference on Robotics and Automation * (ICRA), Philadelphia, PA, USA, 2022, pp. 10412-10419, doi: 10.1109/ICRA46639.2022.9811670. */ -class CentroidalMPC : public System::Source +class CentroidalMPC : public ICentroidalMPC { public: /** @@ -96,7 +84,7 @@ class CentroidalMPC : public System::Source * | `corner_` | `vector` | Position of the corner expressed in the foot frame. I must be from 0 to number_of_corners - 1. | Yes | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler) final; + bool initialize(std::weak_ptr handler) override; // clang-format on /** @@ -105,7 +93,7 @@ class CentroidalMPC : public System::Source * @return True in case of success, false otherwise. * @note This function needs to be called before advance. */ - bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList); + bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) override; /** * Set the state of the centroidal dynamics. @@ -119,7 +107,7 @@ class CentroidalMPC : public System::Source */ bool setState(Eigen::Ref com, Eigen::Ref dcom, - Eigen::Ref angularMomentum); + Eigen::Ref angularMomentum) override; /** * Set the state of the centroidal dynamics. @@ -135,7 +123,7 @@ class CentroidalMPC : public System::Source bool setState(Eigen::Ref com, Eigen::Ref dcom, Eigen::Ref angularMomentum, - const Math::Wrenchd& externalWrench); + const Math::Wrenchd& externalWrench) override; /** * Set the reference trajectories for the CoM and the centroidal angular momentum. @@ -150,25 +138,25 @@ class CentroidalMPC : public System::Source * controller sampling period */ bool setReferenceTrajectory(const std::vector& com, - const std::vector& angularMomentum); + const std::vector& angularMomentum) override; /** * Get the output of the controller * @return a const reference of the output of the controller. */ - const CentroidalMPCOutput& getOutput() const final; + const CentroidalMPCOutput& getOutput() const override; /** * Determines the validity of the object retrieved with getOutput() * @return True if the object is valid, false otherwise. */ - bool isOutputValid() const final; + bool isOutputValid() const override; /** * Perform one control cycle. * @return True if the advance is successfull. */ - bool advance() final; + bool advance() override; private: /** @@ -181,4 +169,5 @@ class CentroidalMPC : public System::Source } // namespace ReducedModelControllers } // namespace BipedalLocomotion + #endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_CENTROIDAL_MPC_H diff --git a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/ICentroidalMPC.h b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/ICentroidalMPC.h new file mode 100644 index 0000000000..5b17507bb5 --- /dev/null +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/ICentroidalMPC.h @@ -0,0 +1,126 @@ +/** + * @file ICentroidalMPC.h + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ICENTROIDAL_MPC_H +#define BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ICENTROIDAL_MPC_H + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#define STR_(x) #x +#define STR(x) STR_(x) + +inline bool casadiVersionIsAtLeast360() +{ + std::string str; + std::stringstream ss(STR(casadi_VERSION)); + + // Use while loop to check the getline() function condition. + int index = 0; + while (getline(ss, str, '.')) + { + if (index == 0 && stoi(str) < 3) + { + return false; + } + if (index == 1 && stoi(str) < 6) + { + return false; + } + index++; + } + + return true; +} + +inline double chronoToSeconds(const std::chrono::nanoseconds& d) +{ + return std::chrono::duration(d).count(); +} + +inline std::vector extractVariablesName(const std::vector& variables) +{ + std::vector variablesName; + variablesName.reserve(variables.size()); + for (const auto& variable : variables) + { + variablesName.push_back(variable.name()); + } + + return variablesName; +} + +template inline auto extractFutureValuesFromState(T& variable) +{ + using Sl = casadi::Slice; + return variable(Sl(), Sl(1, variable.columns())); +} + +template inline auto extractFutureValuesFromState(const T& variable) +{ + using Sl = casadi::Slice; + return variable(Sl(), Sl(1, variable.columns())); +} + + +namespace BipedalLocomotion +{ +namespace ReducedModelControllers +{ + +struct CentroidalMPCOutput +{ + std::map contacts; + Contacts::ContactPhaseList contactPhaseList; /**< Contact phase list generated by the + CentroidalMPC. */ + std::vector comTrajectory; /**< Desired CoM trajectory generated by the + CentroidalMPC. */ +}; + +class ICentroidalMPC : public BipedalLocomotion::System::Source +{ +public: + + virtual bool initialize(std::weak_ptr handler) = 0; + + virtual bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) = 0; + + virtual bool setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum) + = 0; + + virtual bool setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum, + const Math::Wrenchd& externalWrench) + = 0; + + virtual bool setReferenceTrajectory(const std::vector& com, + const std::vector& angularMomentum) + = 0; + + virtual const CentroidalMPCOutput& getOutput() const = 0; + + virtual bool isOutputValid() const = 0; + + virtual bool advance() = 0; +}; + +} // namespace ReducedModelControllers +} // namespace BipedalLocomotion +#endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ICENTROIDAL_MPC_H + diff --git a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/StableCentroidalMPC.h b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/StableCentroidalMPC.h new file mode 100644 index 0000000000..d5b7cd5b65 --- /dev/null +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/StableCentroidalMPC.h @@ -0,0 +1,67 @@ +/** + * @file CentroidalMPC.h + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ADAPTIVE_CENTROIDAL_MPC_H +#define BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ADAPTIVE_CENTROIDAL_MPC_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace ReducedModelControllers +{ + +class StableCentroidalMPC : public ICentroidalMPC +{ +public: + + StableCentroidalMPC(); + + ~StableCentroidalMPC(); + + bool initialize(std::weak_ptr handler) override; + + bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) override; + + bool setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum) override; + + bool setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum, + const Math::Wrenchd& externalWrench) override; + + bool setReferenceTrajectory(const std::vector& com, + const std::vector& angularMomentum) override; + + const CentroidalMPCOutput& getOutput() const override; + + bool isOutputValid() const override; + + bool advance() override; + +private: + + struct Impl; + + std::unique_ptr m_pimpl; /**< Pointer to private implementation */ +}; +} // namespace ReducedModelControllers +} // namespace BipedalLocomotion + + +#endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ADAPTIVE_CENTROIDAL_MPC_H diff --git a/src/ReducedModelControllers/src/CentroidalMPC.cpp b/src/ReducedModelControllers/src/CentroidalMPC.cpp index 20e7b68cb6..2b54989f77 100644 --- a/src/ReducedModelControllers/src/CentroidalMPC.cpp +++ b/src/ReducedModelControllers/src/CentroidalMPC.cpp @@ -20,61 +20,6 @@ using namespace BipedalLocomotion::ReducedModelControllers; using namespace BipedalLocomotion::Contacts; -#define STR_(x) #x -#define STR(x) STR_(x) - -bool casadiVersionIsAtLeast360() -{ - std::string str; - std::stringstream ss(STR(casadi_VERSION)); - - // Use while loop to check the getline() function condition. - int index = 0; - while (getline(ss, str, '.')) - { - if (index == 0 && stoi(str) < 3) - { - return false; - } - if (index == 1 && stoi(str) < 6) - { - return false; - } - index++; - } - - return true; -} - -inline double chronoToSeconds(const std::chrono::nanoseconds& d) -{ - return std::chrono::duration(d).count(); -} - -std::vector extractVariablesName(const std::vector& variables) -{ - std::vector variablesName; - variablesName.reserve(variables.size()); - for (const auto& variable : variables) - { - variablesName.push_back(variable.name()); - } - - return variablesName; -} - -template inline auto extractFutureValuesFromState(T& variable) -{ - using Sl = casadi::Slice; - return variable(Sl(), Sl(1, variable.columns())); -} - -template inline auto extractFutureValuesFromState(const T& variable) -{ - using Sl = casadi::Slice; - return variable(Sl(), Sl(1, variable.columns())); -} - struct CentroidalMPC::Impl { casadi::Opti opti; /**< CasADi opti stack */ @@ -1529,4 +1474,4 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contac m_pimpl->output.contactPhaseList = contactPhaseList; return true; -} +} \ No newline at end of file diff --git a/src/ReducedModelControllers/src/StableCentroidalMPC.cpp b/src/ReducedModelControllers/src/StableCentroidalMPC.cpp new file mode 100644 index 0000000000..92a3573f63 --- /dev/null +++ b/src/ReducedModelControllers/src/StableCentroidalMPC.cpp @@ -0,0 +1,1540 @@ +/** + * @file StableCentroidalMPC.cpp + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion::ReducedModelControllers; +using namespace BipedalLocomotion::Contacts; + + +struct StableCentroidalMPC::Impl +{ + casadi::Opti opti; /**< CasADi opti stack */ + casadi::Function controller; + std::chrono::nanoseconds currentTime{std::chrono::nanoseconds::zero()}; + + CentroidalMPCOutput output; + Contacts::ContactPhaseList contactPhaseList; + Math::LinearizedFrictionCone frictionCone; + + enum class FSM + { + Idle, + Initialized, + OutputValid, + OutputInvalid, + }; + + FSM fsm{FSM::Idle}; + + struct StableConstants + { + const double alpha = 0.3; + const double k1 = 3.0; + }; + StableConstants stableConstants; + + struct CasadiCorner + { + casadi::DM position; + casadi::MX force; + + std::string cornerName; + + CasadiCorner(const std::string& cornerName) + : cornerName(cornerName) + { + } + + CasadiCorner() = default; + + CasadiCorner(const std::string& cornerName, const Corner& other) + : cornerName(cornerName) + { + this->operator=(other); + } + + CasadiCorner& operator=(const Corner& other) + { + this->position.resize(3, 1); + this->position(0, 0) = other.position(0); + this->position(1, 0) = other.position(1); + this->position(2, 0) = other.position(2); + + this->force = casadi::MX::sym(cornerName + "_force", other.force.size(), 1); + + return *this; + } + }; + + struct CasadiContact + { + casadi::MX position; + casadi::MX linearVelocity; + casadi::MX orientation; + casadi::MX isEnabled; + std::vector corners; + + std::string contactName; + + CasadiContact(const std::string& contactName) + : contactName(contactName) + { + } + + CasadiContact& operator=(const DiscreteGeometryContact& other) + { + corners.resize(other.corners.size()); + + for (int i = 0; i < other.corners.size(); i++) + { + this->corners[i].cornerName = contactName + "_" + std::to_string(i); + this->corners[i] = other.corners[i]; + } + + this->orientation = casadi::MX::sym(contactName + "_orientation", 3 * 3); + this->position = casadi::MX::sym(contactName + "_position", 3); + this->linearVelocity = casadi::MX::sym(contactName + "_linear_velocity", 3); + this->isEnabled = casadi::MX::sym(contactName + "_is_enable"); + + return *this; + } + + CasadiContact(const std::string& contactName, const DiscreteGeometryContact& other) + : contactName(contactName) + { + this->operator=(other); + } + }; + + struct CasadiContactWithConstraints : CasadiContact + { + + CasadiContactWithConstraints(const std::string& contactName) + : CasadiContact(contactName) + { + } + + casadi::MX currentPosition; + casadi::MX nominalPosition; + casadi::MX upperLimitPosition; + casadi::MX lowerLimitPosition; + }; + + struct OptimizationSettings + { + int solverVerbosity{0}; /**< Verbosity of ipopt */ + std::string ipoptLinearSolver{"mumps"}; /**< Linear solved used by ipopt */ + double ipoptTolerance{1e-8}; /**< Tolerance of ipopt + (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_tol) */ + int ipoptMaxIteration{3000}; /**< Maximum number of iteration */ + + int horizon; /** contacts; + + /**< Optimization variables from the adaptive redesign*/ + casadi::MX z1; + casadi::MX z2; + casadi::MX thetaHat; + + casadi::MX comReference; + casadi::MX angularMomentumReference; + casadi::MX comCurrent; + casadi::MX dcomCurrent; + casadi::MX angularMomentumCurrent; + casadi::MX externalForce; + casadi::MX externalTorque; + casadi::MX thetaHatCurrent; + + }; + OptimizationVariables optiVariables; /**< Optimization variables */ + + struct ContactsInputs + { + casadi::DM* currentPosition; + casadi::DM* orientation; + casadi::DM* nominalPosition; + casadi::DM* upperLimitPosition; + casadi::DM* lowerLimitPosition; + casadi::DM* isEnabled; + }; + struct ControllerInputs + { + std::map contacts; + + casadi::DM* comReference; + casadi::DM* angularMomentumReference; + casadi::DM* comCurrent; + casadi::DM* dcomCurrent; + casadi::DM* angularMomentumCurrent; + casadi::DM* externalForce; + casadi::DM* externalTorque; + casadi::DM* thetaHatCurrent; + }; + ControllerInputs controllerInputs; /**< The pointers will point to the vectorized input */ + + struct ContactInitialGuess + { + casadi::DM* contactLocation; + std::vector contactForce; + }; + + struct InitialGuess + { + std::map contactsInitialGuess; + + casadi::DM* com; + casadi::DM* angularMomentum; + }; + InitialGuess initialGuess; + + std::vector vectorizedOptiInputs; + + struct Weights + { + Eigen::Vector3d com; + double contactPosition; + Eigen::Vector3d forceRateOfChange; + double angularMomentum; + double contactForceSymmetry; + }; + Weights weights; + + struct ContactBoundingBox + { + Eigen::Vector3d upperLimit; + Eigen::Vector3d lowerLimit; + }; + + std::unordered_map contactBoundingBoxes; + + bool loadContactCorners(std::shared_ptr ptr, + DiscreteGeometryContact& contact) + { + constexpr auto errorPrefix = "[CentroidalMPC::Impl::loadContactCorners]"; + + int numberOfCorners; + if (!ptr->getParameter("number_of_corners", numberOfCorners)) + { + log()->error("{} Unable to get the number of corners."); + return false; + } + contact.corners.resize(numberOfCorners); + + for (std::size_t j = 0; j < numberOfCorners; j++) + { + if (!ptr->getParameter("corner_" + std::to_string(j), contact.corners[j].position)) + { + // prepare the error + std::string cornesNames; + for (std::size_t k = 0; k < numberOfCorners; k++) + { + cornesNames += " corner_" + std::to_string(k); + } + + log()->error("{} Unable to load the corner number {}. Please provide the corners " + "having the following names:{}.", + errorPrefix, + j, + cornesNames); + + return false; + } + } + + return true; + } + + bool loadParameters(std::shared_ptr ptr) + { + constexpr auto logPrefix = "[CentroidalMPC::Impl::loadParameters]"; + + auto getParameter + = [logPrefix](std::shared_ptr ptr, + const std::string& paramName, + auto& param) -> bool { + if (!ptr->getParameter(paramName, param)) + { + log()->error("{} Unable to load the parameter named '{}'.", logPrefix, paramName); + return false; + } + return true; + }; + + auto getOptionalParameter + = [logPrefix](std::shared_ptr ptr, + const std::string& paramName, + auto& param) -> void { + if (!ptr->getParameter(paramName, param)) + { + log()->info("{} Unable to load the parameter named '{}'. The default one will be " + "used '{}'.", + logPrefix, + paramName, + param); + } + }; + + bool ok = getParameter(ptr, "sampling_time", this->optiSettings.samplingTime); + ok = ok && getParameter(ptr, "time_horizon", this->optiSettings.timeHorizon); + + if (!ok) + { + return false; + } + this->optiSettings.horizon + = this->optiSettings.timeHorizon / this->optiSettings.samplingTime; + + int numberOfMaximumContacts = 0; + ok = ok && getParameter(ptr, "number_of_maximum_contacts", numberOfMaximumContacts); + + for (std::size_t i = 0; i < numberOfMaximumContacts; i++) + { + auto contactHandler = ptr->getGroup("CONTACT_" + std::to_string(i)).lock(); + + if (contactHandler == nullptr) + { + log()->error("{} Unable to load the contact {}. Please be sure that CONTACT_{} " + "group exists.", + logPrefix, + i, + i); + return false; + } + + std::string contactName; + ok = ok && getParameter(contactHandler, "contact_name", contactName); + if (!ok) + { + return false; + } + + // set the contact name + this->output.contacts[contactName].name = contactName; + ok = ok + && getParameter(contactHandler, + "bounding_box_upper_limit", + this->contactBoundingBoxes[contactName].upperLimit); + ok = ok + && getParameter(contactHandler, + "bounding_box_lower_limit", + this->contactBoundingBoxes[contactName].lowerLimit); + + if (!this->loadContactCorners(contactHandler, this->output.contacts[contactName])) + { + log()->error("{} Unable to load the contact corners for the contact {}.", + logPrefix, + i); + return false; + } + } + + 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, "angular_momentum_weight", this->weights.angularMomentum); + ok = ok + && getParameter(ptr, + "contact_force_symmetry_weight", + this->weights.contactForceSymmetry); + + // initialize the friction cone + ok = ok && frictionCone.initialize(ptr); + ok = ok && getParameter(ptr, "solver_name", this->optiSettings.solverName); + if (!ok) + { + return false; + } + if (this->optiSettings.solverName != "ipopt" && this->optiSettings.solverName != "sqp") + { + log()->error("{} The solver name '{}' is not supported. The supported solvers are " + "'ipopt' and 'sqp'.", + logPrefix, + this->optiSettings.solverName); + return false; + } + + if (this->optiSettings.solverName == "ipopt") + { + getOptionalParameter(ptr, "linear_solver", this->optiSettings.ipoptLinearSolver); + getOptionalParameter(ptr, "ipopt_tolerance", this->optiSettings.ipoptTolerance); + getOptionalParameter(ptr, "ipopt_max_iteration", this->optiSettings.ipoptMaxIteration); + } else + { + getOptionalParameter(ptr, "jit_compilation", this->optiSettings.isJITEnabled); + getOptionalParameter(ptr, + "number_of_qp_iterations", + this->optiSettings.numberOfQPIterations); + } + + getOptionalParameter(ptr, "solver_verbosity", this->optiSettings.solverVerbosity); + getOptionalParameter(ptr, "is_warm_start_enabled", this->optiSettings.isWarmStartEnabled); + getOptionalParameter(ptr, "is_cse_enabled", this->optiSettings.isCseEnabled); + + return ok; + } + + casadi::Function ode() + { + // Convert DiscreteGeometryContact into a casadiContact object + std::map casadiContacts; + + for (const auto& [key, contact] : this->output.contacts) + { + CasadiContact temp(key); + temp = contact; + auto [contactIt, outcome] = casadiContacts.emplace(key, temp); + } + + // we assume mass equal to 1 + constexpr double mass = 1; + + casadi::MX com = casadi::MX::sym("com_in", 3); + casadi::MX dcom = casadi::MX::sym("dcom_in", 3); + casadi::MX angularMomentum = casadi::MX::sym("angular_momentum_in", 3); + + casadi::MX thetaHat = casadi::MX::sym("theta_hat_in", 3); + casadi::MX comReference = casadi::MX::sym("com_reference_in", 3); + + casadi::MX externalForce = casadi::MX::sym("external_force", 3); + casadi::MX externalTorque = casadi::MX::sym("external_torque", 3); + + casadi::MX ddcom = casadi::MX::sym("ddcom", 3); + casadi::MX angularMomentumDerivative = casadi::MX::sym("angular_momentum_derivative", 3); + + casadi::MX thetaHatDerivative = casadi::MX::sym("theta_hat_derivative", 3); + + casadi::DM gravity = casadi::DM::zeros(3); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + ddcom = gravity + externalForce / mass; + angularMomentumDerivative = externalTorque; + + thetaHatDerivative = - this->stableConstants.k1 * (com - comReference) + dcom; // thetaHatDot = -z_2 , assuming accelaration ref being zero + + std::vector input; + input.push_back(comReference); + input.push_back(thetaHat); + input.push_back(externalForce); + input.push_back(externalTorque); + input.push_back(com); + input.push_back(dcom); + input.push_back(angularMomentum); + + for (const auto& [key, contact] : casadiContacts) + { + input.push_back(contact.position); + input.push_back(contact.orientation); + input.push_back(contact.isEnabled); + input.push_back(contact.linearVelocity); + + for (const auto& corner : contact.corners) + { + using namespace casadi; + ddcom += contact.isEnabled / mass * corner.force; + angularMomentumDerivative + += contact.isEnabled + * MX::cross(MX::mtimes(MX::reshape(contact.orientation, 3, 3), + corner.position) + + contact.position - com, + corner.force); + + input.push_back(corner.force); + } + } + + const double dT = chronoToSeconds(this->optiSettings.samplingTime); + + std::vector outputName{"com", "dcom", "angular_momentum", "theta_hat"}; + std::vector rhs{com + dcom * dT, + dcom + ddcom * dT, + angularMomentum + angularMomentumDerivative * dT, + thetaHat + thetaHatDerivative * dT}; + + for (const auto& [key, contact] : casadiContacts) + { + rhs.push_back(contact.position + (1 - contact.isEnabled) * contact.linearVelocity * dT); + outputName.push_back(key); + } + + return casadi::Function("centroidal_dynamics", + std::move(input), + std::move(rhs), + extractVariablesName(input), + std::move(outputName)); + } + + casadi::Function contactPositionError() + { + casadi::MX contactPosition = casadi::MX::sym("contact_position", 3); + casadi::MX nominalContactPosition = casadi::MX::sym("nominal_contact_position", 3); + casadi::MX contactOrientation = casadi::MX::sym("contact_orientation", 3 * 3); + + // the orientation is stored as a vectorized version of the matrix. We need to reshape it + casadi::MX rhs = casadi::MX::mtimes(casadi::MX::reshape(contactOrientation, 3, 3).T(), + contactPosition - nominalContactPosition); + + return casadi::Function("contact_position_error", + {contactPosition, nominalContactPosition, contactOrientation}, + {rhs}, + extractVariablesName({contactPosition, // + nominalContactPosition, + contactOrientation}), + {"error"}); + } + + void resizeControllerInputs() + { + constexpr int vector3Size = 3; + const int stateHorizon = this->optiSettings.horizon + 1; + + // resize the CoM Trajectory + this->output.comTrajectory.resize(stateHorizon); + + // In case of no warmstart the variables are: + // - centroidalVariables = 8: external force + external torque + com current + dcom current + // + current angular momentum + com reference + // + angular momentum reference + thetaHat current + // - contactVariables = 6: for each contact we have current position + nominal position + + // orientation + is enabled + upper limit in position + // + lower limit in position + constexpr std::size_t centroidalVariables = 8; + constexpr std::size_t contactVariables = 6; + + std::size_t vectorizedOptiInputsSize = centroidalVariables + // + (this->output.contacts.size() * contactVariables); + + if (this->optiSettings.isWarmStartEnabled) + { + // in this case we need to add the com, the angular momentum and the contact location + constexpr std::size_t centroidalVariablesWarmStart = 2; + constexpr std::size_t contactVariablesWarmStart = 1; + vectorizedOptiInputsSize += centroidalVariablesWarmStart + // + (this->output.contacts.size() * contactVariablesWarmStart); + + for (const auto& [key, contact] : this->output.contacts) + { + for (const auto& corner : contact.corners) + { + // for each corner we have the force + vectorizedOptiInputsSize += 1; + } + } + } + + // we reserve in advance so the push_back will not invalidate the pointers + // Indeed the standard guarantees that if the new size() is greater than capacity() then all + // iterators and references (including the end() iterator) are invalidated. Otherwise only + // the end() iterator is invalidated. + // https://en.cppreference.com/w/cpp/container/vector/push_back + this->vectorizedOptiInputs.reserve(vectorizedOptiInputsSize); + + // prepare the controller inputs struct + // The order matches the one required by createController + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, // + this->optiSettings.horizon)); + this->controllerInputs.thetaHatCurrent = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, // + this->optiSettings.horizon)); + this->controllerInputs.externalForce = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, // + this->optiSettings.horizon)); + this->controllerInputs.externalTorque = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size)); + this->controllerInputs.comCurrent = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size)); + this->controllerInputs.dcomCurrent = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size)); + this->controllerInputs.angularMomentumCurrent = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, stateHorizon)); + this->controllerInputs.comReference = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, stateHorizon)); + this->controllerInputs.angularMomentumReference = &this->vectorizedOptiInputs.back(); + + if (this->optiSettings.isWarmStartEnabled) + { + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, stateHorizon)); + this->initialGuess.com = &this->vectorizedOptiInputs.back(); + + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, stateHorizon)); + this->initialGuess.angularMomentum = &this->vectorizedOptiInputs.back(); + } + + for (const auto& [key, contact] : this->output.contacts) + { + // The current position of the contact + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size)); + this->controllerInputs.contacts[key].currentPosition + = &this->vectorizedOptiInputs.back(); + + // The nominal contact position is a parameter that regularize the solution + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, stateHorizon)); + this->controllerInputs.contacts[key].nominalPosition + = &this->vectorizedOptiInputs.back(); + + // The orientation is stored as a vectorized version of the rotation matrix + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(9, this->optiSettings.horizon)); + this->controllerInputs.contacts[key].orientation = &this->vectorizedOptiInputs.back(); + + // Maximum admissible contact force. It is expressed in the contact body frame + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(1, this->optiSettings.horizon)); + this->controllerInputs.contacts[key].isEnabled = &this->vectorizedOptiInputs.back(); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + this->vectorizedOptiInputs.push_back( + casadi::DM::zeros(vector3Size, this->optiSettings.horizon)); + this->controllerInputs.contacts[key].upperLimitPosition + = &this->vectorizedOptiInputs.back(); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + this->vectorizedOptiInputs.push_back( + casadi::DM::zeros(vector3Size, this->optiSettings.horizon)); + this->controllerInputs.contacts[key].lowerLimitPosition + = &this->vectorizedOptiInputs.back(); + + if (this->optiSettings.isWarmStartEnabled) + { + this->vectorizedOptiInputs.push_back(casadi::DM::zeros(vector3Size, stateHorizon)); + this->initialGuess.contactsInitialGuess[key].contactLocation + = &this->vectorizedOptiInputs.back(); + + for (const auto& corner : contact.corners) + { + this->vectorizedOptiInputs.push_back( + casadi::DM::zeros(vector3Size, this->optiSettings.horizon)); + this->initialGuess.contactsInitialGuess[key].contactForce.push_back( + &this->vectorizedOptiInputs.back()); + } + } + } + + assert(vectorizedOptiInputsSize == this->vectorizedOptiInputs.size()); + } + + void populateOptiVariables() + { + constexpr int vector3Size = 3; + const int stateHorizon = this->optiSettings.horizon + 1; + + // create the variables for the state + this->optiVariables.com = this->opti.variable(vector3Size, stateHorizon); + this->optiVariables.dcom = this->opti.variable(vector3Size, stateHorizon); + this->optiVariables.angularMomentum = this->opti.variable(vector3Size, stateHorizon); + + // create the variables for the stability + this->optiVariables.z1 = this->opti.variable(vector3Size); + this->optiVariables.z2 = this->opti.variable(vector3Size); + this->optiVariables.thetaHat = this->opti.variable(vector3Size, stateHorizon); + + + // the casadi contacts depends on the maximum number of contacts + for (const auto& [key, contact] : this->output.contacts) + { + auto [contactIt, outcome] + = this->optiVariables.contacts.insert_or_assign(key, + CasadiContactWithConstraints(key)); + + auto& c = contactIt->second; + + // each contact has a different number of corners + c.corners.resize(contact.corners.size()); + + // the position of the contact is an optimization variable + c.position = this->opti.variable(vector3Size, stateHorizon); + + // the linear velocity of the contact is an optimization variable + c.linearVelocity = this->opti.variable(vector3Size, this->optiSettings.horizon); + + // the orientation is a parameter. The orientation is stored as a vectorized version of + // the rotation matrix + c.orientation = this->opti.parameter(9, this->optiSettings.horizon); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + c.upperLimitPosition = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + c.lowerLimitPosition = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + // Maximum admissible contact force. It is expressed in the contact body frame + c.isEnabled = this->opti.parameter(1, this->optiSettings.horizon); + + // The nominal contact position is a parameter that regularize the solution + c.nominalPosition = this->opti.parameter(vector3Size, stateHorizon); + + c.currentPosition = this->opti.parameter(vector3Size); + + for (int j = 0; j < contact.corners.size(); j++) + { + c.corners[j].force = this->opti.variable(vector3Size, this->optiSettings.horizon); + + c.corners[j].position + = casadi::DM(std::vector(contact.corners[j].position.data(), + contact.corners[j].position.data() + + contact.corners[j].position.size())); + } + } + + this->optiVariables.comCurrent = this->opti.parameter(vector3Size); + this->optiVariables.dcomCurrent = this->opti.parameter(vector3Size); + this->optiVariables.angularMomentumCurrent = this->opti.parameter(vector3Size); + this->optiVariables.comReference = this->opti.parameter(vector3Size, stateHorizon); + this->optiVariables.angularMomentumReference + = this->opti.parameter(vector3Size, stateHorizon); + this->optiVariables.externalForce = this->opti.parameter(vector3Size, // + this->optiSettings.horizon); + this->optiVariables.externalTorque = this->opti.parameter(vector3Size, // + this->optiSettings.horizon); + + this->optiVariables.thetaHatCurrent = this->opti.parameter(vector3Size); + } + + /** + * Setup the optimization problem options + */ + void setupOptiOptions() + { + casadi::Dict casadiOptions; + casadi::Dict solverOptions; + if (this->optiSettings.solverName == "ipopt") + { + if (this->optiSettings.solverVerbosity != 0) + { + casadi_int ipoptVerbosity + = static_cast(optiSettings.solverVerbosity - 1); + solverOptions["print_level"] = ipoptVerbosity; + casadiOptions["print_time"] = true; + } else + { + solverOptions["print_level"] = 0; + casadiOptions["print_time"] = false; + } + + solverOptions["max_iter"] = this->optiSettings.ipoptMaxIteration; + solverOptions["tol"] = this->optiSettings.ipoptTolerance; + solverOptions["linear_solver"] = this->optiSettings.ipoptLinearSolver; + casadiOptions["expand"] = true; + casadiOptions["error_on_fail"] = true; + + this->opti.solver("ipopt", casadiOptions, solverOptions); + return; + } + + // if not ipopt it is sqpmethod + casadi::Dict osqpOptions; + if (this->optiSettings.solverVerbosity != 0) + { + casadiOptions["print_header"] = true; + casadiOptions["print_iteration"] = true; + casadiOptions["print_status"] = true; + casadiOptions["print_time"] = true; + osqpOptions["verbose"] = true; + } else + { + casadiOptions["print_header"] = false; + casadiOptions["print_iteration"] = false; + casadiOptions["print_status"] = false; + casadiOptions["print_time"] = false; + osqpOptions["verbose"] = false; + } + casadiOptions["error_on_fail"] = false; + casadiOptions["expand"] = true; + casadiOptions["qpsol"] = "osqp"; + + solverOptions["error_on_fail"] = false; + + osqpOptions["verbose"] = false; + solverOptions["osqp"] = osqpOptions; + + casadiOptions["qpsol_options"] = solverOptions; + casadiOptions["max_iter"] = this->optiSettings.numberOfQPIterations; + + if (this->optiSettings.isJITEnabled) + { + casadiOptions["jit"] = true; + casadiOptions["compiler"] = "shell"; + + casadi::Dict jitOptions; + jitOptions["flags"] = {"-O3"}; + jitOptions["verbose"] = true; + casadiOptions["jit_options"] = jitOptions; + } + this->opti.solver("sqpmethod", casadiOptions); + } + + casadi::Function createController() + { + using Sl = casadi::Slice; + + this->populateOptiVariables(); + + // get the variables to simplify the readability + auto& com = this->optiVariables.com; + auto& dcom = this->optiVariables.dcom; + auto& angularMomentum = this->optiVariables.angularMomentum; + auto& externalForce = this->optiVariables.externalForce; + auto& externalTorque = this->optiVariables.externalTorque; + + auto& z1 = this->optiVariables.z1; + auto& z2 = this->optiVariables.z2; + + auto& thetaHat = this->optiVariables.thetaHat; + + auto& comReference = this->optiVariables.comReference; + auto& angularMomentumReference = this->optiVariables.angularMomentumReference; + + // prepare the input of the ode + std::vector odeInput; + odeInput.push_back(comReference(Sl(), Sl(0, -1))); + odeInput.push_back(thetaHat(Sl(), Sl(0, -1))); + odeInput.push_back(externalForce); + odeInput.push_back(externalTorque); + odeInput.push_back(com(Sl(), Sl(0, -1))); + odeInput.push_back(dcom(Sl(), Sl(0, -1))); + odeInput.push_back(angularMomentum(Sl(), Sl(0, -1))); + + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + odeInput.push_back(contact.position(Sl(), Sl(0, -1))); + odeInput.push_back(contact.orientation); + odeInput.push_back(contact.isEnabled); + odeInput.push_back(contact.linearVelocity); + + for (const auto& corner : contact.corners) + { + odeInput.push_back(corner.force); + } + } + + // set the feedback + this->opti.subject_to(this->optiVariables.comCurrent == com(Sl(), 0)); + this->opti.subject_to(this->optiVariables.dcomCurrent == dcom(Sl(), 0)); + this->opti.subject_to(this->optiVariables.angularMomentumCurrent + == angularMomentum(Sl(), 0)); + + this->opti.subject_to(this->optiVariables.thetaHatCurrent == thetaHat(Sl(), 0)); + + // stability constraint + + this->opti.subject_to(z1 == com(Sl(),1) - this->optiVariables.comReference(Sl(), 1)); + this->opti.subject_to(z2 == dcom(Sl(),1) + this->stableConstants.k1 * (com(Sl(),1) - this->optiVariables.comReference(Sl(), 1))); + + casadi::DM gravity = casadi::DM::zeros(3); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + auto u_adaptive = -2 * this->stableConstants.k1 * z2 + (this->stableConstants.k1 * this->stableConstants.k1) * z1 - gravity - this->optiVariables.thetaHatCurrent ; // assuming accelaration ref zero + casadi::MX averageForce; + for (const auto& [key, contact] : this->optiVariables.contacts) + { + averageForce = casadi::MX::vertcat( + {contact.isEnabled * contact.corners[0].force(0, Sl()) / contact.corners.size(), + contact.isEnabled * contact.corners[0].force(1, Sl()) / contact.corners.size(), + contact.isEnabled * contact.corners[0].force(2, Sl()) / contact.corners.size()}); + for (int i = 1; i < contact.corners.size(); i++) + { + averageForce += casadi::MX::vertcat( + {contact.isEnabled * contact.corners[i].force(0, Sl()) / contact.corners.size(), + contact.isEnabled * contact.corners[i].force(1, Sl()) / contact.corners.size(), + contact.isEnabled * contact.corners[i].force(2, Sl()) + / contact.corners.size()}); + } + } + this->opti.subject_to( + -casadi::MX::mtimes(z1(Sl(), 0).T(), z1(Sl(), 0)) + - casadi::MX::mtimes(z2(Sl(), 0).T(), z2(Sl(), 0)) + + casadi::MX::mtimes(z1(Sl(), 0).T(), z2(Sl(), 0)) + + casadi::MX::mtimes(z2(Sl(), 0).T(), averageForce - u_adaptive) + <= 0.0); + + this->opti.subject_to(casadi::MX::mtimes(angularMomentum(Sl(),1).T() , angularMomentum(Sl(),1)) <= this->stableConstants.alpha ); + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + this->opti.subject_to(this->optiVariables.contacts.at(key).currentPosition + == contact.position(Sl(), 0)); + } + + // set the dynamics + // map computes the multiple shooting method + auto dynamics = this->ode().map(this->optiSettings.horizon); + auto fullTrajectory = dynamics(odeInput); + this->opti.subject_to(extractFutureValuesFromState(com) == fullTrajectory[0]); + this->opti.subject_to(extractFutureValuesFromState(dcom) == fullTrajectory[1]); + this->opti.subject_to(extractFutureValuesFromState(angularMomentum) == fullTrajectory[2]); + this->opti.subject_to(extractFutureValuesFromState(thetaHat) == fullTrajectory[3]); + + // footstep dynamics + std::size_t contactIndex = 0; + for (const auto& [key, contact] : this->optiVariables.contacts) + { + this->opti.subject_to(extractFutureValuesFromState(contact.position) + == fullTrajectory[4 + contactIndex]); + contactIndex++; + } + + // add constraints for the contacts + auto contactPositionErrorMap = this->contactPositionError().map(this->optiSettings.horizon); + + // convert the eigen matrix into casadi + // please check https://github.com/casadi/casadi/issues/2563 and + // https://groups.google.com/forum/#!topic/casadi-users/npPcKItdLN8 + // Assumption: the matrices are stored as column-major + casadi::DM frictionConeMatrix = casadi::DM::zeros(frictionCone.getA().rows(), // + frictionCone.getA().cols()); + + std::memcpy(frictionConeMatrix.ptr(), + frictionCone.getA().data(), + sizeof(double) * frictionCone.getA().rows() * frictionCone.getA().cols()); + + const casadi::DM zero = casadi::DM::zeros(frictionCone.getA().rows(), 1); + casadi::MX rotatedFrictionCone; + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + auto error + = contactPositionErrorMap({extractFutureValuesFromState(contact.position), + extractFutureValuesFromState(contact.nominalPosition), + contact.orientation}); + + this->opti.subject_to(contact.lowerLimitPosition <= error[0] + <= contact.upperLimitPosition); + + for (int i = 0; i < this->optiSettings.horizon; i++) + { + rotatedFrictionCone + = casadi::MX::mtimes(frictionConeMatrix, // + casadi::MX::reshape(contact.orientation(Sl(), i), 3, 3) + .T()); + + // TODO please if you want to add heel to toe motion you should define a + // contact.maximumNormalForce for each corner. At this stage is too premature. + for (const auto& corner : contact.corners) + { + this->opti.subject_to(casadi::MX::mtimes(rotatedFrictionCone, // + corner.force(Sl(), i)) + <= zero); + + // limit on the normal force + this->opti.subject_to( + 0 <= casadi::MX::mtimes(casadi::MX::reshape(contact.orientation(Sl(), i), + 3, + 3), + corner.force(Sl(), i)(2))); + } + } + } + + // create the cost function + + // (max - mix) * exp(-i) + min + casadi::DM weightCoMZ = casadi::DM::zeros(1, com.columns()); + const double min = this->weights.com(2) / 2; + for (int i = 0; i < com.columns(); i++) + { + 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()))); + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + cost += this->weights.contactPosition + * casadi::MX::sumsqr(contact.nominalPosition - contact.position); + for (const auto& corner : contact.corners) + { + auto forceRateOfChange = casadi::MX::diff(corner.force.T()).T(); + + cost += this->weights.contactForceSymmetry + * casadi::MX::sumsqr(corner.force - averageForce); + + cost += this->weights.forceRateOfChange(0) + * casadi::MX::sumsqr(forceRateOfChange(0, Sl())); + cost += this->weights.forceRateOfChange(1) + * casadi::MX::sumsqr(forceRateOfChange(1, Sl())); + cost += this->weights.forceRateOfChange(2) + * casadi::MX::sumsqr(forceRateOfChange(2, Sl())); + } + } + + this->opti.minimize(cost); + + this->setupOptiOptions(); + + // prepare the casadi function + std::vector input; + std::vector output; + std::vector inputName; + std::vector outputName; + + auto concatenateInput + = [&input, &inputName](const casadi::MX& inputVariable, std::string inputVariableName) { + input.push_back(inputVariable); + inputName.push_back(std::move(inputVariableName)); + }; + + auto concatenateOutput = [&output, &outputName](const casadi::MX& outputVariable, + std::string outputVariableName) { + output.push_back(outputVariable); + outputName.push_back(std::move(outputVariableName)); + }; + + concatenateInput(this->optiVariables.thetaHatCurrent, "theta_hat_current"); + concatenateInput(this->optiVariables.externalForce, "external_force"); + concatenateInput(this->optiVariables.externalTorque, "external_torque"); + concatenateInput(this->optiVariables.comCurrent, "com_current"); + concatenateInput(this->optiVariables.dcomCurrent, "dcom_current"); + concatenateInput(this->optiVariables.angularMomentumCurrent, "angular_momentum_current"); + + concatenateInput(this->optiVariables.comReference, "com_reference"); + concatenateInput(this->optiVariables.angularMomentumReference, + "angular_momentum_reference"); + + // this only if warm-start is enabled + if (this->optiSettings.isWarmStartEnabled) + { + concatenateInput(this->optiVariables.com, "com_warmstart"); + concatenateInput(this->optiVariables.angularMomentum, "angular_momentum_warmstart"); + } + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + concatenateInput(contact.currentPosition, "contact_" + key + "_current_position"); + concatenateInput(contact.nominalPosition, "contact_" + key + "_nominal_position"); + concatenateInput(contact.orientation, "contact_" + key + "_orientation_input"); + concatenateInput(contact.isEnabled, "contact_" + key + "is_enable_in"); + concatenateInput(contact.upperLimitPosition, + "contact_" + key + "_upper_limit_position"); + concatenateInput(contact.lowerLimitPosition, + "contact_" + key + "_lower_limit_position"); + + // this only if warm-start is enabled + if (this->optiSettings.isWarmStartEnabled) + { + concatenateInput(contact.position, "contact_" + key + "_position_warmstart"); + + std::size_t cornerIndex = 0; + for (const auto& corner : contact.corners) + { + concatenateInput(corner.force, + "contact_" + key + "_corner_" + std::to_string(cornerIndex) + + "_force_warmstart"); + cornerIndex++; + } + } + + concatenateOutput(contact.isEnabled, "contact_" + key + "_is_enable"); + concatenateOutput(contact.position, "contact_" + key + "_position"); + concatenateOutput(contact.orientation, "contact_" + key + "_orientation"); + + std::size_t cornerIndex = 0; + for (const auto& corner : contact.corners) + { + concatenateOutput(corner.force, + "contact_" + key + "_corner_" + std::to_string(cornerIndex) + + "_force"); + cornerIndex++; + } + } + + concatenateOutput(this->optiVariables.com, "com"); + + casadi::Dict toFunctionOptions, jitOptions; + if (casadiVersionIsAtLeast360()) + { + toFunctionOptions["cse"] = this->optiSettings.isCseEnabled; + } + + return this->opti + .to_function("controller", input, output, inputName, outputName, toFunctionOptions); + } +}; + +bool StableCentroidalMPC::initialize(std::weak_ptr handler) +{ + constexpr auto errorPrefix = "[CentroidalMPC::initialize]"; + auto ptr = handler.lock(); + + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + if (!m_pimpl->loadParameters(ptr)) + { + log()->error("{} Unable to load the parameters.", errorPrefix); + return false; + } + + m_pimpl->resizeControllerInputs(); + m_pimpl->controller = m_pimpl->createController(); + m_pimpl->fsm = Impl::FSM::Initialized; + + return true; +} + +StableCentroidalMPC::~StableCentroidalMPC() = default; + +StableCentroidalMPC::StableCentroidalMPC() +{ + m_pimpl = std::make_unique(); +} + +const CentroidalMPCOutput& StableCentroidalMPC::getOutput() const +{ + return m_pimpl->output; +} + + + +bool StableCentroidalMPC::isOutputValid() const +{ + return m_pimpl->fsm == Impl::FSM::OutputValid; +} + +bool StableCentroidalMPC::advance() +{ + constexpr auto errorPrefix = "[CentroidalMPC::advance]"; + assert(m_pimpl); + + using Sl = casadi::Slice; + + if (m_pimpl->fsm == Impl::FSM::Idle) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + // invalidate the output + m_pimpl->fsm = Impl::FSM::OutputInvalid; + + // compute the output + std::vector controllerOutput; + try + { + controllerOutput = m_pimpl->controller(m_pimpl->vectorizedOptiInputs); + } catch (const std::exception& e) + { + log()->error("{} Unable to solve the problem. The following exception has been thrown {}.", + errorPrefix, + e.what()); + return false; + } + + // get the solution + auto it = controllerOutput.begin(); + + ContactListMap contactListMap = m_pimpl->output.contactPhaseList.lists(); + for (auto& [key, contact] : m_pimpl->output.contacts) + { + ContactList& contactList = contactListMap.at(key); + + // this is required for toEigen + using namespace BipedalLocomotion::Conversions; + + int index = toEigen(*it).size(); + const int size = toEigen(*it).size(); + for (int i = 0; i < size; i++) + { + if (toEigen(*it)(i) > 0.5) + { + if (i == 0) + { + break; + } else if (toEigen(*it)(i - 1) < 0.5) + { + index = i; + break; + } + } + } + + double isEnabled = toEigen(*it)(0); + std::advance(it, 1); + contact.pose.translation(toEigen(*it).leftCols<1>()); + + if (index < size) + { + const std::chrono::nanoseconds nextPlannedContactTime + = m_pimpl->currentTime + m_pimpl->optiSettings.samplingTime * index; + + auto nextPlannedContact = contactList.getPresentContact(nextPlannedContactTime); + if (nextPlannedContact == contactList.end()) + { + log()->error("[CentroidalMPC::advance] Unable to get the next planned contact"); + return false; + } + + PlannedContact modifiedNextPlannedContact = *nextPlannedContact; + + // only the position is modified by the MPC + modifiedNextPlannedContact.pose.translation(toEigen(*it).col(index)); + + if (!contactList.editContact(nextPlannedContact, modifiedNextPlannedContact)) + { + log()->error("{} Unable to edit the next planned contact at time {}. The contact " + "list contains the following contacts: {}", + errorPrefix, + std::chrono::duration_cast( + nextPlannedContactTime), + contactList.toString()); + return false; + } + } + + std::advance(it, 1); + + // get the orientation + contact.pose.quat(Eigen::Quaterniond( + Eigen::Map(toEigen(*it).leftCols<1>().data()))); + + // get the forces + std::advance(it, 1); + + for (std::size_t cornerIndex = 0; cornerIndex < contact.corners.size(); cornerIndex++) + { + if (m_pimpl->optiSettings.isWarmStartEnabled) + { + toEigen(*m_pimpl->initialGuess.contactsInitialGuess[key].contactForce[cornerIndex]) + .leftCols(m_pimpl->optiSettings.horizon - 1) + = toEigen(*it).rightCols(m_pimpl->optiSettings.horizon - 1); + toEigen(*m_pimpl->initialGuess.contactsInitialGuess[key].contactForce[cornerIndex]) + .rightCols<1>() + = toEigen(*it).rightCols<1>(); + } + if (isEnabled > 0.5) + { + contact.corners[cornerIndex].force = toEigen(*it).leftCols<1>(); + } else + { + contact.corners[cornerIndex].force.setZero(); + } + + std::advance(it, 1); + } + } + + // update the contact phase list + m_pimpl->output.contactPhaseList.setLists(contactListMap); + + for (int i = 0; i < m_pimpl->output.comTrajectory.size(); i++) + { + using namespace BipedalLocomotion::Conversions; + m_pimpl->output.comTrajectory[i] = toEigen(*it).col(i); + } + // advance the time + m_pimpl->currentTime += m_pimpl->optiSettings.samplingTime; + + // Make the output valid + m_pimpl->fsm = Impl::FSM::OutputValid; + + return true; +} + +bool StableCentroidalMPC::setReferenceTrajectory(const std::vector& com, + const std::vector& angularMomentum) +{ + constexpr auto errorPrefix = "[CentroidalMPC::setReferenceTrajectory]"; + + const int stateHorizon = m_pimpl->optiSettings.horizon + 1; + + if (m_pimpl->fsm == Impl::FSM::Idle) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + if (com.size() < stateHorizon) + { + log()->error("{} The CoM trajectory vector should have at least {} elements. Provided " + "size: {}.", + errorPrefix, + stateHorizon, + com.size()); + return false; + } + + if (angularMomentum.size() < stateHorizon) + { + log()->error("{} The angular momentum trajectory vector should have at least {} elements. " + "Provided size: {}.", + errorPrefix, + stateHorizon, + angularMomentum.size()); + return false; + } + + // Since Eigen vector is a contiguous we can copy the CoM and the angular momentum references by + // columns. + for (int i = 0; i < stateHorizon; i++) + { + using namespace BipedalLocomotion::Conversions; + toEigen(*(m_pimpl->controllerInputs.comReference)).col(i) + = Eigen::Map(com[i].data()); + toEigen(*(m_pimpl->controllerInputs.angularMomentumReference)).col(i) + = Eigen::Map(angularMomentum[i].data()); + } + + // if the warmstart is enabled then the reference is used also as warmstart + if (m_pimpl->optiSettings.isWarmStartEnabled) + { + using namespace BipedalLocomotion::Conversions; + toEigen(*(m_pimpl->initialGuess.com)) = toEigen(*(m_pimpl->controllerInputs.comReference)); + toEigen(*(m_pimpl->initialGuess.angularMomentum)) + = toEigen(*(m_pimpl->controllerInputs.angularMomentumReference)); + } + + return true; +} + +bool StableCentroidalMPC::setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum) +{ + const Math::Wrenchd dummy = Math::Wrenchd::Zero(); + return this->setState(com, dcom, angularMomentum, dummy); +} + +bool StableCentroidalMPC::setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum, + const Math::Wrenchd& externalWrench) +{ + constexpr auto errorPrefix = "[CentroidalMPC::setState]"; + assert(m_pimpl); + + if (m_pimpl->fsm == Impl::FSM::Idle) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + auto& inputs = m_pimpl->controllerInputs; + + using namespace BipedalLocomotion::Conversions; + toEigen(*inputs.comCurrent) = com; + toEigen(*inputs.dcomCurrent) = dcom; + toEigen(*inputs.angularMomentumCurrent) = angularMomentum; + + toEigen(*inputs.externalForce).setZero(); + toEigen(*inputs.externalTorque).setZero(); + + toEigen(*inputs.externalForce).leftCols<1>() = externalWrench.force(); + toEigen(*inputs.externalTorque).leftCols<1>() = externalWrench.torque(); + + return true; +} + +bool StableCentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) +{ + constexpr auto errorPrefix = "[CentroidalMPC::setContactPhaseList]"; + assert(m_pimpl); + + if (m_pimpl->fsm == Impl::FSM::Idle) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + if (contactPhaseList.size() == 0) + { + log()->error("{} The contactPhaseList is empty.", errorPrefix); + return false; + } + + for (const auto& [key, list] : contactPhaseList.lists()) + { + if (!list.areContactsSampled(m_pimpl->optiSettings.samplingTime)) + { + log()->error("{} The contact list {} is not sampled at the sampling time {}. Please " + "resample the contacts lists before calling this method.", + errorPrefix, + key, + std::chrono::duration_cast( + m_pimpl->optiSettings.samplingTime)); + return false; + } + } + + m_pimpl->contactPhaseList = contactPhaseList; + + // The orientation is stored as a vectorized version of the rotation matrix + const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity(); + + auto& inputs = m_pimpl->controllerInputs; + + // clear previous data + for (const auto& [key, contact] : m_pimpl->output.contacts) + { + using namespace BipedalLocomotion::Conversions; + + // initialize the current contact pose to zero. If the contact is active the current + // position will be set later on + toEigen(*inputs.contacts[key].currentPosition).setZero(); + + // initialize all the orientation to the identity + toEigen(*inputs.contacts[key].orientation).colwise() + = Eigen::Map(identity.data(), identity.cols() * identity.rows()); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + toEigen(*inputs.contacts[key].upperLimitPosition).setZero(); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + toEigen(*inputs.contacts[key].lowerLimitPosition).setZero(); + + // Maximum admissible contact force. It is expressed in the contact body frame + toEigen(*inputs.contacts[key].isEnabled).setZero(); + + // The nominal contact position is a parameter that regularize the solution + toEigen(*inputs.contacts[key].nominalPosition).setZero(); + } + + const std::chrono::nanoseconds absoluteTimeHorizon + = m_pimpl->currentTime + m_pimpl->optiSettings.timeHorizon; + + // find the contactPhase associated to the current time + auto initialPhase = contactPhaseList.getPresentPhase(m_pimpl->currentTime); + if (initialPhase == contactPhaseList.end()) + { + log()->error("{} Unable to find the contact phase related to the current at time {}. The " + "contact " + "list contains the following contacts: {}", + errorPrefix, + std::chrono::duration_cast(m_pimpl->currentTime), + contactPhaseList.toString()); + return false; + } + + // find the contactPhase associated to the end time + auto finalPhase = contactPhaseList.getPresentPhase(absoluteTimeHorizon); + // if the list is not found the latest contact phase is considered + if (finalPhase == contactPhaseList.end()) + { + finalPhase = std::prev(contactPhaseList.end()); + return false; + } + + int index = 0; + for (auto it = initialPhase; it != std::next(finalPhase); std::advance(it, 1)) + { + const std::chrono::nanoseconds tInitial = std::max(m_pimpl->currentTime, it->beginTime); + const std::chrono::nanoseconds tFinal = std::min(absoluteTimeHorizon, it->endTime); + + const std::chrono::nanoseconds duration = tFinal - tInitial; + const int numberOfSamples = duration / m_pimpl->optiSettings.samplingTime; + + for (const auto& [key, contact] : it->activeContacts) + { + using namespace BipedalLocomotion::Conversions; + + auto inputContact = inputs.contacts.find(key); + if (inputContact == inputs.contacts.end()) + { + log()->error("{} Unable to find the input contact named {}.", errorPrefix, key); + return false; + } + + toEigen(*(inputContact->second.nominalPosition)) + .middleCols(index, numberOfSamples + 1) + .colwise() + = contact->pose.translation(); + + // this is required to reshape the matrix into a vector + const Eigen::Matrix3d orientation = contact->pose.quat().toRotationMatrix(); + toEigen(*(inputContact->second.orientation)).middleCols(index, numberOfSamples).colwise() + = Eigen::Map(orientation.data(), orientation.size()); + + constexpr double isEnabled = 1; + toEigen(*(inputContact->second.isEnabled)) + .middleCols(index, numberOfSamples) + .setConstant(isEnabled); + } + + index += numberOfSamples; + } + + assert(index == m_pimpl->optiSettings.horizon); + + // set the current contact position to for the active contact only + for (auto& [key, contact] : inputs.contacts) + { + using namespace BipedalLocomotion::Conversions; + + toEigen(*contact.currentPosition) = toEigen(*contact.nominalPosition).leftCols<1>(); + + // if warmstart is enabled the contact location is used as warmstart to initialize the + // problem + if (m_pimpl->optiSettings.isWarmStartEnabled) + { + toEigen(*(m_pimpl->initialGuess.contactsInitialGuess[key].contactLocation)) + = toEigen(*contact.nominalPosition); + } + } + + // TODO this part can be improved. For instance you do not need to fill the vectors every time. + for (auto& [key, contact] : inputs.contacts) + { + using namespace BipedalLocomotion::Conversions; + + const auto& boundingBox = m_pimpl->contactBoundingBoxes.at(key); + toEigen(*contact.upperLimitPosition).colwise() = boundingBox.upperLimit; + toEigen(*contact.lowerLimitPosition).colwise() = boundingBox.lowerLimit; + } + + // we store the contact phase list for the output + m_pimpl->output.contactPhaseList = contactPhaseList; + + return true; +} diff --git a/src/ReducedModelControllers/tests/CMakeLists.txt b/src/ReducedModelControllers/tests/CMakeLists.txt index 036b9796ce..8b1fd2a68a 100644 --- a/src/ReducedModelControllers/tests/CMakeLists.txt +++ b/src/ReducedModelControllers/tests/CMakeLists.txt @@ -3,11 +3,16 @@ # 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 StableCentroidalMPC + SOURCES StableCentroidalMPCTest.cpp + LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math + BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners) endif() diff --git a/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp index 9b8abb50ec..a8f078c4b8 100644 --- a/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp +++ b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp @@ -386,4 +386,4 @@ TEST_CASE("CentroidalMPC") REQUIRE(com(0) > 0.25); REQUIRE(std::abs(com(1) - com0(1)) < 0.1); REQUIRE(std::abs(com(2) - com0(2)) < 0.005); -} +} \ No newline at end of file diff --git a/src/ReducedModelControllers/tests/StableCentroidalMPCTest.cpp b/src/ReducedModelControllers/tests/StableCentroidalMPCTest.cpp new file mode 100644 index 0000000000..dc4d1af6f1 --- /dev/null +++ b/src/ReducedModelControllers/tests/StableCentroidalMPCTest.cpp @@ -0,0 +1,383 @@ + +#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 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 StableCentroidalMPC& mpc, + const Eigen::Vector3d& com, + const Eigen::Vector3d& angularMomentum, + const Eigen::Vector3d& comDes, + const std::chrono::nanoseconds& elapsedTime, + const std::chrono::nanoseconds& currentTime, + std::ostream& centroidalMPCData) +{ + + const auto& contactLists = mpc.getOutput().contactPhaseList.lists(); + + for (const auto& [key, contact] : mpc.getOutput().contacts) + { + centroidalMPCData << contact.pose.translation().transpose() << " "; + for (const auto& corner : contact.corners) + { + centroidalMPCData << corner.force.transpose() << " "; + } + + auto contactList = contactLists.find(key); + REQUIRE(contactList != contactLists.end()); + + auto contactIt = contactList->second.getNextContact(currentTime); + if (contactIt == contactList->second.end()) + { + centroidalMPCData << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } else + { + centroidalMPCData << contactIt->pose.translation().transpose() << " "; + } + } + centroidalMPCData << com.transpose() << " " << comDes.transpose() << " " + << angularMomentum.transpose() << " " << elapsedTime.count() << std::endl; +} + +TEST_CASE("StableCentroidalMPC") +{ + + constexpr bool saveDataset = true; + + using namespace std::chrono_literals; + constexpr std::chrono::nanoseconds dT = 200ms; + + std::shared_ptr handler = std::make_shared(); + handler->setParameter("sampling_time", dT); + handler->setParameter("time_horizon", 1s + 200ms); + handler->setParameter("number_of_maximum_contacts", 2); + handler->setParameter("number_of_slices", 1); + handler->setParameter("static_friction_coefficient", 0.33); + handler->setParameter("solver_verbosity", 1); + handler->setParameter("solver_name", "ipopt"); + handler->setParameter("linear_solver", "mumps"); + 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", 2e2); + 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); + + StableCentroidalMPC mpc; + + REQUIRE(mpc.initialize(handler)); + + BipedalLocomotion::Contacts::ContactPhaseList phaseList; + BipedalLocomotion::Contacts::ContactListMap contactListMap; + + BipedalLocomotion::Math::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("StableCentroidalMPCUnitTest.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 = 10; + 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) + { + phaseList = mpc.getOutput().contactPhaseList; + + // 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, + currentTime, + centroidalMPCData); + } + + system->setControlInput({mpc.getOutput().contacts, // + BipedalLocomotion::Math::Wrenchd::Zero()}); + REQUIRE(integrator.integrate(0s, integratorStepTime)); + + controllerIndex++; + if (controllerIndex == dT / integratorStepTime) + { + controllerIndex = 0; + } + + currentTime += integratorStepTime; + } + + 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.05); + REQUIRE(std::abs(com(1) - com0(1)) < 0.1); + REQUIRE(std::abs(com(2) - com0(2)) < 0.005); +}