Implement ICentroidalMPC interface and add the StableCentroidalMPC class

src/ReducedModelControllers/CMakeLists.txt

@@ -8,10 +8,11 @@ if(FRAMEWORK_COMPILE_ReducedModelControllers)
 
   add_bipedal_locomotion_library(
     NAME                   ReducedModelControllers
-    PUBLIC_HEADERS         ${H_PREFIX}/CentroidalMPC.h
-    SOURCES                src/CentroidalMPC.cpp
+    PUBLIC_HEADERS         ${H_PREFIX}/ICentroidalMPC.h ${H_PREFIX}/CentroidalMPC.h ${H_PREFIX}/StableCentroidalMPC.h
+    SOURCES                src/CentroidalMPC.cpp src/StableCentroidalMPC.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts
     PRIVATE_LINK_LIBRARIES casadi BipedalLocomotion::Math BipedalLocomotion::TextLogging BipedalLocomotion::CasadiConversions
     SUBDIRECTORIES         tests)
 
 endif()
+

src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h

@@ -18,25 +18,13 @@
 #include <BipedalLocomotion/Contacts/ContactPhaseList.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/System/Source.h>
+#include <BipedalLocomotion/ReducedModelControllers/ICentroidalMPC.h>
 
 namespace BipedalLocomotion
 {
 namespace ReducedModelControllers
 {
 
-/**
- * CentroidalMPCOutput contains the output of the CentroidalMPC class
- */
-struct CentroidalMPCOutput
-{
-    /** Map containing the contact name and the associated contact. */
-    std::map<std::string, Contacts::DiscreteGeometryContact> contacts;
-    Contacts::ContactPhaseList contactPhaseList; /**< Contact phase list generated by the
-                                                    CentroidalMPC. */
-    std::vector<Eigen::Vector3d> comTrajectory; /**< Desired CoM trajectory generated by the
-                                                    CentroidalMPC. */
-};
-
 /**
  * CentroidalMPC implements a Non-Linear Model Predictive Controller for humanoid robot locomotion
  * with online step adjustment capabilities. The proposed controller considers the Centroidal
@@ -51,7 +39,7 @@ struct CentroidalMPCOutput
  * Locomotion with Step Adjustment," 2022 International Conference on Robotics and Automation
  * (ICRA), Philadelphia, PA, USA, 2022, pp. 10412-10419, doi: 10.1109/ICRA46639.2022.9811670.
  */
-class CentroidalMPC : public System::Source<CentroidalMPCOutput>
+class CentroidalMPC : public  ICentroidalMPC
 {
 public:
     /**
@@ -96,7 +84,7 @@ class CentroidalMPC : public System::Source<CentroidalMPCOutput>
      * |        `corner_<j>`        | `vector<double>` |                 Position of the corner expressed in the foot frame. I must be from 0 to number_of_corners - 1.                   |    Yes    |
      * @return true in case of success/false otherwise.
      */
-    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) final;
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
     // clang-format on
 
     /**
@@ -105,7 +93,7 @@ class CentroidalMPC : public System::Source<CentroidalMPCOutput>
      * @return True in case of success, false otherwise.
      * @note This function needs to be called before advance.
      */
-    bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList);
+    bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) override;
 
     /**
      * Set the state of the centroidal dynamics.
@@ -119,7 +107,7 @@ class CentroidalMPC : public System::Source<CentroidalMPCOutput>
      */
     bool setState(Eigen::Ref<const Eigen::Vector3d> com,
                   Eigen::Ref<const Eigen::Vector3d> dcom,
-                  Eigen::Ref<const Eigen::Vector3d> angularMomentum);
+                  Eigen::Ref<const Eigen::Vector3d> angularMomentum) override;
 
     /**
      * Set the state of the centroidal dynamics.
@@ -135,7 +123,7 @@ class CentroidalMPC : public System::Source<CentroidalMPCOutput>
     bool setState(Eigen::Ref<const Eigen::Vector3d> com,
                   Eigen::Ref<const Eigen::Vector3d> dcom,
                   Eigen::Ref<const Eigen::Vector3d> angularMomentum,
-                  const Math::Wrenchd& externalWrench);
+                  const Math::Wrenchd& externalWrench) override;
 
     /**
      * Set the reference trajectories for the CoM and the centroidal angular momentum.
@@ -150,25 +138,25 @@ class CentroidalMPC : public System::Source<CentroidalMPCOutput>
      * controller sampling period
      */
     bool setReferenceTrajectory(const std::vector<Eigen::Vector3d>& com,
-                                const std::vector<Eigen::Vector3d>& angularMomentum);
+                                const std::vector<Eigen::Vector3d>& angularMomentum) override;
 
     /**
      * Get the output of the controller
      * @return a const reference of the output of the controller.
      */
