Implement CentroidalMPC

src/CMakeLists.txt

@@ -19,3 +19,4 @@ add_subdirectory(Math)
 add_subdirectory(TSID)
 add_subdirectory(Perception)
 add_subdirectory(IK)
+add_subdirectory(ReducedModelControllers)

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<Corner> corners;
+};
+
+}
+}
 
 #endif // BIPEDAL_LOCOMOTION_CONTACTS_CONTACT_H

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
     )

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 <memory>
+#include <tuple>
+#include <map>
+#include <vector>
+
+#include <BipedalLocomotion/Contacts/Contact.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h>
+#include <BipedalLocomotion/Math/Constants.h>
+
+#include <Eigen/Dense>
+
+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<std::string, BipedalLocomotion::Contacts::ContactWithCorners>))
+
+namespace BipedalLocomotion
+{
+namespace ContinuousDynamicalSystem
+{
+
+class CentroidalDynamics : public DynamicalSystem<CentroidalDynamics>
+
+{
+    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<ParametersHandler::IParametersHandler> 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

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 <BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+#include <BipedalLocomotion/Math/Constants.h>
+
+using namespace BipedalLocomotion;
+using namespace BipedalLocomotion::ContinuousDynamicalSystem;
+using namespace BipedalLocomotion::ParametersHandler;
+
+bool CentroidalDynamics::initialize(std::weak_ptr<IParametersHandler> handler)
+{
+    constexpr auto logPrefix = "[CentroidalDynamics::initialize]";
+
+    auto ptr = handler.lock();
+    if (ptr == nullptr)
+    {
+        log()->error("{} The parameter handler is expired. Please call the function passing a "
+                     "pointer pointing an already allocated memory.",
+                     logPrefix);
+        return false;
+    }
+
+    if (!ptr->getParameter("gravity", m_gravity))
+    {
+        log()->info("{} The gravity vector  not found. The default one will be "
+                    "used {}.",
+                    logPrefix,
+                    m_gravity.transpose());
+    }
+
+    if (!ptr->getParameter("mass", m_mass))
+    {
+        log()->info("{} The mass is not found. The default one will be "
+                    "used {}.",
+                    logPrefix,
+                    m_mass);
+    }
+
+    return true;
+}
+
+bool CentroidalDynamics::dynamics(const double& time, StateDerivative& stateDerivative)
+{
+    const auto& [comPosition, comVelocity, angularMomentum] = m_state;
+    auto& [comVelocityOut, comAcceleration, angularMomentumRate] = stateDerivative;
+
+    comVelocityOut = comVelocity;
+    comAcceleration = m_gravity;
+    angularMomentumRate.setZero();
+
+    const auto& contacts = std::get<0>(m_controlInput);
+
+    for (const auto& [key, contact] : contacts)
+    {
+        for (const auto& corner : contact.corners)
+        {
+            comAcceleration.noalias() += 1 / m_mass * contact.pose.asSO3().act(corner.force);
+            angularMomentumRate.noalias() += (contact.pose.act(corner.position) - comPosition)
+                                                 .cross(contact.pose.asSO3().act(corner.force));
+        }
+    }
+
+    return true;
+}
+
+bool CentroidalDynamics::setState(const State& state)
+{
+    m_state = state;
+    return true;
+}
+
+const CentroidalDynamics::State& CentroidalDynamics::getState() const
+{
+    return m_state;
+}
+
+bool CentroidalDynamics::setControlInput(const Input& controlInput)
+{
+    m_controlInput = controlInput;
+    return true;
+}

src/IK/include/BipedalLocomotion/IK/SE3Task.h

@@ -65,6 +65,9 @@ class SE3Task : public System::LinearTask
     std::shared_ptr<iDynTree::KinDynComputations> 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.

src/IK/src/SE3Task.cpp

@@ -123,9 +123,8 @@ bool SE3Task::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> 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<ParametersHandler::IParametersHandler> 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<ParametersHandler::IParametersHandler> 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);
+}

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<ParametersHandler::IParametersHandler> handler);
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler);
 
     /**
      * Get the matrix A.

src/Math/src/LinearizedFrictionCone.cpp

@@ -12,7 +12,7 @@
 
 using namespace BipedalLocomotion::Math;
 
-bool LinearizedFrictionCone::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler)
+bool LinearizedFrictionCone::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)
 {
     constexpr auto errorPrefix = "[LinearizedFrictionCone::initialize]";
 

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()