Feature/centroidal mpc

src/CMakeLists.txt

@@ -20,3 +20,4 @@ add_subdirectory(TSID)
 add_subdirectory(Perception)
 add_subdirectory(IK)
 add_subdirectory(SimplifiedModelControllers)
+add_subdirectory(ReducedModelControllers)

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.

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/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;

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>, Eigen::Vector3d))
+
+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);
+    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;
+}

src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h

@@ -92,6 +92,8 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
      */
     void setContactList(const Contacts::ContactList& contactList);
 
+    void setContactListWithotResettingTheInternalTime(const Contacts::ContactList& contactList);
+
     /**
      * @brief Get the object.
      * @return a const reference of the requested object.

src/Planners/src/SwingFootPlanner.cpp

@@ -123,6 +123,19 @@ bool SwingFootPlanner::initialize(std::weak_ptr<const ParametersHandler::IParame
     return true;
 }
 
+void SwingFootPlanner::setContactListWithotResettingTheInternalTime(const ContactList& contactList)
+{
+    m_contactList = contactList;
+
+    // set the first contact
+    m_currentContactPtr = m_contactList.getPresentContact(m_currentTrajectoryTime);
+    m_state.transform = m_currentContactPtr->pose;
+    // m_state.mixedVelocity.setZero();
+    // m_state.mixedAcceleration.setZero();
+    // m_state.isInContact = true;
+}
+
+
 void SwingFootPlanner::setContactList(const ContactList& contactList)
 {
     // reset the time

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