-    const CentroidalMPCOutput& getOutput() const final;
+    const CentroidalMPCOutput& getOutput() const override;
 
     /**
      * Determines the validity of the object retrieved with getOutput()
      * @return True if the object is valid, false otherwise.
      */
-    bool isOutputValid() const final;
+    bool isOutputValid() const override;
 
     /**
      * Perform one control cycle.
      * @return True if the advance is successfull.
      */
-    bool advance() final;
+    bool advance() override;
 
 private:
     /**
@@ -181,4 +169,5 @@ class CentroidalMPC : public System::Source<CentroidalMPCOutput>
 } // namespace ReducedModelControllers
 } // namespace BipedalLocomotion
 
+
 #endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_CENTROIDAL_MPC_H

src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/ICentroidalMPC.h

@@ -0,0 +1,126 @@
+/**
+ * @file ICentroidalMPC.h
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ICENTROIDAL_MPC_H
+#define BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ICENTROIDAL_MPC_H
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include <casadi/casadi.hpp>
+
+#include <BipedalLocomotion/Contacts/Contact.h>
+#include <BipedalLocomotion/Contacts/ContactPhaseList.h>
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/System/Source.h>
+
+#define STR_(x) #x
+#define STR(x) STR_(x)
+
+inline bool casadiVersionIsAtLeast360()
+{
+    std::string str;
+    std::stringstream ss(STR(casadi_VERSION));
+
+    // Use while loop to check the getline() function condition.
+    int index = 0;
+    while (getline(ss, str, '.'))
+    {
+        if (index == 0 && stoi(str) < 3)
+        {
+            return false;
+        }
+        if (index == 1 && stoi(str) < 6)
+        {
+            return false;
+        }
+        index++;
+    }
+
+    return true;
+}
+
+inline double chronoToSeconds(const std::chrono::nanoseconds& d)
+{
+    return std::chrono::duration<double>(d).count();
+}
+
+inline std::vector<std::string> extractVariablesName(const std::vector<casadi::MX>& variables)
+{
+    std::vector<std::string> variablesName;
+    variablesName.reserve(variables.size());
+    for (const auto& variable : variables)
+    {
+        variablesName.push_back(variable.name());
+    }
+
+    return variablesName;
+}
+
+template <class T> inline auto extractFutureValuesFromState(T& variable)
+{
+    using Sl = casadi::Slice;
+    return variable(Sl(), Sl(1, variable.columns()));
+}
+
+template <class T> inline auto extractFutureValuesFromState(const T& variable)
+{
+    using Sl = casadi::Slice;
+    return variable(Sl(), Sl(1, variable.columns()));
+}
+
+
+namespace BipedalLocomotion
+{
+namespace ReducedModelControllers
+{
+
+struct CentroidalMPCOutput
+{
+    std::map<std::string, Contacts::DiscreteGeometryContact> contacts;
+    Contacts::ContactPhaseList contactPhaseList; /**< Contact phase list generated by the
+                                                    CentroidalMPC. */
+    std::vector<Eigen::Vector3d> comTrajectory; /**< Desired CoM trajectory generated by the
+                                                    CentroidalMPC. */
+};
+
+class ICentroidalMPC : public BipedalLocomotion::System::Source<CentroidalMPCOutput>
+{
+public:
+
+    virtual bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) = 0;
+
+    virtual bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) = 0;
+
+    virtual bool setState(Eigen::Ref<const Eigen::Vector3d> com,
+                          Eigen::Ref<const Eigen::Vector3d> dcom,
+                          Eigen::Ref<const Eigen::Vector3d> angularMomentum)
+        = 0;
+
+    virtual bool setState(Eigen::Ref<const Eigen::Vector3d> com,
+                          Eigen::Ref<const Eigen::Vector3d> dcom,
+                          Eigen::Ref<const Eigen::Vector3d> angularMomentum,
+                          const Math::Wrenchd& externalWrench)
+        = 0;
+
+    virtual bool setReferenceTrajectory(const std::vector<Eigen::Vector3d>& com,
+                                        const std::vector<Eigen::Vector3d>& angularMomentum)
+        = 0;
+
+    virtual const CentroidalMPCOutput& getOutput() const = 0;
+
+    virtual bool isOutputValid() const = 0;
+
+    virtual bool advance() = 0;
+};
+
+} // namespace ReducedModelControllers
+} // namespace BipedalLocomotion
+#endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ICENTROIDAL_MPC_H
+

