From 7a7c9e0633b66c6af265bebae98c1bf5f841c528 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Mar 2021 14:05:28 +0200 Subject: [PATCH 1/9] Implement Corner and ContactWithCorners in the Contacts component --- .../BipedalLocomotion/Contacts/Contact.h | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) 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 From 65a6d4286aa51cdf17740087a6ae21e12c2f063e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Mar 2021 14:07:47 +0200 Subject: [PATCH 2/9] Implement CentroidalDynamics class in ContinuousDynamicalSystem --- src/ContinuousDynamicalSystem/CMakeLists.txt | 6 +- .../CentroidalDynamics.h | 108 ++++++++++++++++++ .../src/CentroidalDynamics.cpp | 90 +++++++++++++++ 3 files changed, 201 insertions(+), 3 deletions(-) create mode 100644 src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h create mode 100644 src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp 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..56567b50fc --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp @@ -0,0 +1,90 @@ +/** + * @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) + { + + log()->info("{} rotation {}.", key, contact.pose.asSO3().rotation()); + + 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; +} From 491c7fa81109803a93e55eb4dc3893ac1f84b6ad Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Mar 2021 14:08:37 +0200 Subject: [PATCH 3/9] Implement first version of the CentroidalMPC --- src/ReducedModelControllers/CMakeLists.txt | 18 + .../ReducedModelControllers/CentroidalMPC.h | 104 ++ .../src/CentroidalMPC.cpp | 907 ++++++++++++++++++ .../tests/CMakeLists.txt | 8 + .../tests/CentroidalMPCTest.cpp | 140 +++ 5 files changed, 1177 insertions(+) create mode 100644 src/ReducedModelControllers/CMakeLists.txt create mode 100644 src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h create mode 100644 src/ReducedModelControllers/src/CentroidalMPC.cpp create mode 100644 src/ReducedModelControllers/tests/CMakeLists.txt create mode 100644 src/ReducedModelControllers/tests/CentroidalMPCTest.cpp 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..c78f237e87 --- /dev/null +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h @@ -0,0 +1,104 @@ +/** + * @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::Advanceable +{ + /** + * 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); + + 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& get() const final; + + /** + * @brief Determines the validity of the object retrieved with get() + * @return True if the object is valid, false otherwise. + */ + bool isValid() 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..cfda172edf --- /dev/null +++ b/src/ReducedModelControllers/src/CentroidalMPC.cpp @@ -0,0 +1,907 @@ +/** + * @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; /** 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"] = true; + } else + { + ipoptOptions["print_level"] = 0; + casadiOptions["print_time"] = false; + } + ipoptOptions["linear_solver"] = this->optiSettings.ipoptLinearSolver; + ipoptOptions["tol"] = this->optiSettings.ipoptTolerance; + casadiOptions["expand"] = true; + + 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()); + + casadi::DM zero = casadi::DM::zeros(frictionCone.getA().rows(), 1); + + 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) + { + + const casadi::MX constraint = casadi::MX::mtimes(frictionConeMatrix, corner.force); + + for (int i = 0; i < corner.force.columns(); i++) + { + this->opti.subject_to(constraint(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())); + + 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.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::get() const +{ + return m_pimpl->state; +} + +bool CentroidalMPC::isValid() const +{ + return true; +} + +bool CentroidalMPC::advance() +{ + constexpr auto errorPrefix = "[CentroidalMPC::advance]"; + assert(m_pimpl); + + 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; + 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); + } + + // 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 = (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; + } + + // 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..0250682b88 --- /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) diff --git a/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp new file mode 100644 index 0000000000..fc27153e7d --- /dev/null +++ b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp @@ -0,0 +1,140 @@ +/** + * @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 + +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", 15); + 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", "foot0"); + 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}); + + auto contact1Handler = std::make_shared(); + contact1Handler->setParameter("number_of_corners", 4); + contact1Handler->setParameter("contact_name", "foot1"); + 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}); + + + handler->setGroup("CONTACT_0", contact0Handler); + handler->setGroup("CONTACT_1", contact1Handler); + + handler->setParameter("com_weight", std::vector{1, 1, 100}); + 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; + + + // left foot + // first footstep + Eigen::Vector3d leftPos; + leftPos << 0, -0.8, 0; + manif::SE3d leftTransform(leftPos, manif::SO3d::Identity()); + REQUIRE(contactListMap["foot0"].addContact(leftTransform, 0.0, 1.0)); + + // second footstep + // leftPos(0) = 0.25; + // leftPos(2) = 0.2; + leftTransform = manif::SE3d(leftPos, manif::SO3d::Identity()); + REQUIRE(contactListMap["foot0"].addContact(leftTransform, 2.0, 7.0)); + + // right foot + // first footstep + Eigen::Vector3d rightPos; + rightPos << 0, 0.8, 0; + manif::SE3d rightTransform(rightPos, manif::SO3d::Identity()); + REQUIRE(contactListMap["foot1"].addContact(rightTransform, 0.0, 3.0)); + + // second footstep + // rightPos(0) = 0.25; + // rightPos(2) = 0.2; + rightTransform = manif::SE3d(rightPos, manif::SO3d::Identity()); + REQUIRE(contactListMap["foot1"].addContact(rightTransform, 4.0, 7.0)); + phaseList.setLists(contactListMap); + + Eigen::Vector3d com0; + com0 << 0, 0, 0.53; + + Eigen::MatrixXd comTraj(3, 150); + comTraj.colwise() = com0; + + + // initialize the dynamical system + auto system = std::make_shared(); + system->setState({com0, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); + + ForwardEuler integrator; + integrator.setIntegrationStep(dT); + REQUIRE(integrator.setDynamicalSystem(system)); + + std::ofstream myFile; + myFile.open("data.txt"); + + // TODO reset contatcs + for(int i = 0; i < 60; i++) + { + const auto& [com, dcom, angularMomentum] = system->getState(); + + REQUIRE(mpc.setState(com, dcom, angularMomentum)); + REQUIRE(mpc.setReferenceTrajectory(comTraj)); + REQUIRE(mpc.setContactPhaseList(phaseList)); + REQUIRE(mpc.advance()); + + for (const auto& [key, contact] : mpc.get().contacts) + { + for(const auto& corner : contact.corners) + { + myFile << corner.force.transpose() << " "; + } + } + myFile << com.transpose(); + + myFile << std::endl; + + system->setControlInput({mpc.get().contacts}); + REQUIRE(integrator.integrate(0, dT)); + } + + myFile.close(); +} From f87ca076927156a97fb9f5fc58b8499f9b8d9ddc Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Mar 2021 14:09:05 +0200 Subject: [PATCH 4/9] Enable the compilation of the CentroidalMPC controller --- src/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) 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) From 3dc83df451bec6fa5491157173ca52cf0467415e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Apr 2021 14:28:26 +0200 Subject: [PATCH 5/9] Remove useless print in CentroidalDynamics --- src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp b/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp index 56567b50fc..777e920ad4 100644 --- a/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp +++ b/src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp @@ -58,9 +58,6 @@ bool CentroidalDynamics::dynamics(const double& time, StateDerivative& stateDeri for (const auto& [key, contact] : contacts) { - - log()->info("{} rotation {}.", key, contact.pose.asSO3().rotation()); - for (const auto& corner : contact.corners) { comAcceleration.noalias() += 1 / m_mass * contact.pose.asSO3().act(corner.force); From 9f973cbe8606f4fae0ae3f00eef9e950f8c410de Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 23 Apr 2021 11:41:53 +0200 Subject: [PATCH 6/9] Add the possibility to enable/disable the SE3Task controller in IK component --- src/IK/include/BipedalLocomotion/IK/SE3Task.h | 7 ++++++ src/IK/src/SE3Task.cpp | 23 ++++++++++++++----- 2 files changed, 24 insertions(+), 6 deletions(-) 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); +} From 17df8a6bfb1ac351d517c5bf28345f1bfbbc7935 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 23 Apr 2021 11:43:27 +0200 Subject: [PATCH 7/9] Change the signature of LinearizedFrictionCone::initialize() --- .../include/BipedalLocomotion/Math/LinearizedFrictionCone.h | 2 +- src/Math/src/LinearizedFrictionCone.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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]"; From f649616c88be774c8c3c6adbe57fc3b32ace8cf2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 23 Apr 2021 11:45:17 +0200 Subject: [PATCH 8/9] Continue the implementation of CentroidalMPC controller --- .../ReducedModelControllers/CentroidalMPC.h | 12 +- .../src/CentroidalMPC.cpp | 157 ++++++++++++++++-- .../tests/CMakeLists.txt | 2 +- 3 files changed, 147 insertions(+), 24 deletions(-) diff --git a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h index c78f237e87..f247869489 100644 --- a/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h @@ -12,7 +12,7 @@ #include #include -#include +#include #include #include #include @@ -31,7 +31,7 @@ struct CentroidalMPCState * DCMPlanner defines a trajectory generator for the variable height Divergent component of motion * (DCM). */ -class CentroidalMPC : public System::Advanceable +class CentroidalMPC : public System::Source { /** * Private implementation @@ -68,7 +68,7 @@ class CentroidalMPC : public System::Advanceable * | `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); + bool initialize(std::weak_ptr handler) final; bool setContactPhaseList(const Contacts::ContactPhaseList &contactPhaseList); @@ -82,15 +82,13 @@ class CentroidalMPC : public System::Advanceable * @brief Get the object. * @return a const reference of the requested object. */ - const CentroidalMPCState& get() const final; + 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 isValid() const final; - - + bool isOutputValid() const final; /** * @brief Advance the internal state. This may change the value retrievable from get(). diff --git a/src/ReducedModelControllers/src/CentroidalMPC.cpp b/src/ReducedModelControllers/src/CentroidalMPC.cpp index cfda172edf..0d860d8a27 100644 --- a/src/ReducedModelControllers/src/CentroidalMPC.cpp +++ b/src/ReducedModelControllers/src/CentroidalMPC.cpp @@ -113,6 +113,18 @@ struct CentroidalMPC::Impl OptimizationSettings optiSettings; /**< Settings */ + struct OptimizationSolution + { + std::unique_ptr 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. */ @@ -160,7 +172,7 @@ struct CentroidalMPC::Impl }; Weights weights; /**< Settings */ - bool loadContactCorners(std::shared_ptr ptr, + bool loadContactCorners(std::shared_ptr ptr, ContactWithCorners& contact) { constexpr auto errorPrefix = "[CentroidalMPC::Impl::loadContactCorners]"; @@ -198,7 +210,7 @@ struct CentroidalMPC::Impl return true; } - bool loadParameters(std::shared_ptr ptr) + bool loadParameters(std::shared_ptr ptr) { constexpr auto errorPrefix = "[CentroidalMPC::Impl::loadParameters]"; @@ -468,7 +480,34 @@ struct CentroidalMPC::Impl */ void setupOptiOptions() { - casadi::Dict 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"] = 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) @@ -479,11 +518,19 @@ struct CentroidalMPC::Impl } else { ipoptOptions["print_level"] = 0; - casadiOptions["print_time"] = false; + 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; - ipoptOptions["tol"] = this->optiSettings.ipoptTolerance; casadiOptions["expand"] = true; + // casadiOptions["warm"] = true; + // ipoptOptions["verbose"] = false; // + + // casadiOptions["qpsol_options"] = ipoptOptions; this->opti.solver("ipopt", casadiOptions, ipoptOptions); } @@ -543,7 +590,8 @@ struct CentroidalMPC::Impl frictionCone.getA().data(), sizeof(double) * frictionCone.getA().rows() * frictionCone.getA().cols()); - casadi::DM zero = casadi::DM::zeros(frictionCone.getA().rows(), 1); + const casadi::DM zero = casadi::DM::zeros(frictionCone.getA().rows(), 1); + casadi::MX constraintFrictionCone; for(const auto& [key, contact] : this->optiVariables.contacts) { @@ -562,12 +610,10 @@ struct CentroidalMPC::Impl // contact.maximumNormalForce for each corner. At this stage is too premature. for (const auto& corner : contact.corners) { - - const casadi::MX constraint = casadi::MX::mtimes(frictionConeMatrix, corner.force); - + constraintFrictionCone = casadi::MX::mtimes(frictionConeMatrix, corner.force); for (int i = 0; i < corner.force.columns(); i++) { - this->opti.subject_to(constraint(Sl(), i) <= zero); + this->opti.subject_to(constraintFrictionCone(Sl(), i) <= zero); } // limit on the normal force @@ -589,15 +635,24 @@ struct CentroidalMPC::Impl * 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())); @@ -637,7 +692,7 @@ struct CentroidalMPC::Impl } }; -bool CentroidalMPC::initialize(std::weak_ptr handler) +bool CentroidalMPC::initialize(std::weak_ptr handler) { constexpr auto errorPrefix = "[CentroidalMPC::initialize]"; auto ptr = handler.lock(); @@ -668,12 +723,12 @@ CentroidalMPC::CentroidalMPC() m_pimpl = std::make_unique(); } -const CentroidalMPCState& CentroidalMPC::get() const +const CentroidalMPCState& CentroidalMPC::getOutput() const { return m_pimpl->state; } -bool CentroidalMPC::isValid() const +bool CentroidalMPC::isOutputValid() const { return true; } @@ -683,6 +738,8 @@ 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.", @@ -693,6 +750,38 @@ bool CentroidalMPC::advance() // 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); @@ -709,6 +798,32 @@ bool CentroidalMPC::advance() 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); @@ -853,8 +968,8 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList &contac if (finalPhase == contactPhaseList.end()) { finalPhase = std::prev(contactPhaseList.end()); - // std::cerr << "error" << std::endl; - // return false; + std::cerr << "error" << std::endl; + return false; } int index = 0; @@ -864,7 +979,8 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList &contac const double tInitial = std::max(m_pimpl->currentTime, it->beginTime); const double tFinal = std::min(absoluteTimeHorizon, it->endTime); - const int numberOfSamples = (tFinal - tInitial) / m_pimpl->optiSettings.samplingTime; + const int numberOfSamples + = std::round((tFinal - tInitial) / m_pimpl->optiSettings.samplingTime); for (const auto& [key, contact] : it->activeContacts) { @@ -894,6 +1010,15 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList &contac 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) { diff --git a/src/ReducedModelControllers/tests/CMakeLists.txt b/src/ReducedModelControllers/tests/CMakeLists.txt index 0250682b88..b6cce9b37d 100644 --- a/src/ReducedModelControllers/tests/CMakeLists.txt +++ b/src/ReducedModelControllers/tests/CMakeLists.txt @@ -5,4 +5,4 @@ add_bipedal_test( NAME CentroidalMPC SOURCES CentroidalMPCTest.cpp - LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math BipedalLocomotion::ContinuousDynamicalSystem) + LINKS BipedalLocomotion::ReducedModelControllers BipedalLocomotion::Math BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::Planners) From 47fcb807d68cc9df6bb5ec859e91c355d4abea37 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 6 May 2021 11:48:12 +0200 Subject: [PATCH 9/9] Update CentroidalMPCTest --- .../tests/CentroidalMPCTest.cpp | 153 +++++++++++++----- 1 file changed, 114 insertions(+), 39 deletions(-) diff --git a/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp index fc27153e7d..2cf64c9d2b 100644 --- a/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp +++ b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp @@ -14,6 +14,7 @@ #include #include #include +#include using namespace BipedalLocomotion::ParametersHandler; using namespace BipedalLocomotion::ReducedModelControllers; @@ -27,32 +28,31 @@ TEST_CASE("CentroidalMPC") std::shared_ptr handler = std::make_shared(); handler->setParameter("controller_sampling_time", dT); - handler->setParameter("controller_horizon", 15); + 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", "foot0"); + 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("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", "foot1"); + 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("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, 100}); + 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); @@ -66,74 +66,149 @@ TEST_CASE("CentroidalMPC") 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, -0.8, 0; + leftPos << -0.0852893, 0.0828008, -0.000110952; manif::SE3d leftTransform(leftPos, manif::SO3d::Identity()); - REQUIRE(contactListMap["foot0"].addContact(leftTransform, 0.0, 1.0)); - - // second footstep - // leftPos(0) = 0.25; - // leftPos(2) = 0.2; - leftTransform = manif::SE3d(leftPos, manif::SO3d::Identity()); - REQUIRE(contactListMap["foot0"].addContact(leftTransform, 2.0, 7.0)); + 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, 0.8, 0; + rightPos << -0.0850079, -0.073266, -0.000111252; manif::SE3d rightTransform(rightPos, manif::SO3d::Identity()); - REQUIRE(contactListMap["foot1"].addContact(rightTransform, 0.0, 3.0)); + 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); - // second footstep - // rightPos(0) = 0.25; - // rightPos(2) = 0.2; - rightTransform = manif::SE3d(rightPos, manif::SO3d::Identity()); - REQUIRE(contactListMap["foot1"].addContact(rightTransform, 4.0, 7.0)); phaseList.setLists(contactListMap); - Eigen::Vector3d com0; - com0 << 0, 0, 0.53; + 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); - Eigen::MatrixXd comTraj(3, 150); - comTraj.colwise() = com0; + // 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({com0, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); + system->setState({comTraj.col(0), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); ForwardEuler integrator; - integrator.setIntegrationStep(dT); + integrator.setIntegrationStep(dT/10.0); REQUIRE(integrator.setDynamicalSystem(system)); std::ofstream myFile; myFile.open("data.txt"); // TODO reset contatcs - for(int i = 0; i < 60; i++) + int controllerIndex = 0; + int index = 0; + for(int i = 0; i < 1000; i++) { const auto& [com, dcom, angularMomentum] = system->getState(); - REQUIRE(mpc.setState(com, dcom, angularMomentum)); - REQUIRE(mpc.setReferenceTrajectory(comTraj)); - REQUIRE(mpc.setContactPhaseList(phaseList)); - REQUIRE(mpc.advance()); + 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.get().contacts) + for (const auto& [key, contact] : mpc.getOutput().contacts) { for(const auto& corner : contact.corners) { myFile << corner.force.transpose() << " "; } } - myFile << com.transpose(); + myFile << com.transpose() << " " << comTraj.col(index).transpose() << " " << angularMomentum.transpose(); myFile << std::endl; - system->setControlInput({mpc.get().contacts}); - REQUIRE(integrator.integrate(0, dT)); + system->setControlInput({mpc.getOutput().contacts}); + + + REQUIRE(integrator.integrate(0, 0.01)); + + controllerIndex++; + if(controllerIndex == 20) + controllerIndex =0; } myFile.close();