Expose RBDL zero moment point

Author: mickaelbegon

binding/python3/wrapper/biorbd_model.py

@@ -146,6 +146,44 @@ def center_of_mass(self, q: BiorbdArray | None = None) -> BiorbdArray:
         update_kinematics = False
         return to_biorbd_array_output(updated_model.CoM(dummy_q, update_kinematics))
 
+    def zero_moment_point(
+        self,
+        q: BiorbdArray,
+        qdot: BiorbdArray,
+        qddot: BiorbdArray,
+        normal: BiorbdArray = (0, 0, 1),
+        point: BiorbdArray = (0, 0, 0),
+    ) -> BiorbdArray:
+        """
+        Get the zero moment point projected on a contact surface.
+
+        Parameters
+        ----------
+        q: BiorbdArray
+            Generalized coordinates
+        qdot: BiorbdArray
+            Generalized velocities
+        qddot: BiorbdArray
+            Generalized accelerations
+        normal: BiorbdArray
+            Normal of the contact surface
+        point: BiorbdArray
+            A point on the contact surface
+
+        Returns
+        -------
+        The zero moment point in the base frame
+        """
+        return to_biorbd_array_output(
+            self.internal.CalcZeroMomentPoint(
+                to_biorbd_array_input(q),
+                to_biorbd_array_input(qdot),
+                to_biorbd_array_input(qddot),
+                to_biorbd_array_input(normal),
+                to_biorbd_array_input(point),
+            )
+        )
+
     @property
     def segments(self) -> SegmentsList:
         """

include/RigidBody/Joints.h

@@ -716,6 +716,38 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model
       utils::Vector3d* changeOfAngularMomentum = NULL,
       bool updateKin = true);
 
+  ///
+  /// \brief Compute the zero moment point on a contact surface.
+  /// \param Q The current joint positions
+  /// \param Qdot The current joint velocities
+  /// \param Qddot The current joint accelerations
+  /// \param updateKin If the kinematics of the model should be computed
+  /// \return The Zero Moment Point projected on the horizontal surface
+  ///
+  utils::Vector3d CalcZeroMomentPoint(
+      const rigidbody::GeneralizedCoordinates& Q,
+      const rigidbody::GeneralizedVelocity& Qdot,
+      const rigidbody::GeneralizedAcceleration& Qddot,
+      bool updateKin = true);
+
+  ///
+  /// \brief Compute the zero moment point on a contact surface.
+  /// \param Q The current joint positions
+  /// \param Qdot The current joint velocities
+  /// \param Qddot The current joint accelerations
+  /// \param normal The normal of the contact surface
+  /// \param point A point on the contact surface
+  /// \param updateKin If the kinematics of the model should be computed
+  /// \return The Zero Moment Point projected on the contact surface
+  ///
+  utils::Vector3d CalcZeroMomentPoint(
+      const rigidbody::GeneralizedCoordinates& Q,
+      const rigidbody::GeneralizedVelocity& Qdot,
+      const rigidbody::GeneralizedAcceleration& Qddot,
+      const utils::Vector3d& normal,
+      const utils::Vector3d& point,
+      bool updateKin = true);
+
   ///
   /// \brief Return the position of the center of mass of each segment
   /// \param Q The generalized coordinates

src/RigidBody/Joints.cpp

@@ -1035,6 +1035,44 @@ void rigidbody::Joints::CalcCenterOfMass(
       false);
 }
 
+utils::Vector3d rigidbody::Joints::CalcZeroMomentPoint(
+    const rigidbody::GeneralizedCoordinates &Q,
+    const rigidbody::GeneralizedVelocity &Qdot,
+    const rigidbody::GeneralizedAcceleration &Qddot,
+    bool updateKin) {
+  return CalcZeroMomentPoint(
+      Q,
+      Qdot,
+      Qddot,
+      utils::Vector3d(0., 0., 1.),
+      utils::Vector3d(0., 0., 0.),
+      updateKin);
+}
+
+utils::Vector3d rigidbody::Joints::CalcZeroMomentPoint(
+    const rigidbody::GeneralizedCoordinates &Q,
+    const rigidbody::GeneralizedVelocity &Qdot,
+    const rigidbody::GeneralizedAcceleration &Qddot,
+    const utils::Vector3d &normal,
+    const utils::Vector3d &point,
+    bool updateKin) {
+#ifdef BIORBD_USE_CASADI_MATH
+  rigidbody::Joints
+#else
+  rigidbody::Joints &
+#endif
+      updatedModel = this->UpdateKinematicsCustom(
+          updateKin ? &Q : nullptr,
+          updateKin ? &Qdot : nullptr,
+          updateKin ? &Qddot : nullptr);
+
+  utils::Vector3d zmp;
+  RigidBodyDynamics::Utils::CalcZeroMomentPoint(
+      updatedModel, Q, Qdot, Qddot, &zmp, normal, point, false);
+
+  return zmp;
+}
+
 std::vector<rigidbody::NodeSegment> rigidbody::Joints::CoMbySegment(
     const rigidbody::GeneralizedCoordinates &Q,
     bool updateKin) {

test/binding/python3/test_binder_python_rigidbody.py

@@ -243,6 +243,42 @@ def test_com(brbd):
     np.testing.assert_almost_equal(com_ddot.squeeze(), expected_com_ddot)
 
 
+@pytest.mark.parametrize("brbd", brbd_to_test)
+def test_zero_moment_point(brbd):
+    m = brbd.Model("../../models/pyomecaman.bioMod")
+
+    q = np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3])
+    q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
+    q_ddot = np.array([10, 10, 10, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30])
+    normal = np.array([0, 0, 1])
+    point = np.array([0, 0, 0.1])
+
+    if brbd.backend == brbd.CASADI:
+        from casadi import MX
+
+        q_sym = MX.sym("q", m.nbQ(), 1)
+        q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1)
+        q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1)
+
+        zero_moment_point_func = brbd.to_casadi_func(
+            "Compute_Zero_Moment_Point",
+            m.CalcZeroMomentPoint,
+            q_sym,
+            q_dot_sym,
+            q_ddot_sym,
+            normal,
+            point,
+        )
+        zero_moment_point = np.array(zero_moment_point_func(q, q_dot, q_ddot))
+
+    elif brbd.backend == brbd.EIGEN3:
+        zero_moment_point = m.CalcZeroMomentPoint(q, q_dot, q_ddot, normal, point).to_array()
+    else:
+        raise NotImplementedError("Backend not implemented in test")
+
+    np.testing.assert_almost_equal(zero_moment_point.squeeze()[2], point[2])
+
+
 @pytest.mark.parametrize("brbd", brbd_to_test)
 def test_set_vector3d(brbd):
     m = brbd.Model("../../models/pyomecaman.bioMod")

test/binding/python3/tests_wrapper/test_wrapper_dynamics.py

@@ -56,6 +56,18 @@ def test_wrapper_dynamics(brbd):
         ],
         decimal=6,
     )
+    np.testing.assert_almost_equal(
+        evaluate(
+            brbd,
+            model.zero_moment_point,
+            q=q,
+            qdot=qdot,
+            qddot=qddot,
+            normal=np.array([0, 0, 1]),
+            point=np.array([0, 0, 0.1]),
+        )[2],
+        0.1,
+    )
 
 
 if __name__ == "__main__":

test/test_rigidbody.cpp

@@ -6,6 +6,7 @@
 
 #include <rbdl/Dynamics.h>
 #include <rbdl/rbdl_math.h>
+#include <rbdl/rbdl_utils.h>
 
 #include "BiorbdModel.h"
 #include "RigidBody/ExternalForceSet.h"
@@ -1467,6 +1468,34 @@ TEST(CoM, kinematics) {
   }
 }
 
+TEST(CoM, zeroMomentPoint) {
+  Model model(modelPathForGeneralTesting);
+  Model expectedModel(modelPathForGeneralTesting);
+  rigidbody::GeneralizedCoordinates Q(model);
+  rigidbody::GeneralizedVelocity Qdot(model);
+  rigidbody::GeneralizedAcceleration Qddot(model);
+  for (size_t i = 0; i < model.nbQ(); ++i) {
+    Q(i, 0) = QtestPyomecaman[i];
+    Qdot(i, 0) = QtestPyomecaman[i] * 10;
+    Qddot(i, 0) = QtestPyomecaman[i] * 100;
+  }
+
+  utils::Vector3d normal(0, 0, 1);
+  utils::Vector3d point(0, 0, 0);
+  utils::Vector3d zeroMomentPoint(
+      model.CalcZeroMomentPoint(Q, Qdot, Qddot, normal, point));
+  utils::Vector3d expectedZeroMomentPoint;
+  RigidBodyDynamics::Utils::CalcZeroMomentPoint(
+      expectedModel, Q, Qdot, Qddot, &expectedZeroMomentPoint, normal, point);
+
+  for (size_t i = 0; i < 3; ++i) {
+    EXPECT_NEAR(
+        static_cast<double>(zeroMomentPoint(i, 0)),
+        static_cast<double>(expectedZeroMomentPoint(i, 0)),
+        requiredPrecision);
+  }
+}
+
 TEST(Segment, copy) {
   Model model(modelPathForGeneralTesting);
   rigidbody::SegmentCharacteristics characteristics(