src/ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/StableCentroidalMPC.h

@@ -0,0 +1,67 @@
+/**
+ * @file CentroidalMPC.h
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ADAPTIVE_CENTROIDAL_MPC_H
+#define BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ADAPTIVE_CENTROIDAL_MPC_H
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include <BipedalLocomotion/Contacts/Contact.h>
+#include <BipedalLocomotion/Contacts/ContactPhaseList.h>
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/System/Source.h>
+#include <BipedalLocomotion/ReducedModelControllers/ICentroidalMPC.h>
+
+namespace BipedalLocomotion
+{
+namespace ReducedModelControllers
+{
+
+class StableCentroidalMPC : public  ICentroidalMPC
+{
+public:
+
+    StableCentroidalMPC();
+
+    ~StableCentroidalMPC();
+
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
+
+    bool setContactPhaseList(const Contacts::ContactPhaseList& contactPhaseList) override;
+
+    bool setState(Eigen::Ref<const Eigen::Vector3d> com,
+                  Eigen::Ref<const Eigen::Vector3d> dcom,
+                  Eigen::Ref<const Eigen::Vector3d> angularMomentum) override;
+
+    bool setState(Eigen::Ref<const Eigen::Vector3d> com,
+                  Eigen::Ref<const Eigen::Vector3d> dcom,
+                  Eigen::Ref<const Eigen::Vector3d> angularMomentum,
+                  const Math::Wrenchd& externalWrench) override;
+
+    bool setReferenceTrajectory(const std::vector<Eigen::Vector3d>& com,
+                                const std::vector<Eigen::Vector3d>& angularMomentum) override;
+
+    const CentroidalMPCOutput& getOutput() const override;
+
+    bool isOutputValid() const override;
+
+    bool advance() override;
+
+private:
+
+    struct Impl;
+
+    std::unique_ptr<Impl> m_pimpl; /**< Pointer to private implementation */
+};
+} // namespace ReducedModelControllers
+} // namespace BipedalLocomotion
+
+
+#endif // BIPEDAL_LOCOMOTION_REDUCE_MODEL_CONTROLLERS_ADAPTIVE_CENTROIDAL_MPC_H

src/ReducedModelControllers/src/CentroidalMPC.cpp

@@ -20,61 +20,6 @@
 using namespace BipedalLocomotion::ReducedModelControllers;
 using namespace BipedalLocomotion::Contacts;
 
-#define STR_(x) #x
-#define STR(x) STR_(x)
-
-bool casadiVersionIsAtLeast360()
-{
-    std::string str;
-    std::stringstream ss(STR(casadi_VERSION));
-
-    // Use while loop to check the getline() function condition.
-    int index = 0;
-    while (getline(ss, str, '.'))
-    {
-        if (index == 0 && stoi(str) < 3)
-        {
-            return false;
-        }
-        if (index == 1 && stoi(str) < 6)
-        {
-            return false;
-        }
-        index++;
-    }
-
-    return true;
-}
-
-inline double chronoToSeconds(const std::chrono::nanoseconds& d)
-{
-    return std::chrono::duration<double>(d).count();
-}
-
-std::vector<std::string> extractVariablesName(const std::vector<casadi::MX>& variables)
-{
-    std::vector<std::string> variablesName;
-    variablesName.reserve(variables.size());
-    for (const auto& variable : variables)
-    {
-        variablesName.push_back(variable.name());
-    }
-
-    return variablesName;
-}
-
-template <class T> inline auto extractFutureValuesFromState(T& variable)
-{
-    using Sl = casadi::Slice;
-    return variable(Sl(), Sl(1, variable.columns()));
-}
-
-template <class T> inline auto extractFutureValuesFromState(const T& variable)
-{
-    using Sl = casadi::Slice;
-    return variable(Sl(), Sl(1, variable.columns()));
-}
-
 struct CentroidalMPC::Impl
 {
     casadi::Opti opti; /**< CasADi opti stack */
@@ -1529,4 +1474,4 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contac
     m_pimpl->output.contactPhaseList = contactPhaseList;
 
     return true;
-}
+}