From 8bf22fa9b9ca01e843eeea519ffeb0426bc3bfe0 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 24 Apr 2026 10:45:51 -0400 Subject: [PATCH] Clamp built-in predicted CoM height to positive vertical velocity --- README.md | 2 +- bioptim/limits/penalty.py | 5 +++-- tests/shard4/test_penalty.py | 9 ++++++++- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index d8d4e207c..e4d1c4b1d 100644 --- a/README.md +++ b/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). diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index a1a8b7114..85ef2f5e0 100644 --- a/bioptim/limits/penalty.py +++ b/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 diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index acf759476..8992805e6 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -857,7 +857,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)