diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7ebdb371e7..2d6bc5c06b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -20,3 +20,4 @@ add_subdirectory(TSID) add_subdirectory(Perception) add_subdirectory(IK) add_subdirectory(SimplifiedModelControllers) +add_subdirectory(ReducedModelControllers) diff --git a/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h b/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h index 4d7dd43f4c..e652df51b1 100644 --- a/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h +++ b/src/Contacts/include/BipedalLocomotion/ContactDetectors/FixedFootDetector.h @@ -92,6 +92,13 @@ class FixedFootDetector : public ContactDetector */ bool updateFixedFoot(); + /** + * Set the internal buffer related to the ContactPhaseList + * @param phaseList a contact phase list + * @note this method sets only the internal buffer used by the detector + */ + void setContactPhaseListPrivate(const ContactPhaseList& phaseList); + public: /** @@ -100,6 +107,16 @@ class FixedFootDetector : public ContactDetector */ bool setContactPhaseList(const ContactPhaseList& phaseList); + /** + * Set the contact phase list + * @param phaseList a contact phase list + * @warning this method does not update the m_currentTime variable. It should be used only if + * the setContactPhaseList method has been called at least once. Here we may have some + * discontinuity in the estimated contact. Please use it only if you are confident on what you + * are doing. + */ + bool setContactPhaseListWithoutResetInternalTime(const ContactPhaseList& phaseList); + /** * Get the fixed foot * @return the fixed foot. 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/Contacts/src/FixedFootDetector.cpp b/src/Contacts/src/FixedFootDetector.cpp index 859bc8fe73..a6bc95f113 100644 --- a/src/Contacts/src/FixedFootDetector.cpp +++ b/src/Contacts/src/FixedFootDetector.cpp @@ -161,7 +161,7 @@ bool FixedFootDetector::updateContactStates() return this->updateFixedFoot(); } -bool FixedFootDetector::setContactPhaseList(const Contacts::ContactPhaseList& phaseList) +void FixedFootDetector::setContactPhaseListPrivate(const Contacts::ContactPhaseList& phaseList) { m_contactPhaselist = phaseList; @@ -198,6 +198,21 @@ bool FixedFootDetector::setContactPhaseList(const Contacts::ContactPhaseList& ph m_contactStates[contact].name = contact; m_contactStates[contact].index = phaseList.lists().find(contact)->second.cbegin()->index; } +} + +bool FixedFootDetector::setContactPhaseListWithoutResetInternalTime( + const Contacts::ContactPhaseList& phaseList) +{ + this->setContactPhaseListPrivate(phaseList); + + // update the fixed foot + return this->updateFixedFoot(); +} + +bool FixedFootDetector::setContactPhaseList(const Contacts::ContactPhaseList& phaseList) +{ + + this->setContactPhaseListPrivate(phaseList); // set the initial time m_currentTime = phaseList.firstPhase()->beginTime; 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..233521d129 --- /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, Eigen::Vector3d)) + +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..8676d0380e --- /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); + const auto& externalDisturbance = std::get<1>(m_controlInput); + comAcceleration += externalDisturbance; + + for (const auto& [key, contact] : contacts) + { + for (const auto& corner : contact.corners) + { + comAcceleration.noalias() += 1 / m_mass * corner.force; + angularMomentumRate.noalias() += (contact.pose.act(corner.position) - comPosition) + .cross(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/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h index e4760648c5..1c065630e1 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h @@ -92,6 +92,8 @@ class SwingFootPlanner : public System::Source */ void setContactList(const Contacts::ContactList& contactList); + void setContactListWithotResettingTheInternalTime(const Contacts::ContactList& contactList); + /** * @brief Get the object. * @return a const reference of the requested object. diff --git a/src/Planners/src/SwingFootPlanner.cpp b/src/Planners/src/SwingFootPlanner.cpp index 5a1b08c4e0..f59afcdcdf 100644 --- a/src/Planners/src/SwingFootPlanner.cpp +++ b/src/Planners/src/SwingFootPlanner.cpp @@ -123,6 +123,19 @@ bool SwingFootPlanner::initialize(std::weak_ptrpose; + // m_state.mixedVelocity.setZero(); + // m_state.mixedAcceleration.setZero(); + // m_state.isInContact = true; +} + + void SwingFootPlanner::setContactList(const ContactList& contactList) { // reset the time 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..2682fedd69 --- /dev/null +++ b/src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h @@ -0,0 +1,106 @@ +/** + * @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 +#include + +namespace BipedalLocomotion +{ +namespace ReducedModelControllers +{ + +struct CentroidalMPCState +{ + std::map contacts; + std::map nextPlannedContact; + double computationalTime{0}; + Eigen::Vector3d externalWrench; +}; + +/** + * 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, + std::optional> externalWrench = {}); + 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..eae7aae555 --- /dev/null +++ b/src/ReducedModelControllers/src/CentroidalMPC.cpp @@ -0,0 +1,1205 @@ +/** + * @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 +#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()); +} + +std::vector extractVariablesName(const std::vector& variables) +{ + std::vector variablesName; + variablesName.reserve(variables.size()); + for (const auto& variable : variables) + { + variablesName.push_back(variable.name()); + } + + return variablesName; +} + +template +auto extractFutureValuesFromState(T& variable) +{ + using Sl = casadi::Slice; + return variable(Sl(), Sl(1, variable.columns())); +} + +template +auto extractFutureValuesFromState(const T& variable) +{ + using Sl = casadi::Slice; + return variable(Sl(), Sl(1, variable.columns())); +} + +struct CentroidalMPC::Impl +{ + casadi::Opti opti; /**< CasADi opti stack */ + casadi::Function controller; + bool isInitialized{false}; + double currentTime{0}; + + CentroidalMPCState state; + Contacts::ContactPhaseList contactPhaseList; + 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 linearVelocity; + casadi::MX orientation; + casadi::MX isEnable; + 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); + this->linearVelocity = casadi::MX::sym("linear_velocity", 3); + this->isEnable = casadi::MX::sym("is_enable"); + + return *this; + } + + CasadiContact(const ContactWithCorners& other) + { + this->operator=(other); + } + }; + + + struct CasadiContactWithConstraints : CasadiContact + { + casadi::MX currentPosition; + casadi::MX nominalPosition; + casadi::MX upperLimitPosition; + casadi::MX lowerLimitPosition; + }; + + 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; + casadi::MX externalWrench; + }; + OptimizationVariables optiVariables; /**< Optimization variables */ + + + struct ContactsInputs + { + casadi::DM currentPosition; + casadi::DM orientation; + casadi::DM nominalPosition; + casadi::DM upperLimitPosition; + casadi::DM lowerLimitPosition; + casadi::DM isEnable; + }; + struct ControllerInputs + { + std::map contacts; + + casadi::DM comReference; + casadi::DM comCurrent; + casadi::DM dcomCurrent; + casadi::DM angularMomentumCurrent; + casadi::DM externalWrench; + }; + ControllerInputs controllerInputs; + + struct Weights + { + Eigen::Vector3d com; + double contactPosition; + Eigen::Vector3d forceRateOfChange; + double angularMomentum; + }; + Weights weights; /**< Settings */ + + struct ContactBoundingBox + { + Eigen::Vector3d upperLimit; + Eigen::Vector3d lowerLimit; + }; + + std::unordered_map contactBoundingBoxes; + + 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; + } + + // set the contact name + this->state.contacts[contactName].name = contactName; + + if (!contactHandler->getParameter("bounding_box_upper_limit", + this->contactBoundingBoxes[contactName].upperLimit)) + { + log()->error("{} Unable to load the bounding box upper limit of the contact number " + "{}.", + errorPrefix, + i); + return false; + } + + if (!contactHandler->getParameter("bounding_box_lower_limit", + this->contactBoundingBoxes[contactName].lowerLimit)) + { + log()->error("{} Unable to load the bounding box lower limit 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 angularMomentum = casadi::MX::sym("angular_momentum", 3); + + casadi::MX externalForce = casadi::MX::sym("external_force", 3); + + casadi::MX ddcom = casadi::MX::sym("ddcom", 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 + externalForce / mass; + angularMomentumDerivative = casadi::DM::zeros(3); + + std::vector input; + input.push_back(externalForce); + input.push_back(com); + input.push_back(dcom); + input.push_back(angularMomentum); + + for(const auto& [key, contact] : casadiContacts) + { + input.push_back(contact.position); + input.push_back(contact.orientation); + input.push_back(contact.isEnable); + input.push_back(contact.linearVelocity); + + for(const auto& corner : contact.corners) + { + + ddcom += contact.isEnable/mass * corner.force; + angularMomentumDerivative + += contact.isEnable + * casadi::MX::cross(casadi::MX::mtimes(casadi::MX::reshape(contact + .orientation, + 3, + 3), + corner.position) + + contact.position - com, + corner.force); + + input.push_back(corner.force); + } + } + + std::vector outputName{"com", "dcom", "angular_momentum"}; + std::vector rhs{com + dcom * this->optiSettings.samplingTime, + dcom + ddcom * this->optiSettings.samplingTime, + angularMomentum + + angularMomentumDerivative + * this->optiSettings.samplingTime}; + + for (const auto& [key, contact] : casadiContacts) + { + rhs.push_back(contact.position + + (1 - contact.isEnable) * contact.linearVelocity + * this->optiSettings.samplingTime); + outputName.push_back(key); + } + + return casadi::Function("centroidal_dynamics", + input, + rhs, + extractVariablesName(input), + outputName); + } + + casadi::Function contactPositionError() + { + casadi::MX contactPosition = casadi::MX::sym("contact_position", 3); + casadi::MX nominalContactPosition = casadi::MX::sym("nominal_contact_position", 3); + casadi::MX contactOrientation = casadi::MX::sym("contact_orientation", 3 * 3); + + // the orientation is stored as a vectorized version of the matrix. We need to reshape it + casadi::MX rhs = casadi::MX::mtimes(casadi::MX::reshape(contactOrientation, 3, 3).T(), + contactPosition - nominalContactPosition); + + return casadi::Function("contact_position_error", + {contactPosition, nominalContactPosition, contactOrientation}, + {rhs}, + extractVariablesName({contactPosition, // + nominalContactPosition, + contactOrientation}), + {"error"}); + } + + void resizeControllerInputs() + { + constexpr int vector3Size = 3; + const int stateHorizon = this->optiSettings.horizon + 1; + + // 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, stateHorizon); + this->controllerInputs.externalWrench = casadi::DM::zeros(vector3Size, // + this->optiSettings.horizon); + + for (const auto& [key, contact] : this->state.contacts) + { + // The current position of the contact + this->controllerInputs.contacts[key].currentPosition + = casadi::DM::zeros(vector3Size); + + // The orientation is stored as a vectorized version of the rotation matrix + this->controllerInputs.contacts[key].orientation + = casadi::DM::zeros(9, this->optiSettings.horizon); + + // 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].isEnable + = 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, stateHorizon); + } + } + + void populateOptiVariables() + { + constexpr int vector3Size = 3; + const int stateHorizon = this->optiSettings.horizon + 1; + + // create the variables for the state + this->optiVariables.com = this->opti.variable(vector3Size, stateHorizon); + this->optiVariables.dcom = this->opti.variable(vector3Size, stateHorizon); + this->optiVariables.angularMomentum = this->opti.variable(vector3Size, stateHorizon); + + // the casadi contacts depends on the maximum number of contacts + for (const auto& [key, contact] : this->state.contacts) + { + auto [contactIt, outcome] + = this->optiVariables.contacts.insert_or_assign(key, CasadiContactWithConstraints()); + + + auto& c = contactIt->second; + + // each contact has a different number of corners + c.corners.resize(contact.corners.size()); + + // the position of the contact is an optimization variable + c.position = this->opti.variable(vector3Size, stateHorizon); + + // the linear velocity of the contact is an optimization variable + c.linearVelocity = this->opti.variable(vector3Size, this->optiSettings.horizon); + + // the orientation is a parameter. The orientation is stored as a vectorized version of + // the rotation matrix + c.orientation = this->opti.parameter(9, this->optiSettings.horizon); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + c.upperLimitPosition = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + c.lowerLimitPosition = this->opti.parameter(vector3Size, this->optiSettings.horizon); + + // Maximum admissible contact force. It is expressed in the contact body frame + c.isEnable = this->opti.parameter(1, this->optiSettings.horizon); + + // The nominal contact position is a parameter that regularize the solution + c.nominalPosition = this->opti.parameter(vector3Size, stateHorizon); + + c.currentPosition = this->opti.parameter(vector3Size); + + for (int j = 0; j < contact.corners.size(); j++) + { + c.corners[j].force = this->opti.variable(vector3Size, this->optiSettings.horizon); + + c.corners[j].position + = casadi::DM(std::vector(contact.corners[j].position.data(), + contact.corners[j].position.data() + + contact.corners[j].position.size())); + } + } + + this->optiVariables.comCurrent = this->opti.parameter(vector3Size); + this->optiVariables.dcomCurrent = this->opti.parameter(vector3Size); + this->optiVariables.angularMomentumCurrent = this->opti.parameter(vector3Size); + this->optiVariables.comReference = this->opti.parameter(vector3Size, stateHorizon); + this->optiVariables.externalWrench = 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; + casadiOptions["expand"] = true; + + this->opti.solver("ipopt", casadiOptions, ipoptOptions); + } + + casadi::Function createController() + { + using Sl = casadi::Slice; + + this->populateOptiVariables(); + + // + auto& com = this->optiVariables.com; + auto& dcom = this->optiVariables.dcom; + auto& angularMomentum = this->optiVariables.angularMomentum; + auto& externalWrench = this->optiVariables.externalWrench; + + // prepare the input of the ode + std::vector odeInput; + odeInput.push_back(externalWrench); + odeInput.push_back(com(Sl(), Sl(0, -1))); + odeInput.push_back(dcom(Sl(), Sl(0, -1))); + odeInput.push_back(angularMomentum(Sl(), Sl(0, -1))); + for (const auto& [key, contact] : this->optiVariables.contacts) + { + odeInput.push_back(contact.position(Sl(), Sl(0, -1))); + odeInput.push_back(contact.orientation); + odeInput.push_back(contact.isEnable); + odeInput.push_back(contact.linearVelocity); + + for(const auto& corner : contact.corners) + { + odeInput.push_back(corner.force); + } + } + + // 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)); + for (const auto& [key, contact] : this->optiVariables.contacts) + { + this->opti.subject_to(this->optiVariables.contacts.at(key).currentPosition + == contact.position(Sl(), 0)); + } + + // set the dynamics + // map computes the multiple shooting method + auto dynamics = this->ode().map(this->optiSettings.horizon); + auto fullTrajectory = dynamics(odeInput); + this->opti.subject_to(extractFutureValuesFromState(com) == fullTrajectory[0]); + this->opti.subject_to(extractFutureValuesFromState(dcom) == fullTrajectory[1]); + this->opti.subject_to(extractFutureValuesFromState(angularMomentum) == fullTrajectory[2]); + + // footstep dynamics + std::size_t contactIndex = 0; + for (const auto& [key, contact] : this->optiVariables.contacts) + { + this->opti.subject_to(extractFutureValuesFromState(contact.position) + == fullTrajectory[3 + contactIndex]); + contactIndex++; + } + + // add constraints for the contacts + auto contactPositionErrorMap = this->contactPositionError().map(this->optiSettings.horizon); + + // convert the eigen matrix into casadi + // please check https://github.com/casadi/casadi/issues/2563 and + // https://groups.google.com/forum/#!topic/casadi-users/npPcKItdLN8 + // Assumption: the matrices 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 rotatedFrictionCone; + + for(const auto& [key, contact] : this->optiVariables.contacts) + { + auto error = contactPositionErrorMap({extractFutureValuesFromState(contact.position), + extractFutureValuesFromState(contact.nominalPosition), + contact.orientation}); + + this->opti.subject_to(contact.lowerLimitPosition <= error[0] + <= contact.upperLimitPosition); + + for (int i = 0; i < this->optiSettings.horizon; i++) + { + rotatedFrictionCone + = casadi::MX::mtimes(frictionConeMatrix, // + casadi::MX::reshape(contact.orientation(Sl(), i), 3, 3) + .T()); + + // TODO please if you want to add heel to toe motion you should define a + // contact.maximumNormalForce for each corner. At this stage is too premature. + for (const auto& corner : contact.corners) + { + this->opti.subject_to(casadi::MX::mtimes(rotatedFrictionCone, // + corner.force(Sl(), i)) + <= zero); + + // limit on the normal force + this->opti.subject_to( + 0 <= casadi::MX::mtimes(casadi::MX::reshape(contact.orientation(Sl(), i), + 3, + 3), + corner.force(Sl(), i)(2))); + } + } + } + + // create the cost function + auto& comReference = this->optiVariables.comReference; + + casadi::DM weightCoMZ = casadi::DM::zeros(1, com.columns()); + double min = this->weights.com(2)/2; + for (int i = 0; i < com.columns(); i++) + { + weightCoMZ(Sl(), i) = (this->weights.com(2) - min) * std::exp(-i) + min; + } + + std::cerr << "------------------_>>> " << weightCoMZ << std::endl; + + // (max - mix) * expo + min + + casadi::MX cost + = this->weights.angularMomentum * 10 * casadi::MX::sumsqr(angularMomentum(0, Sl())) + + this->weights.angularMomentum * casadi::MX::sumsqr(angularMomentum(1, Sl())) + + this->weights.angularMomentum * casadi::MX::sumsqr(angularMomentum(2, Sl())) + + this->weights.com(0) * casadi::MX::sumsqr(com(0, Sl()) - comReference(0, Sl())) + + this->weights.com(1) * casadi::MX::sumsqr(com(1, Sl()) - comReference(1, Sl())) + + casadi::MX::sumsqr(weightCoMZ * (com(2, Sl()) - comReference(2, Sl()))); + + casadi::MX averageForce; + for (const auto& [key, contact] : this->optiVariables.contacts) + { + cost += this->weights.contactPosition + * casadi::MX::sumsqr(contact.nominalPosition - contact.position); + + averageForce = casadi::MX::vertcat( + {contact.isEnable * contact.corners[0].force(0, Sl()) / contact.corners.size(), + contact.isEnable * contact.corners[0].force(1, Sl()) / contact.corners.size(), + contact.isEnable * contact.corners[0].force(2, Sl()) / contact.corners.size()}); + for (int i = 1; i < contact.corners.size(); i++) + { + averageForce += casadi::MX::vertcat( + {contact.isEnable * contact.corners[i].force(0, Sl()) / contact.corners.size(), + contact.isEnable * contact.corners[i].force(1, Sl()) / contact.corners.size(), + contact.isEnable * contact.corners[i].force(2, Sl()) + / 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; + std::vector inputName; + std::vector outputName; + + input.push_back(this->optiVariables.externalWrench); + input.push_back(this->optiVariables.comCurrent); + input.push_back(this->optiVariables.dcomCurrent); + input.push_back(this->optiVariables.angularMomentumCurrent); + input.push_back(this->optiVariables.comReference); + + inputName.push_back("external_wrench"); + inputName.push_back("com_current"); + inputName.push_back("dcom_current"); + inputName.push_back("angular_momentum_current"); + inputName.push_back("com_reference"); + + for (const auto& [key, contact] : this->optiVariables.contacts) + { + input.push_back(contact.currentPosition); + input.push_back(contact.nominalPosition); + input.push_back(contact.orientation); + input.push_back(contact.isEnable); + input.push_back(contact.upperLimitPosition); + input.push_back(contact.lowerLimitPosition); + + inputName.push_back("contact_" + key + "_current_position"); + inputName.push_back("contact_" + key + "_nominal_position"); + inputName.push_back("contact_" + key + "_orientation"); + inputName.push_back("contact_" + key + "_is_enable"); + inputName.push_back("contact_" + key + "_upper_limit_position"); + inputName.push_back("contact_" + key + "_lower_limit_position"); + + output.push_back(contact.isEnable); + output.push_back(contact.position); + output.push_back(contact.orientation); + + + outputName.push_back("contact_" + key + "_is_enable"); + outputName.push_back("contact_" + key + "_position"); + outputName.push_back("contact_" + key + "_orientation"); + + std::size_t cornerIndex = 0; + for (const auto& corner : contact.corners) + { + output.push_back(corner.force); + outputName.push_back("contact_" + key + "_corner_" + std::to_string(cornerIndex) + + "_force"); + cornerIndex ++; + } + } + + // controller:(com_current[3], dcom_current[3], angular_momentum_current[3], + // com_reference[3x16], contact_left_foot_current_position[3], + // contact_left_foot_nominal_position[3x16], contact_left_foot_orientation[9x15], + // contact_left_foot_is_enable[1x15], contact_left_foot_upper_limit_position[3x15], + // contact_left_foot_lower_limit_position[3x15], contact_right_foot_current_position[3], + // contact_right_foot_nominal_position[3x16], contact_right_foot_orientation[9x15], + // contact_right_foot_is_enable[1x15], contact_right_foot_upper_limit_position[3x15], + // contact_right_foot_lower_limit_position[3x15]) --------------------> + // (contact_left_foot_position[3x16], + // contact_left_foot_is_enable[1x15], contact_left_foot_corner_0_force[3x15], + // contact_left_foot_corner_1_force[3x15], contact_left_foot_corner_2_force[3x15], + // contact_left_foot_corner_3_force[3x15], contact_right_foot_position[3x16], + // contact_right_foot_is_enable[1x15], contact_right_foot_corner_0_force[3x15], + // contact_right_foot_corner_1_force[3x15], contact_right_foot_corner_2_force[3x15], + // contact_right_foot_corner_3_force[3x15]) + return this->opti.to_function("controller", input, output, inputName, outputName); + } +}; + +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:(com_current[3], dcom_current[3], angular_momentum_current[3], com_reference[3x16], + // contact_left_foot_current_position[3], contact_left_foot_nominal_position[3x16], contact_left_foot_orientation[9x15], + // contact_left_foot_is_enable[1x15], contact_left_foot_upper_limit_position[3x15], contact_left_foot_lower_limit_position[3x15], + // contact_right_foot_current_position[3], contact_right_foot_nominal_position[3x16], contact_right_foot_orientation[9x15], + // contact_right_foot_is_enable[1x15], contact_right_foot_upper_limit_position[3x15], contact_right_foot_lower_limit_position[3x15]) + // --------------------> + // (contact_left_foot_position[3x16], contact_left_foot_is_enable[1x15], contact_left_foot_corner_0_force[3x15], + // contact_left_foot_corner_1_force[3x15], + // contact_left_foot_corner_2_force[3x15], + // contact_left_foot_corner_3_force[3x15], + // contact_right_foot_position[3x16], contact_right_foot_is_enable[1x15], contact_right_foot_corner_0_force[3x15], + // contact_right_foot_corner_1_force[3x15], + // contact_right_foot_corner_2_force[3x15], + // contact_right_foot_corner_3_force[3x15]) + + const auto& inputs = m_pimpl->controllerInputs; + + std::vector vectorizedInputs; + vectorizedInputs.push_back(inputs.externalWrench); + 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.currentPosition); + vectorizedInputs.push_back(contact.nominalPosition); + vectorizedInputs.push_back(contact.orientation); + vectorizedInputs.push_back(contact.isEnable); + vectorizedInputs.push_back(contact.upperLimitPosition); + vectorizedInputs.push_back(contact.lowerLimitPosition); + } + + // compute the output + auto controllerOutput = m_pimpl->controller(vectorizedInputs); + + // get the solution + auto it = controllerOutput.begin(); + m_pimpl->state.nextPlannedContact.clear(); + for (auto & [key, contact] : m_pimpl->state.contacts) + { + // the first output tell us if a contact is enabled + + + log()->info("actovation sequence {}", toEigen(*it)); + + int index = toEigen(*it).size(); + const int size = toEigen(*it).size(); + for (int i = 0; i < size; i++) + { + if (toEigen(*it)(i) > 0.5) + { + if (i == 0) + { + break; + } else if (toEigen(*it)(i - 1) < 0.5) + { + index = i; + break; + } + } + } + + double isEnable = toEigen(*it)(0); + std::advance(it, 1); + contact.pose.translation(toEigen(*it).leftCols<1>()); + + if (index < size) + { + m_pimpl->state.nextPlannedContact[key].name = key; + m_pimpl->state.nextPlannedContact[key].pose.translation(toEigen(*it).col(index)); + } + + std::advance(it, 1); + contact.pose.quat(Eigen::Quaterniond( + Eigen::Map(toEigen(*it).leftCols<1>().data()))); + + if (index < size) + { + m_pimpl->state.nextPlannedContact[key].pose.quat(Eigen::Quaterniond( + Eigen::Map(toEigen(*it).leftCols<1>().data()))); + + // this is wrong but if I dont put index + 1 I get strange values when index = 1 + const double nextPlannedContactTime + = m_pimpl->currentTime + m_pimpl->optiSettings.samplingTime * (index + 1); + auto nextPlannedContact = m_pimpl->contactPhaseList.lists().at(key).getPresentContact( + nextPlannedContactTime); + if (nextPlannedContact == m_pimpl->contactPhaseList.lists().at(key).end()) + { + log()->error("[CentroidalMPC::advance] Unable to get the next planned contact"); + return false; + } + + log()->warn("[CentroidalMPC] key {} next planned contact pose {} activation time {} deactivation time {} next planned contact time {} index {}", + key, + m_pimpl->state.nextPlannedContact[key].pose.translation().transpose(), + nextPlannedContact->activationTime, + nextPlannedContact->deactivationTime, + nextPlannedContactTime, index); + + m_pimpl->state.nextPlannedContact[key].activationTime = nextPlannedContact->activationTime; + m_pimpl->state.nextPlannedContact[key].deactivationTime = nextPlannedContact->deactivationTime; + m_pimpl->state.nextPlannedContact[key].index = nextPlannedContact->index; + m_pimpl->state.nextPlannedContact[key].type = nextPlannedContact->type; + } + + std::advance(it, 1); + + for (auto& corner : contact.corners) + { + // isEnable == 1 means that the contact is active + if (isEnable > 0.5) + { + corner.force = toEigen(*it).leftCols<1>(); + } else + { + corner.force.setZero(); + } + 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); + + const int stateHorizon = m_pimpl->optiSettings.horizon + 1; + + 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() < stateHorizon) + { + 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(stateHorizon); + + return true; +} + +bool CentroidalMPC::setState(Eigen::Ref com, + Eigen::Ref dcom, + Eigen::Ref angularMomentum, + std::optional> externalWrench) +{ + 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; + + toEigen(inputs.externalWrench).setZero(); + m_pimpl->state.externalWrench = Eigen::Vector3d::Zero(); + + if (externalWrench) + { + toEigen(inputs.externalWrench).leftCols<1>() = externalWrench.value(); + m_pimpl->state.externalWrench = externalWrench.value(); + } + + std::cerr << "external wrench" << std::endl << inputs.externalWrench << std::endl; + + 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; + } + + m_pimpl->contactPhaseList = contactPhaseList; + + // The orientation is stored as a vectorized version of the rotation matrix + const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity(); + + auto& inputs = m_pimpl->controllerInputs; + + // clear previous data + for (const auto& [key, contact] : m_pimpl->state.contacts) + { + // initialize the current contact pose to zero. If the contact is active the current + // position will be set later on + toEigen(inputs.contacts[key].currentPosition).setZero(); + + // initialize all the orientation to the identity + toEigen(inputs.contacts[key].orientation).colwise() + = Eigen::Map(identity.data(), identity.cols() * identity.rows()); + + // Upper limit of the position of the contact. It is expressed in the contact body frame + toEigen(inputs.contacts[key].upperLimitPosition).setZero(); + + // Lower limit of the position of the contact. It is expressed in the contact body frame + toEigen(inputs.contacts[key].lowerLimitPosition).setZero(); + + // Maximum admissible contact force. It is expressed in the contact body frame + toEigen(inputs.contacts[key].isEnable).setZero(); + + // The nominal contact position is a parameter that regularize the solution + toEigen(inputs.contacts[key].nominalPosition).setZero(); + } + + const double absoluteTimeHorizon = m_pimpl->currentTime + m_pimpl->optiSettings.timeHorizon; + + // find the contactPhase associated to the current time + auto initialPhase = contactPhaseList.getPresentPhase(m_pimpl->currentTime); + if (initialPhase == contactPhaseList.end()) + { + log()->error("{} Unable to find the contact phase related to the current time.", + errorPrefix); + return false; + } + + // find the contactPhase associated to the end time + auto finalPhase = contactPhaseList.getPresentPhase(absoluteTimeHorizon); + // if the list is not found the latest contact phase is considered + if (finalPhase == contactPhaseList.end()) + { + finalPhase = std::prev(contactPhaseList.end()); + 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 = inputs.contacts.find(key); + if (inputContact == inputs.contacts.end()) + { + log()->error("{} Unable to find the input contact named {}.", errorPrefix, key); + return false; + } + + toEigen(inputContact->second.nominalPosition) + .middleCols(index, numberOfSamples + 1) + .colwise() + = contact->pose.translation(); + + // this is required to reshape the matrix into a vector + const Eigen::Matrix3d orientation = contact->pose.quat().toRotationMatrix(); + toEigen(inputContact->second.orientation).middleCols(index, numberOfSamples).colwise() + = Eigen::Map(orientation.data(), orientation.size()); + + constexpr double maxForce = 1; + toEigen(inputContact->second.isEnable) + .middleCols(index, numberOfSamples) + .setConstant(maxForce); + } + + index += numberOfSamples; + } + + assert(index == m_pimpl->optiSettings.horizon); + + // set the current contact position to for the active contact only + for (auto& [key, contact] : inputs.contacts) + { + toEigen(contact.currentPosition) = toEigen(contact.nominalPosition).leftCols<1>(); + } + + // for (const auto& [key, contact] : inputs.contacts) + // { + // log()->info("{} current Pose {}:", key, toEigen(contact.currentPosition).transpose()); + // log()->info("{} nominal Pose {}:", key, toEigen(contact.nominalPosition)); + // log()->info("{} maximumnormalforce {}", key, toEigen(contact.isEnable)); + // } + + // TODO this part can be improved. For instance you do not need to fill the vectors every time. + for (auto& [key, contact] : inputs.contacts) + { + const auto& boundingBox = m_pimpl->contactBoundingBoxes.at(key); + toEigen(contact.upperLimitPosition).colwise() = boundingBox.upperLimit; + toEigen(contact.lowerLimitPosition).colwise() = boundingBox.lowerLimit; + } + + 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..b81b142da2 --- /dev/null +++ b/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp @@ -0,0 +1,360 @@ +/** + * @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 + +void updateContactPhaseList( + const std::map& nextPlannedContacts, + BipedalLocomotion::Contacts::ContactPhaseList& phaseList) +{ + auto newList = phaseList.lists(); + for (const auto& [key, contact] : nextPlannedContacts) + { + auto it = newList.at(key).getPresentContact(contact.activationTime); + newList.at(key).editContact(it, contact); + } + + phaseList.setLists(newList); +} + +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); + handler->setParameter("linear_solver", "ma97"); + + auto contact0Handler = std::make_shared(); + contact0Handler->setParameter("number_of_corners", 4); + contact0Handler->setParameter("contact_name", "left_foot"); + contact0Handler->setParameter("corner_0", std::vector{0.1, 0.05, 0}); + contact0Handler->setParameter("corner_1", std::vector{0.1, -0.05, 0}); + contact0Handler->setParameter("corner_2", std::vector{-0.1, -0.05, 0}); + contact0Handler->setParameter("corner_3", std::vector{-0.1, 0.05, 0}); + contact0Handler->setParameter("bounding_box_lower_limit", std::vector{0, 0, 0}); + contact0Handler->setParameter("bounding_box_upper_limit", std::vector{0, 0, 0}); + + auto contact1Handler = std::make_shared(); + contact1Handler->setParameter("number_of_corners", 4); + contact1Handler->setParameter("contact_name", "right_foot"); + contact1Handler->setParameter("corner_0", std::vector{0.1, 0.05, 0}); + contact1Handler->setParameter("corner_1", std::vector{0.1, -0.05, 0}); + contact1Handler->setParameter("corner_2", std::vector{-0.1, -0.05, 0}); + contact1Handler->setParameter("corner_3", std::vector{-0.1, 0.05, 0}); + contact1Handler->setParameter("bounding_box_lower_limit", std::vector{0, 0, 0}); + contact1Handler->setParameter("bounding_box_upper_limit", std::vector{0, 0, 0}); + + + handler->setGroup("CONTACT_0", contact0Handler); + handler->setGroup("CONTACT_1", contact1Handler); + + handler->setParameter("com_weight", std::vector{1, 1, 1000}); + handler->setParameter("contact_position_weight", 1e3); + handler->setParameter("force_rate_of_change_weight", std::vector{10, 10, 10}); + handler->setParameter("angular_momentum_weight", 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 = 1; + constexpr double scalingPos = 4.0; + constexpr double scalingPosY = 12; + + // // t 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 + // // L |+++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|++++++++++|---|+++| + // // R |+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++| + + Eigen::Vector3d leftPosition; + leftPosition << 0, 0.08, 0; + manif::SE3d leftTransform(leftPosition, manif::SO3d::Identity()); + contactListMap["left_foot"].addContact(leftTransform, 0.0, 1.0 * scaling); + + leftPosition(0) += 0.05 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 2.0 * scaling, 5.0 * scaling); + + leftPosition(0) += 0.1 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 6.0 * scaling, 9.0 * scaling); + + leftPosition(0) += 0.05 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 10.0 * scaling, 13.0 * scaling); + + leftPosition(0) += 0.15 * scalingPos; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 14.0 * scaling, 17.0 * scaling); + + leftPosition(1) -= 0.01 * scalingPosY; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 18.0 * scaling, 21.0 * scaling); + + leftPosition(1) -= 0.01 * scalingPosY; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 22.0 * scaling, 25.0 * scaling); + + leftPosition(1) -= 0.01 * scalingPosY; + leftTransform.translation(leftPosition); + contactListMap["left_foot"].addContact(leftTransform, 26.0 * scaling, 29.0 * scaling); + + // // t 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 + // 22 23 24 25 26 27 + // // L + // |+++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|++++++++++|---|+++| + // // R + // |+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++++|---|+++++++++| + + // right foot + // first footstep + Eigen::Vector3d rightPosition; + rightPosition << 0, -0.08, 0; + manif::SE3d rightTransform(rightPosition, manif::SO3d::Identity()); + + contactListMap["right_foot"].addContact(rightTransform, 0.0, 3.0 * scaling); + + rightPosition(0) += 0.1 * scalingPos; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 4.0 * scaling, 7.0 * scaling); + + rightPosition(0) += 0.1 * scalingPos; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 8.0 * scaling, 11.0 * scaling); + + rightPosition(0) += 0.1 * scalingPos; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 12.0 * scaling, 15.0 * scaling); + + rightPosition(0) += 0.05 * scalingPos; + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 16.0 * scaling, 19.0 * scaling); + + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 20.0 * scaling, 23.0 * scaling); + + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 24.0 * scaling, 27.0 * scaling); + + rightPosition(1) -= 0.01 * scalingPosY; + rightTransform.translation(rightPosition); + contactListMap["right_foot"].addContact(rightTransform, 28.0 * scaling, 29.0 * scaling); + + // contactListMap = BipedalLocomotion::Contacts::contactListMapFromJson("footsteps.json"); + phaseList.setLists(contactListMap); + + std::vector comKnots; + std::vector timeKnots; + + timeKnots.push_back(phaseList.cbegin()->beginTime); + Eigen::Vector3d com0; + com0 << 0, 0, 0.53; + comKnots.push_back(com0); + for (auto it = phaseList.begin(); it != phaseList.end(); std::advance(it, 1)) + { + if (it->activeContacts.size() == 2 && it != phaseList.begin() + && it != phaseList.lastPhase()) + { + timeKnots.emplace_back((it->endTime + it->beginTime) / 2); + + auto contactIt = it->activeContacts.cbegin(); + const Eigen::Vector3d p1 = contactIt->second->pose.translation(); + std::advance(contactIt, 1); + const Eigen::Vector3d p2 = contactIt->second->pose.translation(); + + Eigen::Vector3d desiredCoMPosition = (p1 + p2) / 2.0; + desiredCoMPosition(2) += com0(2); + + comKnots.emplace_back(desiredCoMPosition); + } + + else if (it->activeContacts.size() == 2 && it == phaseList.lastPhase()) + { + timeKnots.push_back(it->endTime); + auto contactIt = it->activeContacts.cbegin(); + const Eigen::Vector3d p1 = contactIt->second->pose.translation(); + std::advance(contactIt, 1); + const Eigen::Vector3d p2 = contactIt->second->pose.translation(); + + Eigen::Vector3d desiredCoMPosition = (p1 + p2) / 2.0; + desiredCoMPosition(2) += com0(2); + + comKnots.emplace_back(desiredCoMPosition); + } + } + + comSpline.setInitialConditions(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + comSpline.setFinalConditions(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + comSpline.setKnots(comKnots, timeKnots); + + Eigen::Vector3d velocity, acceleration; + Eigen::MatrixXd comTraj(3, 1500); + + + int tempInt = 1000; + for (int i = 0; i < tempInt / scaling; i++) + { + // TODO remove me + comSpline.evaluatePoint(i * dT, comTraj.col(i), velocity, acceleration); + } + + // i = 3 + // * * + // 1 2 3 4 5 + comTraj.rightCols(comTraj.cols() - tempInt).colwise() = comTraj.col(tempInt - 1); + + // initialize the dynamical system + auto system = std::make_shared(); + system->setState({comTraj.col(0), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); + + + double integratorStepTime = 0.01; + ForwardEuler integrator; + integrator.setIntegrationStep(integratorStepTime); + REQUIRE(integrator.setDynamicalSystem(system)); + + std::ofstream myFile; + myFile.open("data.txt"); + + // TODO reset contatcs + int controllerIndex = 0; + int index = 0; + + double elapsedTime = 0; + double currentTime = 0; + auto phaseIt = phaseList.getPresentPhase(currentTime); + + for (int i = 0; i < 1000; i++) + { + const auto& [com, dcom, angularMomentum] = system->getState(); + + if(controllerIndex == 0) + { + // update the phaseList this happens only when a new contact should be established + auto newPhaseIt = phaseList.getPresentPhase(currentTime); + if(newPhaseIt != phaseIt) + { + std::cout << "neww phase" << std::endl; + + // check if new contact is established + if (phaseIt->activeContacts.size() == 1 && newPhaseIt->activeContacts.size() == 2) + { + updateContactPhaseList(mpc.getOutput().nextPlannedContact, phaseList); + + // the iterators have been modified we have to compute the new one + phaseIt = phaseList.getPresentPhase(currentTime); + } + else + { + // the iterators did not change no need to get the present phase again + phaseIt = newPhaseIt; + } + + } + + REQUIRE(mpc.setState(com, dcom, angularMomentum)); + // REQUIRE(mpc.setReferenceTrajectory(comTraj.rightCols(comTraj.cols() - index))); + REQUIRE(mpc.setReferenceTrajectory(comTraj)); + REQUIRE(mpc.setContactPhaseList(phaseList)); + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + REQUIRE(mpc.advance()); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + elapsedTime = std::chrono::duration_cast(end - begin).count(); + index++; + currentTime += dT; + } + + if (i == 0) + { + for (const auto& [key, contact] : mpc.getOutput().contacts) + { + myFile << key << "_pos_x " << key << "_pos_y " << key << "_pos_z "; + for (int j = 0; j < contact.corners.size(); j++) + { + myFile << key << "_" << j << "_x" + << " " << key << "_" << j << "_y" + << " " << key << "_" << j << "_z "; + } + + myFile << key << "_next_pos_x " << key << "_next_pos_y " << key << "_next_pos_z "; + } + myFile << "com_x com_y com_z des_com_x des_com_y des_com_z ang_x ang_y ang_z " + "elapsed_time" + << std::endl; + } + + for (const auto& [key, contact] : mpc.getOutput().contacts) + { + myFile << contact.pose.translation().transpose() << " "; + for (const auto& corner : contact.corners) + { + myFile << corner.force.transpose() << " "; + } + + auto nextPlannedContact = mpc.getOutput().nextPlannedContact.find(key); + if (nextPlannedContact == mpc.getOutput().nextPlannedContact.end()) + { + myFile << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + else + { + myFile << nextPlannedContact->second.pose.translation().transpose() << " "; + } + } + myFile << com.transpose() << " " << comTraj.col(index).transpose() << " " + << angularMomentum.transpose() << " " << elapsedTime << std::endl; + + + Eigen::Vector3d externalInput = Eigen::Vector3d::Zero(); + if (i > 150 && i < 170) + { + externalInput(1) = 1; + } + + system->setControlInput({mpc.getOutput().contacts, externalInput}); + + REQUIRE(integrator.integrate(0, 0.01)); + + controllerIndex++; + if(controllerIndex == int(dT/integratorStepTime)) + controllerIndex = 0; + } + + myFile.close(); +}