diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4848c096ae..200196dd2c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -19,3 +19,4 @@ add_subdirectory(Math) add_subdirectory(TSID) add_subdirectory(Perception) add_subdirectory(IK) +add_subdirectory(ReducedModelControllers) diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h b/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h index 2c50bc473d..9fef70e6ae 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h @@ -122,7 +122,21 @@ struct ContactWrench : public ContactBase using EstimatedLandmark = EstimatedContact; -} // namespace Contacts -} // namespace BipedalLocomotion +/** + * @brief Definition of a corner + */ +struct Corner +{ + Eigen::Vector3d position; + Eigen::Vector3d force; +}; + +struct ContactWithCorners : ContactBase +{ + std::vector corners; +}; + +} +} #endif // BIPEDAL_LOCOMOTION_CONTACTS_CONTACT_H diff --git a/src/ContinuousDynamicalSystem/CMakeLists.txt b/src/ContinuousDynamicalSystem/CMakeLists.txt index 79fcc66085..f5acb0dff8 100644 --- a/src/ContinuousDynamicalSystem/CMakeLists.txt +++ b/src/ContinuousDynamicalSystem/CMakeLists.txt @@ -13,12 +13,12 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) PUBLIC_HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/FloatingBaseDynamicsWithCompliantContacts.h ${H_PREFIX}/FixedBaseDynamics.h ${H_PREFIX}/Integrator.h ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h - ${H_PREFIX}/CompliantContactWrench.h + ${H_PREFIX}/CompliantContactWrench.h ${H_PREFIX}/CentroidalDynamics.h PRIVATE_HEADERS ${H_PREFIX}/impl/traits.h - SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp + SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp src/CentroidalDynamics.cpp PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels iDynTree::idyntree-high-level iDynTree::idyntree-model - Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math + Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math BipedalLocomotion::Contacts PRIVATE_LINK_LIBRARIES BipedalLocomotion::CommonConversions SUBDIRECTORIES tests ) diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h new file mode 100644 index 0000000000..048d7b540b --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h @@ -0,0 +1,108 @@ +/** + * @file CentroidalDynamics.h + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_CENROIDAL_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_CENROIDAL_DYNAMICS_H + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +class CentroidalDynamics; +} +} // namespace BipedalLocomotion + + +// Please read it as +// BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( +// FixedBaseDynamics, +// (joint velocities, joint positions), +// (joint accelerations, joints velocities), +// (joint torques) +BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( + CentroidalDynamics, + (Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d), + (Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d), + (std::map)) + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +class CentroidalDynamics : public DynamicalSystem + +{ + Eigen::Vector3d m_gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector + */ + double m_mass{1}; + + State m_state; + Input m_controlInput; + +public: + + /** + * Initialize the FixedBaseDynamics system. + * @param handler pointer to the parameter handler. + * @note The following parameters are used + * | Parameter Name | Type | Description | Mandatory | + * |:--------------:|:--------:|:--------------------------------------------------------------------------------------------:|:---------:| + * | `gravity` | `double` | Value of the Gravity. If not defined Math::StandardAccelerationOfGravitation is used | No | + * | `mass` | `double` | Value of the mass. If not defined 1 is used. | No | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + + /** + * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivative& stateDerivative); + + + /** + * Set the state of the dynamical system. + * @param state tuple containing a const reference to the state elements. + * @return true in case of success, false otherwise. + */ + bool setState(const State& state); + + /** + * Get the state to the dynamical system. + * @return the current state of the dynamical system + */ + const State& getState() const; + + /** + * Set the control input to the dynamical system. + * @param controlInput the value of the control input used to compute the system dynamics. + * @return true in case of success, false otherwise. + */ + bool setControlInput(const Input& controlInput); +}; +} +} + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_CENROIDAL_DYNAMICS_H diff --git a/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp b/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp new file mode 100644 index 0000000000..777e920ad4 --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp @@ -0,0 +1,87 @@ +/** + * @file CentroidalDynamics.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include +#include + +using namespace BipedalLocomotion; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; +using namespace BipedalLocomotion::ParametersHandler; + +bool CentroidalDynamics::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[CentroidalDynamics::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is expired. Please call the function passing a " + "pointer pointing an already allocated memory.", + logPrefix); + return false; + } + + if (!ptr->getParameter("gravity", m_gravity)) + { + log()->info("{} The gravity vector not found. The default one will be " + "used {}.", + logPrefix, + m_gravity.transpose()); + } + + if (!ptr->getParameter("mass", m_mass)) + { + log()->info("{} The mass is not found. The default one will be " + "used {}.", + logPrefix, + m_mass); + } + + return true; +} + +bool CentroidalDynamics::dynamics(const double& time, StateDerivative& stateDerivative) +{ + const auto& [comPosition, comVelocity, angularMomentum] = m_state; + auto& [comVelocityOut, comAcceleration, angularMomentumRate] = stateDerivative; + + comVelocityOut = comVelocity; + comAcceleration = m_gravity; + angularMomentumRate.setZero(); + + const auto& contacts = std::get<0>(m_controlInput); + + for (const auto& [key, contact] : contacts) + { + for (const auto& corner : contact.corners) + { + comAcceleration.noalias() += 1 / m_mass * contact.pose.asSO3().act(corner.force); + angularMomentumRate.noalias() += (contact.pose.act(corner.position) - comPosition) + .cross(contact.pose.asSO3().act(corner.force)); + } + } + + return true; +} + +bool CentroidalDynamics::setState(const State& state) +{ + m_state = state; + return true; +} + +const CentroidalDynamics::State& CentroidalDynamics::getState() const +{ + return m_state; +} + +bool CentroidalDynamics::setControlInput(const Input& controlInput) +{ + m_controlInput = controlInput; + return true; +} diff --git a/src/IK/include/BipedalLocomotion/IK/SE3Task.h b/src/IK/include/BipedalLocomotion/IK/SE3Task.h index dde6523112..14e32affa7 100644 --- a/src/IK/include/BipedalLocomotion/IK/SE3Task.h +++ b/src/IK/include/BipedalLocomotion/IK/SE3Task.h @@ -65,6 +65,9 @@ class SE3Task : public System::LinearTask std::shared_ptr m_kinDyn; /**< Pointer to a KinDynComputations object */ + double m_kpLinear; + double m_kpAngular; + public: /** @@ -118,6 +121,10 @@ class SE3Task : public System::LinearTask */ bool setSetPoint(const manif::SE3d& I_H_F, const manif::SE3d::Tangent& mixedVelocity); + void enableControl(); + + void disableControl(); + /** * Get the size of the task. (I.e the number of rows of the vector b) * @return the size of the task. diff --git a/src/IK/src/SE3Task.cpp b/src/IK/src/SE3Task.cpp index c7de848b06..a072d42d49 100644 --- a/src/IK/src/SE3Task.cpp +++ b/src/IK/src/SE3Task.cpp @@ -123,9 +123,8 @@ bool SE3Task::initialize(std::weak_ptr pa } // set the gains for the controllers - double kpLinear; - double kpAngular; - if (!ptr->getParameter("kp_linear", kpLinear)) + + if (!ptr->getParameter("kp_linear", m_kpLinear)) { log()->error("{}, [{} {}] Unable to get the proportional linear gain.", errorPrefix, @@ -134,7 +133,7 @@ bool SE3Task::initialize(std::weak_ptr pa return false; } - if (!ptr->getParameter("kp_angular", kpAngular)) + if (!ptr->getParameter("kp_angular", m_kpAngular)) { log()->error("{}, [{} {}] Unable to get the proportional angular gain.", errorPrefix, @@ -143,8 +142,8 @@ bool SE3Task::initialize(std::weak_ptr pa return false; } - m_R3Controller.setGains(kpLinear); - m_SO3Controller.setGains(kpAngular); + m_R3Controller.setGains(m_kpLinear); + m_SO3Controller.setGains(m_kpAngular); // set the description m_description = std::string(descriptionPrefix) + frameName + "."; @@ -211,3 +210,15 @@ bool SE3Task::isValid() const { return m_isValid; } + +void SE3Task::enableControl() +{ + m_R3Controller.setGains(m_kpLinear); + m_SO3Controller.setGains(m_kpAngular); +} + +void SE3Task::disableControl() +{ + m_R3Controller.setGains(0); + m_SO3Controller.setGains(0); +} diff --git a/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h b/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h index ce13425bc4..e25c8eff04 100644 --- a/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h +++ b/src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h @@ -49,7 +49,7 @@ class LinearizedFrictionCone * | `static_friction_coefficient` | `double` | Static friction coefficient. | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler); + bool initialize(std::weak_ptr handler); /** * Get the matrix A. diff --git a/src/Math/src/LinearizedFrictionCone.cpp b/src/Math/src/LinearizedFrictionCone.cpp index 58bb4622c5..d043a5fe55 100644 --- a/src/Math/src/LinearizedFrictionCone.cpp +++ b/src/Math/src/LinearizedFrictionCone.cpp @@ -12,7 +12,7 @@ using namespace BipedalLocomotion::Math; -bool LinearizedFrictionCone::initialize(std::weak_ptr handler) +bool LinearizedFrictionCone::initialize(std::weak_ptr handler) { constexpr auto errorPrefix = "[LinearizedFrictionCone::initialize]"; diff --git a/src/ReducedModelControllers/CMakeLists.txt b/src/ReducedModelControllers/CMakeLists.txt new file mode 100644 index 0000000000..fe0b9983a8 --- /dev/null +++ b/src/ReducedModelControllers/CMakeLists.txt @@ -0,0 +1,18 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + + +# if (FRAMEWORK_COMPILE_ReducedModelControllers) + + set(H_PREFIX include/BipedalLocomotion/ReducedModelControllers) + + add_bipedal_locomotion_library( + NAME ReducedModelControllers + PUBLIC_HEADERS ${H_PREFIX}/CentroidalMPC.h + SOURCES src/CentroidalMPC.cpp + PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts + PRIVATE_LINK_LIBRARIES casadi BipedalLocomotion::Math BipedalLocomotion::TextLogging + SUBDIRECTORIES tests) + +# endif() diff --git a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h new file mode 100644 index 0000000000..f247869489 --- /dev/null +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h @@ -0,0 +1,102 @@ +/** + * @file CentroidalMPC.h + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_CENTROIDAL_MPC_H +#define BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_CENTROIDAL_MPC_H + +#include +#include +#include + +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace ReducedModelControllers +{ + +struct CentroidalMPCState +{ + std::map contacts; +}; + +/** + * DCMPlanner defines a trajectory generator for the variable height Divergent component of motion + * (DCM). + */ +class CentroidalMPC : public System::Source +{ + /** + * Private implementation + */ + struct Impl; + + std::unique_ptr m_pimpl; /**< Pointer to private implementation */ + +public: + /** + * Constructor. + */ + CentroidalMPC(); + + /** + * Destructor. + */ + ~CentroidalMPC(); + + /** + * Initialize the planner. + * @param handler pointer to the parameter handler. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:---------------------------------:|:----------:|:----------------------------------------------------------------------------------------------------------------------------------------------:|:---------:| + * | `linear_solver` | `string` | Linear solver used by ipopt, the default value is mumps. Please check https://coin-or.github.io/Ipopt/#PREREQUISITES for the available solvers | No | + * | `planner_sampling_time` | `double` | Sampling time of the planner | Yes | + * | `number_of_foot_corners` | `int` | Number of the corner of the polygon used to describe the foot. E.g. 4 | Yes | + * | `foot_corner_` | `Vector3d` | A 3d vector describing the position of the corner w.r.t. frame associated to the foot. `i = 0:number_of_foot_corners`. | Yes | + * | `omega_dot_weight` | `double` | Weight associated to the \f$\dot{omega}\f$ | Yes | + * | `dcm_tracking_weight` | `double` | Weight associated to the DCM tracking | Yes | + * | `omega_dot_rate_of_change_weight` | `double` | Weight associated to the rate of change of \f$\dot{omega}\f$ | Yes | + * | `vrp_rate_of_change_weight` | `double` | Weight associated to the rate of change of the VRP | Yes | + * | `dcm_rate_of_change_weight` | `double` | Weight associated to the rate of change of the DCM | Yes | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler) final; + + bool setContactPhaseList(const Contacts::ContactPhaseList &contactPhaseList); + + bool setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum); + + bool setReferenceTrajectory(Eigen::Ref com); + + /** + * @brief Get the object. + * @return a const reference of the requested object. + */ + const CentroidalMPCState& getOutput() const final; + + /** + * @brief Determines the validity of the object retrieved with get() + * @return True if the object is valid, false otherwise. + */ + bool isOutputValid() const final; + + /** + * @brief Advance the internal state. This may change the value retrievable from get(). + * @return True if the advance is successfull. + */ + bool advance() final; +}; +} // namespace ReducedModelControllers +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_CENTROIDAL_MPC_H diff --git a/src/ReducedModelControllers/src/CentroidalMPC.cpp b/src/ReducedModelControllers/src/CentroidalMPC.cpp new file mode 100644 index 0000000000..0d860d8a27 --- /dev/null +++ b/src/ReducedModelControllers/src/CentroidalMPC.cpp @@ -0,0 +1,1032 @@ +/** + * @file CentroidalMPC.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +#include +#include +#include +#include + +using namespace BipedalLocomotion::ReducedModelControllers; +using namespace BipedalLocomotion::Contacts; + +Eigen::Map toEigen(casadi::DM& input) +{ + return Eigen::Map(input.ptr(), input.rows(), input.columns()); +} + +Eigen::Map toEigen(const casadi::DM& input) +{ + return Eigen::Map(input.ptr(), input.rows(), input.columns()); +} + +struct CentroidalMPC::Impl +{ + casadi::Opti opti; /**< CasADi opti stack */ + casadi::Function controller; + bool isInitialized{false}; + double currentTime{0}; + + CentroidalMPCState state; + + Math::LinearizedFrictionCone frictionCone; + + struct CasadiCorner + { + casadi::DM position; + casadi::MX force; + + CasadiCorner() = default; + CasadiCorner(const Corner& other) + { + 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("force", other.force.size(), 1); + + return *this; + } + }; + + struct CasadiContact + { + casadi::MX position; + casadi::MX orientation; + std::vector corners; + + CasadiContact() = default; + + CasadiContact& operator=(const ContactWithCorners& other) + { + corners.resize(other.corners.size()); + + for (int i = 0; i < other.corners.size(); i++) + { + this->corners[i] = other.corners[i]; + } + + this->orientation = casadi::MX::sym("orientation", 3, 3); + this->position = casadi::MX::sym("position", 3, 1); + + return *this; + } + + CasadiContact(const ContactWithCorners& other) + { + this->operator=(other); + } + }; + + + struct CasadiContactWithConstraints : CasadiContact + { + casadi::MX nominalPosition; + casadi::MX upperLimitPosition; + casadi::MX lowerLimitPosition; + casadi::MX limitVelocity; + casadi::MX maximumNormalForce; + }; + + struct OptimizationSettings + { + unsigned long solverVerbosity{1}; /**< 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) */ + + double samplingTime; /**< Sampling time of the planner in seconds */ + int horizon; /** solution; /**< Pointer to the solution of the optimization + problem */ + casadi::DM dcm; /**< Optimal DCM trajectory */ + casadi::DM omega; /**< Optimal omega trajectory */ + casadi::DM vrp; /**< Optimal VRP trajectory */ + casadi::DM omegaDot; /**< Optimal omega rate of change trajectory */ + }; + OptimizationSolution optiSolution; /**< Solution of the optimization problem */ + + + /** + * OptimizationVariables contains the optimization variables expressed as CasADi elements. + */ + struct OptimizationVariables + { + casadi::MX com; + casadi::MX dcom; + casadi::MX angularMomentum; + std::map contacts; + + casadi::MX comReference; + casadi::MX comCurrent; + casadi::MX dcomCurrent; + casadi::MX angularMomentumCurrent; + }; + OptimizationVariables optiVariables; /**< Optimization variables */ + + + struct ContactsInputs + { + casadi::DM orientation; + casadi::DM nominalPosition; + casadi::DM upperLimitPosition; + casadi::DM lowerLimitPosition; + casadi::DM limitVelocity; + casadi::DM maximumNormalForce; + }; + struct ControllerInputs + { + std::map contacts; + + casadi::DM comReference; + casadi::DM comCurrent; + casadi::DM dcomCurrent; + casadi::DM angularMomentumCurrent; + }; + ControllerInputs controllerInputs; + + struct Weights + { + Eigen::Vector3d com; + double contactPosition; + Eigen::Vector3d forceRateOfChange; + double angularMomentum; + }; + Weights weights; /**< Settings */ + + bool loadContactCorners(std::shared_ptr ptr, + ContactWithCorners& 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 errorPrefix = "[CentroidalMPC::Impl::loadParameters]"; + + if (!ptr->getParameter("controller_sampling_time", this->optiSettings.samplingTime)) + { + log()->error("{} Unable to load the sampling time of the controller.", errorPrefix); + return false; + } + + if (!ptr->getParameter("controller_horizon", this->optiSettings.horizon)) + { + log()->error("{} Unable to load the controller horizon the controller.", errorPrefix); + return false; + } + + this->optiSettings.timeHorizon = this->optiSettings.horizon * this->optiSettings.samplingTime; + + int numberOfMaximumContacts; + if (!ptr->getParameter("number_of_maximum_contacts", numberOfMaximumContacts)) + { + log()->error("{} Unable to load the maximum number of contacts.", errorPrefix); + return false; + } + + 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.", + errorPrefix, + i, + i); + return false; + } + + std::string contactName; + if (!contactHandler->getParameter("contact_name", contactName)) + { + log()->error("{} Unable to get the name of the contact number {}.", errorPrefix, i); + return false; + } + + if (!this->loadContactCorners(contactHandler, this->state.contacts[contactName])) + { + log()->error("{} Unable to load the contact corners for the contact {}.", + errorPrefix, + i); + return false; + } + } + + if (!ptr->getParameter("linear_solver", this->optiSettings.ipoptLinearSolver)) + { + log()->info("{} The default linear solver will be used: {}.", + errorPrefix, + this->optiSettings.ipoptLinearSolver); + } + + if (!ptr->getParameter("ipopt_tolerance", this->optiSettings.ipoptTolerance)) + { + log()->info("{} The default ipopt tolerance will be used: {}.", + errorPrefix, + this->optiSettings.ipoptTolerance); + } + + bool ok = true; + ok = ok && ptr->getParameter("com_weight", this->weights.com); + ok = ok && ptr->getParameter("contact_position_weight", this->weights.contactPosition); + ok = ok && ptr->getParameter("force_rate_of_change_weight", this->weights.forceRateOfChange); + ok = ok && ptr->getParameter("angular_momentum_weight", this->weights.angularMomentum); + + // initialize the friction cone + ok = ok && frictionCone.initialize(ptr); + + if (!ok) + { + log()->error("{} Unable to load the weight of the cost function.", errorPrefix); + return false; + } + + return true; + } + + casadi::Function ode() + { + // Convert BipedalLocomotion::ReducedModelControllers::ContactWithCorners into a + // casadiContact object + std::map casadiContacts(this->state.contacts.begin(), + this->state.contacts.end()); + + // we assume mass equal to 1 + constexpr double mass = 1; + + casadi::MX com = casadi::MX::sym("com", 3); + casadi::MX dcom = casadi::MX::sym("dcom", 3); + casadi::MX ddcom = casadi::MX::sym("ddcom", 3); + casadi::MX angularMomentum = casadi::MX::sym("angular_momentum", 3); + casadi::MX angularMomentumDerivative = casadi::MX::sym("angular_momentum_derivative", 3); + + casadi::DM gravity = casadi::DM::zeros(3); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + ddcom = gravity; + angularMomentumDerivative = casadi::DM::zeros(3); + + std::vector input; + 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(casadi::MX::reshape(contact.orientation, 9, 1)); + for(const auto& corner : contact.corners) + { + + ddcom += 1/mass * casadi::MX::mtimes(contact.orientation, corner.force); + + angularMomentumDerivative + += casadi::MX::cross(casadi::MX::mtimes(contact.orientation, corner.position) + + contact.position - com, + casadi::MX::mtimes(contact.orientation, corner.force)); + + input.push_back(corner.force); + } + } + + casadi::MX rhs = casadi::MX::vertcat( + {com + dcom * this->optiSettings.samplingTime, + dcom + ddcom * this->optiSettings.samplingTime, + angularMomentum + angularMomentumDerivative * this->optiSettings.samplingTime}); + + return casadi::Function("centroidal_dynamics", + input, + {com + dcom * this->optiSettings.samplingTime, + dcom + ddcom * this->optiSettings.samplingTime, + angularMomentum + + angularMomentumDerivative + * this->optiSettings.samplingTime}); + } + + + 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), + contactPosition - nominalContactPosition); + + return casadi::Function("contact_position_error", + {contactPosition, nominalContactPosition, contactOrientation}, + {rhs}); + } + + void resizeControllerInputs() + { + constexpr int vector3Size = 3; + + // prepare the controller inputs struct + this->controllerInputs.comCurrent = casadi::DM::zeros(vector3Size); + this->controllerInputs.dcomCurrent = casadi::DM::zeros(vector3Size); + this->controllerInputs.angularMomentumCurrent = casadi::DM::zeros(vector3Size); + this->controllerInputs.comReference = casadi::DM::zeros(vector3Size, this->optiSettings.horizon); + + for (const auto& [key, contact] : this->state.contacts) + { + // The orientation is stored as a vectorized version of the rotation matrix + this->controllerInputs.contacts[key].orientation + = casadi::DM::zeros(9, this->optiSettings.horizon); + + // This is the admissible velocity of the contact (It will be different to zero only + // when a new contact is created) + this->controllerInputs.contacts[key].limitVelocity + = casadi::DM::zeros(vector3Size, this->optiSettings.horizon - 1); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + this->controllerInputs.contacts[key].upperLimitPosition + = casadi::DM::zeros(vector3Size, this->optiSettings.horizon); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + this->controllerInputs.contacts[key].lowerLimitPosition + = casadi::DM::zeros(vector3Size, this->optiSettings.horizon); + + // Maximum admissible contact force. It is expressed in the contact body frame + this->controllerInputs.contacts[key].maximumNormalForce + = casadi::DM::zeros(1, this->optiSettings.horizon); + + // The nominal contact position is a parameter that regularize the solution + this->controllerInputs.contacts[key].nominalPosition + = casadi::DM::zeros(vector3Size, this->optiSettings.horizon); + } + } + + void populateOptiVariables() + { + constexpr int vector3Size = 3; + + // create the variables for the state + this->optiVariables.com = this->opti.variable(vector3Size, this->optiSettings.horizon + 1); + this->optiVariables.dcom = this->opti.variable(vector3Size, this->optiSettings.horizon + 1); + this->optiVariables.angularMomentum + = this->opti.variable(vector3Size, this->optiSettings.horizon + 1); + + // the casadi contacts depends on the maximum number of contacts + for (const auto& [key, contact] : this->state.contacts) + { + // each contact has a different number of corners + this->optiVariables.contacts[key].corners.resize(contact.corners.size()); + + // the position of the contact is an optimization variable + this->optiVariables.contacts[key].position + = this->opti.variable(vector3Size, this->optiSettings.horizon); + + // the orientation is a parameter. The orientation is stored as a vectorized version of + // the rotation matrix + this->optiVariables.contacts[key].orientation + = this->opti.parameter(9, this->optiSettings.horizon); + + // This is the admissible velocity of the contact (It will be different to zero only + // when a new contact is created) + this->optiVariables.contacts[key].limitVelocity + = this->opti.parameter(vector3Size, this->optiSettings.horizon - 1); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + this->optiVariables.contacts[key].upperLimitPosition + = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + this->optiVariables.contacts[key].lowerLimitPosition + = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + // Maximum admissible contact force. It is expressed in the contact body frame + this->optiVariables.contacts[key].maximumNormalForce + = this->opti.parameter(1, this->optiSettings.horizon); + + // The nominal contact position is a parameter that regularize the solution + this->optiVariables.contacts[key].nominalPosition + = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + for (int j = 0; j < contact.corners.size(); j++) + { + this->optiVariables.contacts[key].corners[j].force + = this->opti.variable(vector3Size, this->optiSettings.horizon); + + this->optiVariables.contacts[key].corners[j].position = casadi::DM( + std::vector(this->state.contacts[key].corners[j].position.data(), + this->state.contacts[key].corners[j].position.data() + + this->state.contacts[key].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, this->optiSettings.horizon); + } + + /** + * Setup the optimization problem options + */ + void setupOptiOptions() + { + // casadi::Dict casadiOptions; + // casadi::Dict ipoptOptions; + + // if (this->optiSettings.solverVerbosity != 0) + // { + // casadi_int ipoptVerbosity = static_cast(optiSettings.solverVerbosity - 1); + // // ipoptOptions["print_level"] = ipoptVerbosity; + // casadiOptions["print_time"] = false; + // } else + // { + // // ipoptOptions["print_level"] = 0; + // casadiOptions["print_time"] = false; + // } + // casadiOptions["qpsol"] = "osqp"; + // // casadiOptions["print_iteration"] = false; + // // casadiOptions["print_header"] = false; + // casadiOptions["convexify_strategy"] = "regularize"; + + // // ipoptOptions["tol"] = this->optiSettings.ipoptTolerance; + // casadiOptions["expand"] = true; + // // casadiOptions["warm"] = true; + // ipoptOptions["verbose"] = false; // + + // casadiOptions["qpsol_options"] = ipoptOptions; + + // this->opti.solver("sqpmethod", casadiOptions); + + casadi::Dict casadiOptions; + casadi::Dict ipoptOptions; + + if (this->optiSettings.solverVerbosity != 0) + { + casadi_int ipoptVerbosity = static_cast(optiSettings.solverVerbosity - 1); + ipoptOptions["print_level"] = ipoptVerbosity; + casadiOptions["print_time"] = true; + } else + { + ipoptOptions["print_level"] = 0; + casadiOptions["print_time"] = true; + } + // casadiOptions["qpsol"] = "qrqp"; + // casadiOptions["print_iteration"] = false; + // casadiOptions["print_header"] = false; + // casadiOptions["convexify_strategy"] = "regularize"; + + ipoptOptions["linear_solver"] = this->optiSettings.ipoptLinearSolver; + casadiOptions["expand"] = true; + // casadiOptions["warm"] = true; + // ipoptOptions["verbose"] = false; // + + // casadiOptions["qpsol_options"] = ipoptOptions; + + this->opti.solver("ipopt", casadiOptions, ipoptOptions); + } + + casadi::Function createController() + { + using Sl = casadi::Slice; + + this->populateOptiVariables(); + + std::vector controlInput; + for(const auto& [key, contact] : this->optiVariables.contacts) + { + controlInput.push_back(contact.position); + controlInput.push_back(contact.orientation); + + for(const auto& corner : contact.corners) + { + controlInput.push_back(corner.force); + } + } + + // + auto& com = this->optiVariables.com; + auto& dcom = this->optiVariables.dcom; + auto& angularMomentum = this->optiVariables.angularMomentum; + + // set the initial values + 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)); + + // system dynamics + std::vector tmp; + tmp.push_back(com(Sl(), Sl(0, -1))); + tmp.push_back(dcom(Sl(), Sl(0, -1))); + tmp.push_back(angularMomentum(Sl(), Sl(0, -1))); + tmp.insert(tmp.end(), controlInput.begin(), controlInput.end()); + + auto dynamics = this->ode().map(this->optiSettings.horizon); + + auto fullTrajectory = dynamics(tmp); + this->opti.subject_to(com(Sl(), Sl(1, com.columns())) == fullTrajectory[0]); + this->opti.subject_to(dcom(Sl(), Sl(1, com.columns())) == fullTrajectory[1]); + this->opti.subject_to(angularMomentum(Sl(), Sl(1, angularMomentum.columns())) == fullTrajectory[2]); + + // 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 as 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 constraintFrictionCone; + + for(const auto& [key, contact] : this->optiVariables.contacts) + { + + auto error = contactPositionErrorMap({contact.position, + contact.nominalPosition, + contact.orientation}); + + this->opti.subject_to(contact.lowerLimitPosition <= error[0] <= contact.upperLimitPosition); + + this->opti.subject_to(-contact.limitVelocity + <= casadi::MX::diff(contact.position.T()).T() + <= contact.limitVelocity); + + // 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) + { + constraintFrictionCone = casadi::MX::mtimes(frictionConeMatrix, corner.force); + for (int i = 0; i < corner.force.columns(); i++) + { + this->opti.subject_to(constraintFrictionCone(Sl(), i) <= zero); + } + + // limit on the normal force + this->opti.subject_to(casadi::DM::zeros(1, corner.force.columns()) + <= corner.force(2, Sl()) <= contact.maximumNormalForce); + } + } + + // create the cost function + casadi::MX cost = this->weights.angularMomentum + * casadi::MX::sumsqr(this->optiVariables.angularMomentum) + + this->weights.com(0) + * casadi::MX::sumsqr(com(0, Sl(1, com.columns())) + - this->optiVariables.comReference(0, Sl())) + + this->weights.com(1) + * casadi::MX::sumsqr(com(1, Sl(1, com.columns())) + - this->optiVariables.comReference(1, Sl())) + + this->weights.com(2) + * casadi::MX::sumsqr(com(2, Sl(1, com.columns())) + - this->optiVariables.comReference(2, Sl())); + + casadi::MX averageForce; + for (const auto& [key, contact] : this->optiVariables.contacts) + { + cost += this->weights.contactPosition + * casadi::MX::sumsqr(contact.nominalPosition - contact.position); + + averageForce = contact.corners[0].force / contact.corners.size(); + for (int i = 1; i < contact.corners.size(); i++) + { + averageForce += contact.corners[i].force / contact.corners.size(); + } + + for (const auto& corner : contact.corners) + { + auto forceRateOfChange = casadi::MX::diff(corner.force.T()).T(); + + cost += 10 * 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; + + input.push_back(this->optiVariables.comCurrent); + input.push_back(this->optiVariables.dcomCurrent); + input.push_back(this->optiVariables.angularMomentumCurrent); + input.push_back(this->optiVariables.comReference); + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + input.push_back(contact.nominalPosition); + input.push_back(contact.orientation); + input.push_back(contact.maximumNormalForce); + input.push_back(contact.upperLimitPosition); + input.push_back(contact.lowerLimitPosition); + input.push_back(contact.limitVelocity); + + output.push_back(contact.position); + + for (const auto& corner : contact.corners) + { + output.push_back(corner.force); + } + } + return this->opti.to_function("controller", input, output); + } +}; + +bool CentroidalMPC::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->isInitialized = true; + + return true; +} + +CentroidalMPC::~CentroidalMPC() = default; + +CentroidalMPC::CentroidalMPC() +{ + m_pimpl = std::make_unique(); +} + +const CentroidalMPCState& CentroidalMPC::getOutput() const +{ + return m_pimpl->state; +} + +bool CentroidalMPC::isOutputValid() const +{ + return true; +} + +bool CentroidalMPC::advance() +{ + constexpr auto errorPrefix = "[CentroidalMPC::advance]"; + assert(m_pimpl); + + using Sl = casadi::Slice; + + if (!m_pimpl->isInitialized) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + // controller:(i0[3],i1[3],i2[3],i3[3x10],i4[3x10],i5[9x10],i6[1x10],i7[3x10],i8[3x10],i9[3x9],i10[3x10],i11[9x10],i12[1x10],i13[3x10],i14[3x10],i15[3x9])->(o0[3x10],o1[3x10],o2[3x10],o3[3x10],o4[3x10],o5[3x10],o6[3x10],o7[3x10],o8[3x10],o9[3x10]) + + const auto& inputs = m_pimpl->controllerInputs; + // m_pimpl->opti.set_value(m_pimpl->optiVariables.comCurrent, inputs.comCurrent); + // m_pimpl->opti.set_value(m_pimpl->optiVariables.dcomCurrent, inputs.dcomCurrent); + // m_pimpl->opti.set_value(m_pimpl->optiVariables.angularMomentumCurrent, + // inputs.angularMomentumCurrent); + + // m_pimpl->opti.set_value(m_pimpl->optiVariables.comReference, inputs.comReference); + + // for (const auto& [key, contact] : inputs.contacts) + // { + // m_pimpl->opti.set_value(m_pimpl->optiVariables.contacts[key].nominalPosition, + // contact.nominalPosition); + // m_pimpl->opti.set_value(m_pimpl->optiVariables.contacts[key].orientation, + // contact.orientation); + + // m_pimpl->opti.set_value(m_pimpl->optiVariables.contacts[key].maximumNormalForce, + // contact.maximumNormalForce); + // m_pimpl->opti.set_value(m_pimpl->optiVariables.contacts[key].upperLimitPosition, + // contact.upperLimitPosition); + + // m_pimpl->opti.set_value(m_pimpl->optiVariables.contacts[key].lowerLimitPosition, + // contact.lowerLimitPosition); + + // m_pimpl->opti.set_value(m_pimpl->optiVariables.contacts[key].limitVelocity, + // contact.limitVelocity); + // } + + // + // m_pimpl->opti.set_initial(m_pimpl->optiVariables.com(Sl(), + // Sl(1, + // m_pimpl->optiVariables.com.columns())), + // inputs.comReference); + + std::vector vectorizedInputs; + vectorizedInputs.push_back(inputs.comCurrent); + vectorizedInputs.push_back(inputs.dcomCurrent); + vectorizedInputs.push_back(inputs.angularMomentumCurrent); + vectorizedInputs.push_back(inputs.comReference); + + for (const auto & [key, contact] : inputs.contacts) + { + vectorizedInputs.push_back(contact.nominalPosition); + vectorizedInputs.push_back(contact.orientation); + vectorizedInputs.push_back(contact.maximumNormalForce); + vectorizedInputs.push_back(contact.upperLimitPosition); + vectorizedInputs.push_back(contact.lowerLimitPosition); + vectorizedInputs.push_back(contact.limitVelocity); + } + + // try + // { + // m_pimpl->optiSolution.solution = std::make_unique(m_pimpl->opti.solve()); + // } catch (const std::exception& e) + // { + // log()->error("{} Unable to solve the optimization problem. The following exception has " + // "been thrown by the solver: {}.", + // errorPrefix, + // e.what()); + + // return false; + // } + + // // get the solution + // for (auto& [key, contact] : m_pimpl->state.contacts) + // { + // contact.pose.translation(toEigen( + // m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.contacts[key].position))); + + // for (int i = 0; i < contact.corners.size(); i++) + // { + // contact.corners[i].force = toEigen(m_pimpl->optiSolution.solution->value( + // m_pimpl->optiVariables.contacts[key].corners[i].force)); + // } + // } + + // compute the output + auto controllerOutput = m_pimpl->controller(vectorizedInputs); + + // get the solution + auto it = controllerOutput.begin(); + for (auto & [key, contact] : m_pimpl->state.contacts) + { + // the first output is the position of the contact + contact.pose.translation(toEigen(*it).leftCols<1>()); + std::advance(it, 1); + for(auto& corner : contact.corners) + { + corner.force = toEigen(*it).leftCols<1>(); + std::advance(it, 1); + } + } + + // advance the time + m_pimpl->currentTime += m_pimpl->optiSettings.samplingTime; + return true; +} + +bool CentroidalMPC::setReferenceTrajectory(Eigen::Ref com) +{ + constexpr auto errorPrefix = "[CentroidalMPC::setReferenceTrajectory]"; + assert(m_pimpl); + + if (!m_pimpl->isInitialized) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + if (com.rows() != 3) + { + log()->error("{} The CoM matrix should have three rows.", errorPrefix); + return false; + } + + if (com.cols() < m_pimpl->optiSettings.horizon) + { + log()->error("{} The CoM matrix should have at least {} columns. The number of columns is " + "equal to the horizon you set in the initialization phase.", + errorPrefix, + m_pimpl->optiSettings.horizon); + return false; + } + + toEigen(m_pimpl->controllerInputs.comReference) = com.leftCols(m_pimpl->optiSettings.horizon); + + return true; +} + +bool CentroidalMPC::setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum) +{ + constexpr auto errorPrefix = "[CentroidalMPC::setState]"; + assert(m_pimpl); + + if (!m_pimpl->isInitialized) + { + log()->error("{} The controller is not initialized please call initialize() method.", + errorPrefix); + return false; + } + + auto & inputs = m_pimpl->controllerInputs; + toEigen(inputs.comCurrent) = com; + toEigen(inputs.dcomCurrent) = dcom; + toEigen(inputs.angularMomentumCurrent) = angularMomentum; + + return true; +} + +bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList &contactPhaseList) +{ + constexpr auto errorPrefix = "[CentroidalMPC::setContactPhaseList]"; + assert(m_pimpl); + + if (!m_pimpl->isInitialized) + { + 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; + } + + // clear previous data + for (const auto& [key, contact] : m_pimpl->state.contacts) + { + + // The orientation is stored as a vectorized version of the rotation matrix + toEigen(m_pimpl->controllerInputs.contacts[key].orientation).setZero(); + + // M_Pimpl is the admissible velocity of the contact (It will be different to zero only + // when a new contact is created) + toEigen(m_pimpl->controllerInputs.contacts[key].limitVelocity).setZero(); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + toEigen(m_pimpl->controllerInputs.contacts[key].upperLimitPosition).setZero(); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + toEigen(m_pimpl->controllerInputs.contacts[key].lowerLimitPosition).setZero(); + + // Maximum admissible contact force. It is expressed in the contact body frame + toEigen(m_pimpl->controllerInputs.contacts[key].maximumNormalForce).setZero(); + + // The nominal contact position is a parameter that regularize the solution + toEigen(m_pimpl->controllerInputs.contacts[key].nominalPosition).setZero(); + } + + const double absoluteTimeHorizon = m_pimpl->currentTime + m_pimpl->optiSettings.timeHorizon; + + // find the contactPhase associated to the current time + auto initialPhase + = std::find_if(contactPhaseList.begin(), contactPhaseList.end(), [=](const auto& phase) { + return phase.beginTime <= m_pimpl->currentTime + && phase.endTime > m_pimpl->currentTime; + }); + + if (initialPhase == contactPhaseList.end()) + { + log()->error("{} Unable to find the contact phase related to the current time.", + errorPrefix); + return false; + } + + // find the contactPhase associated to the end time + auto finalPhase + = std::find_if(contactPhaseList.begin(), contactPhaseList.end(), [=](const auto& phase) { + return phase.beginTime <= absoluteTimeHorizon && phase.endTime > absoluteTimeHorizon; + }); + + // if the list is not found the latest contact phase is considered + if (finalPhase == contactPhaseList.end()) + { + finalPhase = std::prev(contactPhaseList.end()); + std::cerr << "error" << std::endl; + return false; + } + + int index = 0; + for (auto it = initialPhase; it != std::next(finalPhase); std::advance(it, 1)) + { + // TODO check what it's going on if t horizon is greater then last endtime + const double tInitial = std::max(m_pimpl->currentTime, it->beginTime); + const double tFinal = std::min(absoluteTimeHorizon, it->endTime); + + const int numberOfSamples + = std::round((tFinal - tInitial) / m_pimpl->optiSettings.samplingTime); + + for (const auto& [key, contact] : it->activeContacts) + { + auto inputContact = m_pimpl->controllerInputs.contacts.find(key); + if (inputContact == m_pimpl->controllerInputs.contacts.end()) + { + log()->error("{} Unable to find the input contact named {}.", errorPrefix, key); + return false; + } + + toEigen(inputContact->second.nominalPosition) + .middleCols(index, numberOfSamples) + .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 maxForce = 1e5; + toEigen(inputContact->second.maximumNormalForce) + .middleCols(index, numberOfSamples) + .setConstant(maxForce); + } + + index += numberOfSamples; + } + + assert(index == m_pimpl->optiSettings.horizon); + + for (const auto& [key, contact] : m_pimpl->controllerInputs.contacts) + { + + log()->info("{} maximumnormalforce {}", + key, + toEigen(contact.maximumNormalForce)); + } + // TODO Implement this part only if you want to add the push recovery + for (auto& [key, contact] : m_pimpl->controllerInputs.contacts) + { + constexpr double maxVelocity = 1e5; + toEigen(contact.upperLimitPosition).setZero(); + toEigen(contact.lowerLimitPosition).setZero(); + toEigen(contact.limitVelocity).setConstant(maxVelocity); + } + + return true; +} diff --git a/src/ReducedModelControllers/tests/CMakeLists.txt b/src/ReducedModelControllers/tests/CMakeLists.txt new file mode 100644 index 0000000000..b6cce9b37d --- /dev/null +++ b/src/ReducedModelControllers/tests/CMakeLists.txt @@ -0,0 +1,8 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +add_bipedal_test( + NAME CentroidalMPC + SOURCES CentroidalMPCTest.cpp + LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners) diff --git a/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp new file mode 100644 index 0000000000..2cf64c9d2b --- /dev/null +++ b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp @@ -0,0 +1,215 @@ +/** + * @file CentroidalMPCTest.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// Catch2 +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::ReducedModelControllers; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +#include + +TEST_CASE("CentroidalMPC") +{ + constexpr double dT = 0.1; + + std::shared_ptr handler = std::make_shared(); + handler->setParameter("controller_sampling_time", dT); + handler->setParameter("controller_horizon", 20); + handler->setParameter("number_of_maximum_contacts", 2); + handler->setParameter("number_of_slices", 1); + handler->setParameter("static_friction_coefficient", 0.33); + + 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.06, -0.05, 0}); + contact0Handler->setParameter("corner_3", std::vector{-0.06, 0.05, 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.06, -0.05, 0}); + contact1Handler->setParameter("corner_3", std::vector{-0.06, 0.05, 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", 1.0); + handler->setParameter("force_rate_of_change_weight", std::vector{1, 1, 1}); + handler->setParameter("angular_momentum_weight", 1e5); + + CentroidalMPC mpc; + + REQUIRE(mpc.initialize(handler)); + + + BipedalLocomotion::Contacts::ContactPhaseList phaseList; + BipedalLocomotion::Contacts::ContactListMap contactListMap; + + + BipedalLocomotion::Planners::QuinticSpline comSpline; + std::vector knots; + std::vector time; + + + constexpr double scaling = 0.5; + // t 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 + // L |+++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++| + // R |+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++| + // left foot + // first footstep + Eigen::Vector3d leftPos; + leftPos << -0.0852893, 0.0828008, -0.000110952; + manif::SE3d leftTransform(leftPos, manif::SO3d::Identity()); + contactListMap["left_foot"].addContact(leftTransform, 0.0, 1.0 * scaling); + contactListMap["left_foot"].addContact(leftTransform, 2.0* scaling, 5.0* scaling); + contactListMap["left_foot"].addContact(leftTransform, 6.0* scaling, 9.0* scaling); + contactListMap["left_foot"].addContact(leftTransform, 10.0* scaling, 13.0* scaling); + contactListMap["left_foot"].addContact(leftTransform, 14.0* scaling, 17.0* scaling); + + // right foot + // first footstep + Eigen::Vector3d rightPos; + rightPos << -0.0850079, -0.073266, -0.000111252; + manif::SE3d rightTransform(rightPos, manif::SO3d::Identity()); + contactListMap["right_foot"].addContact(rightTransform, 0.0, 3.0* scaling); + contactListMap["right_foot"].addContact(rightTransform, 4.0* scaling, 7.0* scaling); + contactListMap["right_foot"].addContact(rightTransform, 8.0* scaling, 11.0* scaling); + contactListMap["right_foot"].addContact(rightTransform, 12.0* scaling, 15.0* scaling); + contactListMap["right_foot"].addContact(rightTransform, 16.0* scaling, 17.0* scaling); + + phaseList.setLists(contactListMap); + + Eigen::Vector3d comtemp; + comtemp << -0.0557448, 0.00497309, 0.528696; + + // knots.push_back(comtemp); + // time.push_back(0); + + // comtemp(1) = rightPos(1); + // knots.push_back(comtemp); + // time.push_back(1.5); + + // comtemp(1) = leftPos(1); + // knots.push_back(comtemp); + // time.push_back(3.5); + + // comtemp(1) = rightPos(1); + // knots.push_back(comtemp); + // time.push_back(5.5); + + // comtemp(1) = leftPos(1); + // knots.push_back(comtemp); + // time.push_back(7.5); + + // comtemp(1) = rightPos(1); + // knots.push_back(comtemp); + // time.push_back(9.5); + + // comtemp(1) = leftPos(1); + // knots.push_back(comtemp); + // time.push_back(11.5); + + + // comtemp(1) = rightPos(1); + // knots.push_back(comtemp); + // time.push_back(13.5); + + // comtemp(1) = leftPos(1); + // knots.push_back(comtemp); + // time.push_back(15.5); + + + // comtemp(1) = 0; + // knots.push_back(comtemp); + // time.push_back(17); + + // comSpline.setKnots(knots, time); + // comSpline.setInitialConditions(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + // comSpline.setFinalConditions(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + + Eigen::MatrixXd comTraj(3, 1000); + comTraj.colwise() = comtemp; + + // Eigen::Vector3d dummy; + // for(int i = 0; i < 170; i++) + // { + // comSpline.evaluatePoint(i * dT, comTraj.col(i), dummy, dummy); + // } + + // std::cerr << comTraj.leftCols(10) << std::endl; + + // comTraj.rightCols(1000 - 170).colwise() = comTraj.col(170); + + // std::cerr << comTraj.leftCols(10) << std::endl; + + // initialize the dynamical system + auto system = std::make_shared(); + system->setState({comTraj.col(0), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); + + ForwardEuler integrator; + integrator.setIntegrationStep(dT/10.0); + REQUIRE(integrator.setDynamicalSystem(system)); + + std::ofstream myFile; + myFile.open("data.txt"); + + // TODO reset contatcs + int controllerIndex = 0; + int index = 0; + for(int i = 0; i < 1000; i++) + { + const auto& [com, dcom, angularMomentum] = system->getState(); + + if(controllerIndex == 0) + { + REQUIRE(mpc.setState(com, dcom, angularMomentum)); + // REQUIRE(mpc.setReferenceTrajectory(comTraj.rightCols(comTraj.cols() - index))); + REQUIRE(mpc.setReferenceTrajectory(comTraj)); + REQUIRE(mpc.setContactPhaseList(phaseList)); + REQUIRE(mpc.advance()); + index++; + } + + for (const auto& [key, contact] : mpc.getOutput().contacts) + { + for(const auto& corner : contact.corners) + { + myFile << corner.force.transpose() << " "; + } + } + myFile << com.transpose() << " " << comTraj.col(index).transpose() << " " << angularMomentum.transpose(); + + myFile << std::endl; + + system->setControlInput({mpc.getOutput().contacts}); + + + REQUIRE(integrator.integrate(0, 0.01)); + + controllerIndex++; + if(controllerIndex == 20) + controllerIndex =0; + } + + myFile.close(); +}