Merge pull request #705 from ami-iit/centroidal_dynamics_wrench

Give the possibility to set an external wrench in the CentroidalDynamics

CHANGELOG.md

@@ -6,6 +6,7 @@ All notable changes to this project are documented in this file.
 
 ### Changed
 - Use `std::chorno::nanoseconds` in `clock` and `AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/702)
+- Give the possibility to set an external wrench in the `CentroidalDynamics` instead of a pure force (https://github.com/ami-iit/bipedal-locomotion-framework/pull/705)
 
 ### Fixed
 

src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h

@@ -16,8 +16,8 @@
 
 #include <BipedalLocomotion/Contacts/Contact.h>
 #include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
-#include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/GenericContainer/NamedTuple.h>
+#include <BipedalLocomotion/Math/Constants.h>
 
 #include <Eigen/Dense>
 
@@ -32,17 +32,17 @@ template <> struct traits<CentroidalDynamics>
 {
     using Contacts = std::map<std::string, //
                               BipedalLocomotion::Contacts::DiscreteGeometryContact>;
-    using ExternalForce = std::optional<Eigen::Vector3d>;
+    using ExternalWrench = std::optional<BipedalLocomotion::Math::Wrenchd>;
 
     using State = GenericContainer::named_tuple<BLF_NAMED_PARAM(com_pos, Eigen::Vector3d),
-                                      BLF_NAMED_PARAM(com_vel, Eigen::Vector3d),
-                                      BLF_NAMED_PARAM(angular_momentum, Eigen::Vector3d)>;
+                                                BLF_NAMED_PARAM(com_vel, Eigen::Vector3d),
+                                                BLF_NAMED_PARAM(angular_momentum, Eigen::Vector3d)>;
     using StateDerivative
         = GenericContainer::named_tuple<BLF_NAMED_PARAM(com_vel, Eigen::Vector3d),
-                              BLF_NAMED_PARAM(com_acc, Eigen::Vector3d),
-                              BLF_NAMED_PARAM(angular_momentum_rate, Eigen::Vector3d)>;
+                                        BLF_NAMED_PARAM(com_acc, Eigen::Vector3d),
+                                        BLF_NAMED_PARAM(angular_momentum_rate, Eigen::Vector3d)>;
     using Input = GenericContainer::named_tuple<BLF_NAMED_PARAM(contacts, Contacts),
-                                      BLF_NAMED_PARAM(external_force, ExternalForce)>;
+                                                BLF_NAMED_PARAM(external_wrench, ExternalWrench)>;
     using DynamicalSystem = CentroidalDynamics;
 };
 } // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal
@@ -52,6 +52,7 @@ namespace BipedalLocomotion
 namespace ContinuousDynamicalSystem
 {
 
+// clang-format off
 /**
  * CentroidalDynamics describes the centroidal dynamics of a multi-body system.
  * The centroidal momentum \f${}_{\bar{G}} h ^\top= \begin{bmatrix} {}_{\bar{G}} h^{p\top} & {}_{\bar{G}} h^{\omega\top} \end{bmatrix} \in \mathbb{R}^6\f$
@@ -91,13 +92,13 @@ namespace ContinuousDynamicalSystem
  * |`angular_momentum_rate` | `Eigen::Vector3d` | The centroidal angular momentum rate of change written respect the inertial frame. |
  *
  * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple
- * |       Name       |                                          Type                                           |                                                         Description                                                         |
- * |:----------------:|:---------------------------------------------------------------------------------------:|:---------------------------------------------------------------------------------------------------------------------------:|
- * |    `contacts`    | `std::unordered_map<std::string, BipedalLocomotion::Contacts::DiscreteGeometryContact>` |  List of contact where each force applied in the discere geometry contact is expressed with respect to the inertial frame   |
- * | `external_force` |                           `std::optional<Eigen::Vector3d>`                              |          Optional force applied to the robot center of mass. The coordinates are expressed in the inertial frame            |
+ * |        Name       |                                          Type                                           |                                                         Description                                                         |
+ * |:-----------------:|:---------------------------------------------------------------------------------------:|:---------------------------------------------------------------------------------------------------------------------------:|
+ * |     `contacts`    | `std::unordered_map<std::string, BipedalLocomotion::Contacts::DiscreteGeometryContact>` |  List of contact where each force applied in the discrete geometry contact is expressed with respect to the inertial frame  |
+ * | `external_wrench` |               `std::optional<BipedalLocomotion::Math::Wrenchd>`                         |         Optional wrench applied to the robot center of mass. The coordinates are expressed in the inertial frame            |
  */
 class CentroidalDynamics : public DynamicalSystem<CentroidalDynamics>
-
+// clang-format on
 {
     /** Gravity vector expressed in the inertial frame*/
     Eigen::Vector3d m_gravity{0, 0, -Math::StandardAccelerationOfGravitation};
@@ -107,7 +108,7 @@ class CentroidalDynamics : public DynamicalSystem<CentroidalDynamics>
     Input m_controlInput; /**< Input of the dynamical system */
 
 public:
-
+    // clang-format off
     /**
      * Initialize the CentroidalDynamics system.
      * @param handler pointer to the parameter handler.
@@ -119,6 +120,7 @@ class CentroidalDynamics : public DynamicalSystem<CentroidalDynamics>
      * @return true in case of success/false otherwise.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler);
+    // clang-format on
 
     /**
      * Computes the centroidal momentum dynamics. It return \f$f(x, u, t)\f$.

src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp

@@ -43,7 +43,8 @@ bool CentroidalDynamics::initialize(std::weak_ptr<const IParametersHandler> hand
     return true;
 }
 
-bool CentroidalDynamics::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative)
+bool CentroidalDynamics::dynamics(const std::chrono::nanoseconds& time,
+                                  StateDerivative& stateDerivative)
 {
     using namespace BipedalLocomotion::GenericContainer::literals;
 
@@ -52,13 +53,16 @@ bool CentroidalDynamics::dynamics(const std::chrono::nanoseconds& time, StateDer
 
     comVelocityOut = comVelocity;
     comAcceleration = m_gravity;
-    angularMomentumRate.setZero();
 
     const auto& contacts = m_controlInput.get_from_hash<"contacts"_h>();
-    const auto& externalDisturbance = m_controlInput.get_from_hash<"external_force"_h>();
+    const auto& externalDisturbance = m_controlInput.get_from_hash<"external_wrench"_h>();
     if (externalDisturbance)
     {
-        comAcceleration += externalDisturbance.value();
+        comAcceleration += externalDisturbance.value().force();
+        angularMomentumRate = externalDisturbance.value().torque();
+    } else
+    {
+        angularMomentumRate.setZero();
     }
 
     for (const auto& [key, contact] : contacts)

src/ReducedModelControllers/tests/CentroidalMPCTest.cpp

@@ -367,7 +367,8 @@ TEST_CASE("CentroidalMPC")
                                centroidalMPCData);
         }
 
-        system->setControlInput({mpc.getOutput().contacts, Eigen::Vector3d::Zero()});
+        system->setControlInput({mpc.getOutput().contacts, //
+                                 BipedalLocomotion::Math::Wrenchd::Zero()});
         REQUIRE(integrator.integrate(0s, integratorStepTime));
 
         controllerIndex++;