Clamp built-in predicted CoM height to positive vertical velocity

Author: mickaelbegon

README.md

@@ -1335,7 +1335,7 @@ Here a list of objective function with its type (Lagrange and/or Mayer) in alpha
 - **MINIMIZE_MARKERS_VELOCITY or MINIMIZE_MARKERS_ACCELERATION** (Lagrange and Mayer) — Minimizes the marker velocities or accelerations toward zero (or a target).
 - **MINIMIZE_MARKERS** (Lagrange and Mayer) — Minimizes the position of the markers toward zero (or a target). The extra parameter `axis_to_track: Axis = (Axis.X, Axis.Y, Axis.Z)` can be sent to specify the axes along which the markers should be minimized.
 - **MINIMIZE_MUSCLES_CONTROL** (Lagrange) —  Minimizes the muscles' controls (part of the control variables) toward zero (or a target).
-- **MINIMIZE_PREDICTED_COM_HEIGHT** (Mayer)  — Minimizes the maximal height of the center of mass, predicted from the parabolic equation, assuming vertical axis is Z (2): CoM_dot[2]**2 / (2 * -g) + CoM[2]. To maximize a jump, one can use this function at the end of the push-off phase and declare a weight of -1.
+- **MINIMIZE_PREDICTED_COM_HEIGHT** (Mayer)  — Minimizes the maximal height of the center of mass, predicted from the parabolic equation, assuming vertical axis is Z (2): max(CoM_dot[2], 0)**2 / (2 * -g) + CoM[2]. To maximize a jump, one can use this function at the end of the push-off phase and declare a weight of -1.
 - **MINIMIZE_SOFT_CONTACT_FORCES** (Lagrange) — Minimizes the external forces induced by soft contacts (or a target).
 - **MINIMIZE_STATE_DERIVATIVE** (Lagrange) — Minimizes the difference between a state at a node and the same state at the next node, i.e., minimizes the generalized state derivative.
 - **MINIMIZE_STATE** (Lagrange and Mayer) — Minimizes the state variable towards zero (or a target).

bioptim/limits/penalty.py

@@ -606,7 +606,7 @@ def minimize_qddot(penalty: PenaltyOption, controller: PenaltyController):
         def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyController):
             """
             Minimize the prediction of the center of mass maximal height from the parabolic equation,
-            assuming vertical axis is Z (2): CoM_dot[2]**2 / (2 * -g) + com[2]
+            assuming vertical axis is Z (2): max(CoM_dot[2], 0)**2 / (2 * -g) + com[2]
             By default this function is not quadratic, meaning that it minimizes towards infinity.
 
             Parameters
@@ -622,7 +622,8 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle
             com_dot = controller.model.center_of_mass_velocity()(
                 controller.q, controller.qdot, controller.parameters.cx
             )
-            com_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2]
+            com_dot_z = if_else(com_dot[2] > 0, com_dot[2], 0)
+            com_height = (com_dot_z * com_dot_z) / (2 * -g) + com[2]
             return com_height
 
         @staticmethod

tests/shard4/test_penalty.py

@@ -858,7 +858,14 @@ def test_penalty_minimize_predicted_com_height(value, phase_dynamics):
     penalty = Objective(penalty_type)
     res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d)
 
-    expected = np.array(0.0501274 if value == 0.1 else -3.72579)
+    q = x[0][: ocp.nlp[0].model.nb_q]
+    qdot = x[0][ocp.nlp[0].model.nb_q :]
+    com = ocp.nlp[0].model.center_of_mass()(q, DM())
+    com_dot = ocp.nlp[0].model.center_of_mass_velocity()(q, qdot, DM())
+    gravity_z = ocp.nlp[0].model.gravity()(DM())[2]
+    positive_vertical_velocity = max(float(com_dot[2]), 0.0)
+    expected = float(com[2]) + positive_vertical_velocity**2 / (2 * -float(gravity_z))
+
     npt.assert_almost_equal(res, expected)