From a90326dfaffd4bb54b5996c80dedc9c2395f5d20 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Wed, 13 Jan 2021 14:39:03 -0500 Subject: [PATCH 01/12] Added objectives position and velocity of the CoM at each node --- bioptim/limits/objective_functions.py | 4 +++ bioptim/limits/penalty.py | 49 +++++++++++++++++++++++++++ 2 files changed, 53 insertions(+) diff --git a/bioptim/limits/objective_functions.py b/bioptim/limits/objective_functions.py index cdee30675..5316ddc30 100644 --- a/bioptim/limits/objective_functions.py +++ b/bioptim/limits/objective_functions.py @@ -334,6 +334,8 @@ class Lagrange(Enum): TRACK_ALL_CONTROLS = (PenaltyType.TRACK_ALL_CONTROLS,) MINIMIZE_CONTACT_FORCES = (PenaltyType.MINIMIZE_CONTACT_FORCES,) TRACK_CONTACT_FORCES = (PenaltyType.TRACK_CONTACT_FORCES,) + MINIMIZE_COM_HEIGHT = (PenaltyType.MINIMIZE_COM_HEIGHT,) + MINIMIZE_COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) ALIGN_SEGMENT_WITH_CUSTOM_RT = (PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT,) ALIGN_MARKER_WITH_SEGMENT_AXIS = (PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS,) CUSTOM = (PenaltyType.CUSTOM,) @@ -368,6 +370,8 @@ class Mayer(Enum): MINIMIZE_CONTACT_FORCES = (PenaltyType.MINIMIZE_CONTACT_FORCES,) TRACK_CONTACT_FORCES = (PenaltyType.TRACK_CONTACT_FORCES,) MINIMIZE_PREDICTED_COM_HEIGHT = (PenaltyType.MINIMIZE_PREDICTED_COM_HEIGHT,) + MINIMIZE_COM_HEIGHT = (PenaltyType.MINIMIZE_COM_HEIGHT,) + MINIMIZE_COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) ALIGN_SEGMENT_WITH_CUSTOM_RT = (PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT,) ALIGN_MARKER_WITH_SEGMENT_AXIS = (PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS,) CUSTOM = (PenaltyType.CUSTOM,) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 787590b3f..1aeebbb67 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -346,6 +346,53 @@ def minimize_predicted_com_height(penalty, ocp, nlp, t, x, u, p): CoM_height = (CoM_dot[2] * CoM_dot[2]) / (2 * -g) + CoM[2] penalty.type.get_type().add_to_penalty(ocp, nlp, CoM_height, penalty) + @staticmethod + def minimize_com_height(penalty, ocp, nlp, t, x, u, p): + """ + Adds the objective that the height of the center of mass of the model should be minimized. + The height is assumed to be the third axis. + """ + + target = None + if penalty.target is not None: + target = PenaltyFunctionAbstract._check_and_fill_tracking_data_size(penalty.target, (1, len(x))) + + PenaltyFunctionAbstract._add_to_casadi_func(nlp, "biorbd_CoM", nlp.model.CoM, nlp.q) + for i, v in enumerate(x): + q = nlp.mapping["q"].expand.map(v[: nlp.shape["q"]]) + CoM = nlp.casadi_func["biorbd_CoM"](q) + CoM_height = CoM[2] + penalty.sliced_target = target[:, i] if target is not None else None + penalty.type.get_type().add_to_penalty(ocp, nlp, CoM_height, penalty) + + @staticmethod + def minimize_com_velocity(penalty, ocp, nlp, t, x, u, p, axis=None): + """ + Adds the objective that the velocity of the center of mass of the model should be minimized. + If no axis is specified, the squared-norm of the CoM's velocity is mimimized. + Otherwise, the projection of the CoM's velocity on the specified axis is minimized. + """ + + target = None + if penalty.target is not None: + target = PenaltyFunctionAbstract._check_and_fill_tracking_data_size(penalty.target, (1, len(x))) + + PenaltyFunctionAbstract._add_to_casadi_func(nlp, "biorbd_CoMdot", nlp.model.CoMdot, nlp.q, nlp.q_dot) + for i, v in enumerate(x): + q = nlp.mapping["q"].expand.map(v[: nlp.shape["q"]]) + q_dot = nlp.mapping["q_dot"].expand.map(v[nlp.shape["q"] :]) + CoM_dot = nlp.casadi_func["biorbd_CoMdot"](q, q_dot) + + if axis == None: + CoM_dot_proj = CoM_dot[0] ** 2 + CoM_dot[1] ** 2 + CoM_dot[2] ** 2 + elif not isinstance(axis, Axe): + raise RuntimeError("axis must be a bioptim.Axe") + else: + CoM_dot_proj = CoM_dot[axis] + + penalty.sliced_target = target[:, i] if target is not None else None + penalty.type.get_type().add_to_penalty(ocp, nlp, CoM_dot_proj, penalty) + @staticmethod def minimize_contact_forces(penalty, ocp, nlp, t, x, u, p): """ @@ -745,6 +792,8 @@ class PenaltyType(Enum): MINIMIZE_CONTACT_FORCES = PenaltyFunctionAbstract.Functions.minimize_contact_forces TRACK_CONTACT_FORCES = MINIMIZE_CONTACT_FORCES MINIMIZE_PREDICTED_COM_HEIGHT = PenaltyFunctionAbstract.Functions.minimize_predicted_com_height + MINIMIZE_COM_HEIGHT = PenaltyFunctionAbstract.Functions.minimize_com_height + MINIMIZE_COM_VELOCITY = PenaltyFunctionAbstract.Functions.minimize_com_velocity ALIGN_SEGMENT_WITH_CUSTOM_RT = PenaltyFunctionAbstract.Functions.align_segment_with_custom_rt ALIGN_MARKER_WITH_SEGMENT_AXIS = PenaltyFunctionAbstract.Functions.align_marker_with_segment_axis CUSTOM = PenaltyFunctionAbstract.Functions.custom From 605db4374aef34499d8ae762d7da00b95d82d367 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Wed, 13 Jan 2021 14:39:47 -0500 Subject: [PATCH 02/12] Added test + blacked --- .../maximize_predicted_height_CoM.py | 25 +++++++-- ...t_global_torque_driven_with_contact_ocp.py | 51 +++++++++++++++---- 2 files changed, 63 insertions(+), 13 deletions(-) diff --git a/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py b/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py index 8d0bec89b..0b99cd8fa 100644 --- a/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py +++ b/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py @@ -13,10 +13,18 @@ InitialGuessList, ShowResult, OdeSolver, + Axe, ) -def prepare_ocp(model_path, phase_time, number_shooting_points, use_actuators=False, ode_solver=OdeSolver.RK): +def prepare_ocp( + model_path, + phase_time, + number_shooting_points, + use_actuators=False, + ode_solver=OdeSolver.RK, + objective_name="MINIMIZE_PREDICTED_COM_HEIGHT", +): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) @@ -30,7 +38,12 @@ def prepare_ocp(model_path, phase_time, number_shooting_points, use_actuators=Fa # Add objective functions objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) + if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT": + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) + elif objective_name == "MINIMIZE_COM_HEIGHT": + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_HEIGHT, weight=-1) + elif objective_name == "MINIMIZE_COM_VELOCITY": + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, axis=Axe.Z, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=1 / 100) # Dynamics @@ -81,7 +94,13 @@ def prepare_ocp(model_path, phase_time, number_shooting_points, use_actuators=Fa model_path = "2segments_4dof_2contacts.bioMod" t = 0.5 ns = 20 - ocp = prepare_ocp(model_path=model_path, phase_time=t, number_shooting_points=ns, use_actuators=False) + ocp = prepare_ocp( + model_path=model_path, + phase_time=t, + number_shooting_points=ns, + use_actuators=False, + objective_name="MINIMIZE_COM_VELOCITY", + ) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) diff --git a/tests/test_global_torque_driven_with_contact_ocp.py b/tests/test_global_torque_driven_with_contact_ocp.py index 782fa13db..96e9d05fc 100644 --- a/tests/test_global_torque_driven_with_contact_ocp.py +++ b/tests/test_global_torque_driven_with_contact_ocp.py @@ -16,7 +16,10 @@ @pytest.mark.parametrize("ode_solver", [OdeSolver.RK, OdeSolver.IRK]) -def test_maximize_predicted_height_CoM(ode_solver): +@pytest.mark.parametrize( + "objective_name", ["MINIMIZE_PREDICTED_COM_HEIGHT", "MINIMIZE_COM_HEIGHT", "MINIMIZE_COM_VELOCITY"] +) +def test_maximize_predicted_height_CoM(ode_solver, objective_name): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "maximize_predicted_height_CoM", @@ -31,13 +34,12 @@ def test_maximize_predicted_height_CoM(ode_solver): number_shooting_points=20, use_actuators=False, ode_solver=ode_solver, + objective_name=objective_name, ) sol = ocp.solve() - # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) - np.testing.assert_almost_equal(f[0, 0], 0.7592028279017864) # Check constraints g = np.array(sol["g"]) @@ -48,15 +50,44 @@ def test_maximize_predicted_height_CoM(ode_solver): states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] - # initial and final position + # initial position np.testing.assert_almost_equal(q[:, 0], np.array((0.0, 0.0, -0.5, 0.5))) - np.testing.assert_almost_equal(q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996))) - # initial and final velocities + # initial velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) - np.testing.assert_almost_equal(qdot[:, -1], np.array((1.2636414, -1.3010929, -3.6274687, 3.6274687))) - # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array((-22.1218282))) - np.testing.assert_almost_equal(tau[:, -1], np.array(0.2653957)) + + if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT": + # Check objective function value + np.testing.assert_almost_equal(f[0, 0], 0.7592028279017864) + + # final position + np.testing.assert_almost_equal(q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996))) + # final velocities + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.2636414, -1.3010929, -3.6274687, 3.6274687))) + # initial and final controls + np.testing.assert_almost_equal(tau[:, 0], np.array((-22.1218282))) + np.testing.assert_almost_equal(tau[:, -1], np.array(0.2653957)) + elif objective_name == "MINIMIZE_COM_HEIGHT": + # Check objective function value + np.testing.assert_almost_equal(f[0, 0], 0.6202830719254125) + + # final position + np.testing.assert_almost_equal(q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996))) + # final velocities + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.2497589, -1.28679918, -3.5876173, 3.5876173))) + # initial and final controls + np.testing.assert_almost_equal(tau[:, 0], np.array((-22.09830329))) + np.testing.assert_almost_equal(tau[:, -1], np.array(-0.20473133)) + elif objective_name == "MINIMIZE_COM_VELOCITY": + # Check objective function value + np.testing.assert_almost_equal(f[0, 0], 0.5378476767999407) + + # final position + np.testing.assert_almost_equal(q[:, -1], np.array((0.11896505, -0.09043769, -0.79999934, 0.79999934))) + # final velocities + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.24296589, -1.27980392, -3.56811564, 3.56811564))) + # initial and final controls + np.testing.assert_almost_equal(tau[:, 0], np.array((-21.98085033))) + np.testing.assert_almost_equal(tau[:, -1], np.array(-0.32294386)) # save and load TestUtils.save_and_load(sol, ocp, False) From ed9b8961ba174a9e3779de242a897476ea1e3fcc Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 10:54:25 -0500 Subject: [PATCH 03/12] Changed height for position --- bioptim/limits/objective_functions.py | 4 ++-- bioptim/limits/penalty.py | 20 ++++++++++++++------ 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/bioptim/limits/objective_functions.py b/bioptim/limits/objective_functions.py index 5316ddc30..f900e7f6a 100644 --- a/bioptim/limits/objective_functions.py +++ b/bioptim/limits/objective_functions.py @@ -334,7 +334,7 @@ class Lagrange(Enum): TRACK_ALL_CONTROLS = (PenaltyType.TRACK_ALL_CONTROLS,) MINIMIZE_CONTACT_FORCES = (PenaltyType.MINIMIZE_CONTACT_FORCES,) TRACK_CONTACT_FORCES = (PenaltyType.TRACK_CONTACT_FORCES,) - MINIMIZE_COM_HEIGHT = (PenaltyType.MINIMIZE_COM_HEIGHT,) + MINIMIZE_COM_POSITION = (PenaltyType.MINIMIZE_COM_POSITION,) MINIMIZE_COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) ALIGN_SEGMENT_WITH_CUSTOM_RT = (PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT,) ALIGN_MARKER_WITH_SEGMENT_AXIS = (PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS,) @@ -370,7 +370,7 @@ class Mayer(Enum): MINIMIZE_CONTACT_FORCES = (PenaltyType.MINIMIZE_CONTACT_FORCES,) TRACK_CONTACT_FORCES = (PenaltyType.TRACK_CONTACT_FORCES,) MINIMIZE_PREDICTED_COM_HEIGHT = (PenaltyType.MINIMIZE_PREDICTED_COM_HEIGHT,) - MINIMIZE_COM_HEIGHT = (PenaltyType.MINIMIZE_COM_HEIGHT,) + MINIMIZE_COM_POSITION = (PenaltyType.MINIMIZE_COM_POSITION,) MINIMIZE_COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) ALIGN_SEGMENT_WITH_CUSTOM_RT = (PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT,) ALIGN_MARKER_WITH_SEGMENT_AXIS = (PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS,) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 1aeebbb67..ae0bb4831 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -347,10 +347,11 @@ def minimize_predicted_com_height(penalty, ocp, nlp, t, x, u, p): penalty.type.get_type().add_to_penalty(ocp, nlp, CoM_height, penalty) @staticmethod - def minimize_com_height(penalty, ocp, nlp, t, x, u, p): + def minimize_com_position(penalty, ocp, nlp, t, x, u, p, axis=None): """ - Adds the objective that the height of the center of mass of the model should be minimized. - The height is assumed to be the third axis. + Adds the objective that the position of the center of mass of the model should be minimized. + If no axis is specified, the squared-norm of the CoM's position is mimimized. + Otherwise, the projection of the CoM's position on the specified axis is minimized. """ target = None @@ -361,9 +362,16 @@ def minimize_com_height(penalty, ocp, nlp, t, x, u, p): for i, v in enumerate(x): q = nlp.mapping["q"].expand.map(v[: nlp.shape["q"]]) CoM = nlp.casadi_func["biorbd_CoM"](q) - CoM_height = CoM[2] + + if axis == None: + CoM_proj = CoM[0] ** 2 + CoM[1] ** 2 + CoM[2] ** 2 + elif not isinstance(axis, Axe): + raise RuntimeError("axis must be a bioptim.Axe") + else: + CoM_proj = CoM[axis] + penalty.sliced_target = target[:, i] if target is not None else None - penalty.type.get_type().add_to_penalty(ocp, nlp, CoM_height, penalty) + penalty.type.get_type().add_to_penalty(ocp, nlp, CoM_proj, penalty) @staticmethod def minimize_com_velocity(penalty, ocp, nlp, t, x, u, p, axis=None): @@ -792,7 +800,7 @@ class PenaltyType(Enum): MINIMIZE_CONTACT_FORCES = PenaltyFunctionAbstract.Functions.minimize_contact_forces TRACK_CONTACT_FORCES = MINIMIZE_CONTACT_FORCES MINIMIZE_PREDICTED_COM_HEIGHT = PenaltyFunctionAbstract.Functions.minimize_predicted_com_height - MINIMIZE_COM_HEIGHT = PenaltyFunctionAbstract.Functions.minimize_com_height + MINIMIZE_COM_POSITION = PenaltyFunctionAbstract.Functions.minimize_com_position MINIMIZE_COM_VELOCITY = PenaltyFunctionAbstract.Functions.minimize_com_velocity ALIGN_SEGMENT_WITH_CUSTOM_RT = PenaltyFunctionAbstract.Functions.align_segment_with_custom_rt ALIGN_MARKER_WITH_SEGMENT_AXIS = PenaltyFunctionAbstract.Functions.align_marker_with_segment_axis From 0443aa5302f058abf0df71f2eb15faa1d981803a Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 10:56:54 -0500 Subject: [PATCH 04/12] Added to constraints --- bioptim/limits/constraints.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index a0fc4f16e..fdf9702ab 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -282,6 +282,8 @@ class ConstraintFcn(Enum): TRACK_CONTACT_FORCES = (PenaltyType.TRACK_CONTACT_FORCES,) ALIGN_SEGMENT_WITH_CUSTOM_RT = (PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT,) ALIGN_MARKER_WITH_SEGMENT_AXIS = (PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS,) + MINIMIZE_COM_POSITION = (PenaltyType.MINIMIZE_COM_POSITION,) + MINIMIZE_COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) CUSTOM = (PenaltyType.CUSTOM,) CONTACT_FORCE = (ConstraintFunction.Functions.contact_force,) NON_SLIPPING = (ConstraintFunction.Functions.non_slipping,) From 416ed6697327b94bbab2acde760814df0a1c2edc Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 16:02:17 -0500 Subject: [PATCH 05/12] Changed name for constraints --- bioptim/limits/constraints.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index fdf9702ab..430f163f6 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -282,8 +282,8 @@ class ConstraintFcn(Enum): TRACK_CONTACT_FORCES = (PenaltyType.TRACK_CONTACT_FORCES,) ALIGN_SEGMENT_WITH_CUSTOM_RT = (PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT,) ALIGN_MARKER_WITH_SEGMENT_AXIS = (PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS,) - MINIMIZE_COM_POSITION = (PenaltyType.MINIMIZE_COM_POSITION,) - MINIMIZE_COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) + COM_POSITION = (PenaltyType.MINIMIZE_COM_POSITION,) + COM_VELOCITY = (PenaltyType.MINIMIZE_COM_VELOCITY,) CUSTOM = (PenaltyType.CUSTOM,) CONTACT_FORCE = (ConstraintFunction.Functions.contact_force,) NON_SLIPPING = (ConstraintFunction.Functions.non_slipping,) From a836934c8e4397fbf69ea468892dc34e40367b6e Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 16:02:42 -0500 Subject: [PATCH 06/12] Added quadratic (for targetting) --- bioptim/limits/penalty.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index ae0bb4831..5c4ca8152 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -583,6 +583,8 @@ def _parameter_modifier(penalty_function, parameters): or penalty_function == PenaltyType.ALIGN_SEGMENT_WITH_CUSTOM_RT or penalty_function == PenaltyType.ALIGN_MARKER_WITH_SEGMENT_AXIS or penalty_function == PenaltyType.MINIMIZE_TORQUE_DERIVATIVE + or penalty_function == PenaltyType.MINIMIZE_COM_POSITION + or penalty_function == PenaltyType.MINIMIZE_COM_VELOCITY ): parameters.quadratic = True else: From a05f4c2d9ceafc80b1a7727660d83e1d41aa8c43 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 16:03:14 -0500 Subject: [PATCH 07/12] changed example to add constraints --- .../maximize_predicted_height_CoM.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py b/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py index 0b99cd8fa..3ad4f7f05 100644 --- a/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py +++ b/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py @@ -1,4 +1,5 @@ import biorbd +import numpy as np from bioptim import ( OptimalControlProgram, @@ -14,6 +15,9 @@ ShowResult, OdeSolver, Axe, + ConstraintList, + ConstraintFcn, + Node, ) @@ -24,6 +28,7 @@ def prepare_ocp( use_actuators=False, ode_solver=OdeSolver.RK, objective_name="MINIMIZE_PREDICTED_COM_HEIGHT", + com_constraints=False, ): # --- Options --- # # Model path @@ -40,8 +45,8 @@ def prepare_ocp( objective_functions = ObjectiveList() if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT": objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) - elif objective_name == "MINIMIZE_COM_HEIGHT": - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_HEIGHT, weight=-1) + elif objective_name == "MINIMIZE_COM_POSITION": + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION, axis=Axe.Z, weight=-1) elif objective_name == "MINIMIZE_COM_VELOCITY": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, axis=Axe.Z, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=1 / 100) @@ -53,6 +58,12 @@ def prepare_ocp( else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) + # Constraints + constraints = ConstraintList() + if com_constraints: + constraints.add(ConstraintFcn.COM_VELOCITY, node=Node.ALL, min_bound=np.array([-100, -100, -100]), max_bound=np.array([100, 100, 100])) + constraints.add(ConstraintFcn.COM_POSITION, node=Node.ALL, min_bound=np.array([-1, -1, -1]), max_bound=np.array([1, 1, 1])) + # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q @@ -85,6 +96,7 @@ def prepare_ocp( x_bounds, u_bounds, objective_functions, + constraints=constraints, tau_mapping=tau_mapping, ode_solver=ode_solver, ) @@ -100,6 +112,7 @@ def prepare_ocp( number_shooting_points=ns, use_actuators=False, objective_name="MINIMIZE_COM_VELOCITY", + com_constraints=True, ) # --- Solve the program --- # From 5446e4fcbefeb65effa0c3a2bcdd2bb7df9bf917 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 16:03:47 -0500 Subject: [PATCH 08/12] Adjusted test for Axis=Axe.Z --- ...t_global_torque_driven_with_contact_ocp.py | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/tests/test_global_torque_driven_with_contact_ocp.py b/tests/test_global_torque_driven_with_contact_ocp.py index 96e9d05fc..3527df9e9 100644 --- a/tests/test_global_torque_driven_with_contact_ocp.py +++ b/tests/test_global_torque_driven_with_contact_ocp.py @@ -17,9 +17,10 @@ @pytest.mark.parametrize("ode_solver", [OdeSolver.RK, OdeSolver.IRK]) @pytest.mark.parametrize( - "objective_name", ["MINIMIZE_PREDICTED_COM_HEIGHT", "MINIMIZE_COM_HEIGHT", "MINIMIZE_COM_VELOCITY"] + "objective_name", ["MINIMIZE_PREDICTED_COM_HEIGHT", "MINIMIZE_COM_POSITION", "MINIMIZE_COM_VELOCITY"] ) -def test_maximize_predicted_height_CoM(ode_solver, objective_name): +@pytest.mark.parametrize("com_constraints", [False, True]) +def test_maximize_predicted_height_CoM(ode_solver, objective_name, com_constraints): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "maximize_predicted_height_CoM", @@ -35,6 +36,7 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name): use_actuators=False, ode_solver=ode_solver, objective_name=objective_name, + com_constraints=com_constraints, ) sol = ocp.solve() @@ -43,8 +45,11 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name): # Check constraints g = np.array(sol["g"]) - np.testing.assert_equal(g.shape, (160, 1)) - np.testing.assert_almost_equal(g, np.zeros((160, 1))) + if com_constraints: + np.testing.assert_equal(g.shape, (202, 1)) + else: + np.testing.assert_equal(g.shape, (160, 1)) + np.testing.assert_almost_equal(g, np.zeros((160, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) @@ -66,28 +71,28 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name): # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-22.1218282))) np.testing.assert_almost_equal(tau[:, -1], np.array(0.2653957)) - elif objective_name == "MINIMIZE_COM_HEIGHT": + elif objective_name == "MINIMIZE_COM_POSITION": # Check objective function value - np.testing.assert_almost_equal(f[0, 0], 0.6202830719254125) + np.testing.assert_almost_equal(f[0, 0], 0.458575464873056) # final position np.testing.assert_almost_equal(q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996))) # final velocities - np.testing.assert_almost_equal(qdot[:, -1], np.array((1.2497589, -1.28679918, -3.5876173, 3.5876173))) + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.24525494, -1.28216182, -3.57468814, 3.57468814))) # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array((-22.09830329))) - np.testing.assert_almost_equal(tau[:, -1], np.array(-0.20473133)) + np.testing.assert_almost_equal(tau[:, 0], np.array((-21.96213697))) + np.testing.assert_almost_equal(tau[:, -1], np.array(-0.22120207)) elif objective_name == "MINIMIZE_COM_VELOCITY": # Check objective function value - np.testing.assert_almost_equal(f[0, 0], 0.5378476767999407) + np.testing.assert_almost_equal(f[0, 0], 0.4709888694097001) # final position - np.testing.assert_almost_equal(q[:, -1], np.array((0.11896505, -0.09043769, -0.79999934, 0.79999934))) + np.testing.assert_almost_equal(q[:, -1], np.array((0.1189652 , -0.09043785, -0.79999979, 0.79999979))) # final velocities - np.testing.assert_almost_equal(qdot[:, -1], np.array((1.24296589, -1.27980392, -3.56811564, 3.56811564))) + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.26103572, -1.29841047, -3.61998944, 3.61998944))) # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array((-21.98085033))) - np.testing.assert_almost_equal(tau[:, -1], np.array(-0.32294386)) + np.testing.assert_almost_equal(tau[:, 0], np.array((-22.18008227))) + np.testing.assert_almost_equal(tau[:, -1], np.array(-0.02280469)) # save and load TestUtils.save_and_load(sol, ocp, False) From 88a69eef3eb1d62afccb2abad0ff781485ecb8cd Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 16:04:00 -0500 Subject: [PATCH 09/12] Added tests to penalty_test --- tests/test_penalty.py | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/tests/test_penalty.py b/tests/test_penalty.py index e422c4cbc..efbbc9759 100644 --- a/tests/test_penalty.py +++ b/tests/test_penalty.py @@ -689,6 +689,39 @@ def test_penalty_minimize_predicted_com_height(value): ) +@pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer, ConstraintFcn]) +@pytest.mark.parametrize("value", [0.1, -10]) +def test_penalty_minimize_com_position(value, penalty_origin): + ocp = prepare_test_ocp() + x = [DM.ones((12, 1)) * value] + if 'COM_POSITION' in penalty_origin._member_names_: + penalty_type = penalty_origin.COM_POSITION + else: + penalty_type = penalty_origin.MINIMIZE_COM_POSITION + + if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)): + penalty = Objective(penalty_type) + else: + penalty = Constraint(penalty_type) + + penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], []) + + if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)): + res = ocp.nlp[0].J[0][0]["val"] + else: + res = ocp.nlp[0].g[0][0]["val"] + + expected = np.array(0.0075) + if value == -10: + expected = np.array(50.0024995) + + np.testing.assert_almost_equal(res, expected) + + if isinstance(penalty_type, ConstraintFcn): + np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min, np.array(0)) + np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max, np.array(0)) + + @pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer, ConstraintFcn]) @pytest.mark.parametrize("value", [0.1, -10]) def test_penalty_align_segment_with_custom_rt(penalty_origin, value): From 325c4469b8a27146af0fb185b256587d8d4e6528 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 14 Jan 2021 16:04:39 -0500 Subject: [PATCH 10/12] Blacked --- .../maximize_predicted_height_CoM.py | 11 +++++++++-- tests/test_global_torque_driven_with_contact_ocp.py | 6 +++--- tests/test_penalty.py | 2 +- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py b/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py index 3ad4f7f05..e51aeb581 100644 --- a/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py +++ b/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py @@ -61,8 +61,15 @@ def prepare_ocp( # Constraints constraints = ConstraintList() if com_constraints: - constraints.add(ConstraintFcn.COM_VELOCITY, node=Node.ALL, min_bound=np.array([-100, -100, -100]), max_bound=np.array([100, 100, 100])) - constraints.add(ConstraintFcn.COM_POSITION, node=Node.ALL, min_bound=np.array([-1, -1, -1]), max_bound=np.array([1, 1, 1])) + constraints.add( + ConstraintFcn.COM_VELOCITY, + node=Node.ALL, + min_bound=np.array([-100, -100, -100]), + max_bound=np.array([100, 100, 100]), + ) + constraints.add( + ConstraintFcn.COM_POSITION, node=Node.ALL, min_bound=np.array([-1, -1, -1]), max_bound=np.array([1, 1, 1]) + ) # Path constraint nb_q = biorbd_model.nbQ() diff --git a/tests/test_global_torque_driven_with_contact_ocp.py b/tests/test_global_torque_driven_with_contact_ocp.py index 3527df9e9..f36164a29 100644 --- a/tests/test_global_torque_driven_with_contact_ocp.py +++ b/tests/test_global_torque_driven_with_contact_ocp.py @@ -78,7 +78,7 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name, com_constrain # final position np.testing.assert_almost_equal(q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996))) # final velocities - np.testing.assert_almost_equal(qdot[:, -1], np.array((1.24525494, -1.28216182, -3.57468814, 3.57468814))) + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.24525494, -1.28216182, -3.57468814, 3.57468814))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-21.96213697))) np.testing.assert_almost_equal(tau[:, -1], np.array(-0.22120207)) @@ -87,9 +87,9 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name, com_constrain np.testing.assert_almost_equal(f[0, 0], 0.4709888694097001) # final position - np.testing.assert_almost_equal(q[:, -1], np.array((0.1189652 , -0.09043785, -0.79999979, 0.79999979))) + np.testing.assert_almost_equal(q[:, -1], np.array((0.1189652, -0.09043785, -0.79999979, 0.79999979))) # final velocities - np.testing.assert_almost_equal(qdot[:, -1], np.array((1.26103572, -1.29841047, -3.61998944, 3.61998944))) + np.testing.assert_almost_equal(qdot[:, -1], np.array((1.26103572, -1.29841047, -3.61998944, 3.61998944))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-22.18008227))) np.testing.assert_almost_equal(tau[:, -1], np.array(-0.02280469)) diff --git a/tests/test_penalty.py b/tests/test_penalty.py index efbbc9759..74e309666 100644 --- a/tests/test_penalty.py +++ b/tests/test_penalty.py @@ -694,7 +694,7 @@ def test_penalty_minimize_predicted_com_height(value): def test_penalty_minimize_com_position(value, penalty_origin): ocp = prepare_test_ocp() x = [DM.ones((12, 1)) * value] - if 'COM_POSITION' in penalty_origin._member_names_: + if "COM_POSITION" in penalty_origin._member_names_: penalty_type = penalty_origin.COM_POSITION else: penalty_type = penalty_origin.MINIMIZE_COM_POSITION From 2e948cc86b11891a9adcfebc2534fd5d131f6252 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Fri, 15 Jan 2021 16:17:36 -0500 Subject: [PATCH 11/12] Changed CoM size for axis == None --- bioptim/limits/penalty.py | 2 +- tests/test_global_torque_driven_with_contact_ocp.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 5c4ca8152..5c031f9aa 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -364,7 +364,7 @@ def minimize_com_position(penalty, ocp, nlp, t, x, u, p, axis=None): CoM = nlp.casadi_func["biorbd_CoM"](q) if axis == None: - CoM_proj = CoM[0] ** 2 + CoM[1] ** 2 + CoM[2] ** 2 + CoM_proj = CoM elif not isinstance(axis, Axe): raise RuntimeError("axis must be a bioptim.Axe") else: diff --git a/tests/test_global_torque_driven_with_contact_ocp.py b/tests/test_global_torque_driven_with_contact_ocp.py index f36164a29..5a10e13ce 100644 --- a/tests/test_global_torque_driven_with_contact_ocp.py +++ b/tests/test_global_torque_driven_with_contact_ocp.py @@ -46,7 +46,7 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name, com_constrain # Check constraints g = np.array(sol["g"]) if com_constraints: - np.testing.assert_equal(g.shape, (202, 1)) + np.testing.assert_equal(g.shape, (244, 1)) else: np.testing.assert_equal(g.shape, (160, 1)) np.testing.assert_almost_equal(g, np.zeros((160, 1))) From 839c457a42f12fcacde482a2215e37c0593af6df Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 18 Jan 2021 14:33:57 -0500 Subject: [PATCH 12/12] Changed test for 3 components --- tests/test_penalty.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_penalty.py b/tests/test_penalty.py index 74e309666..edf1eb23b 100644 --- a/tests/test_penalty.py +++ b/tests/test_penalty.py @@ -711,9 +711,9 @@ def test_penalty_minimize_com_position(value, penalty_origin): else: res = ocp.nlp[0].g[0][0]["val"] - expected = np.array(0.0075) + expected = np.array([[0.05], [0.05], [0.05]]) if value == -10: - expected = np.array(50.0024995) + expected = np.array([[-5], [0.05], [-5]]) np.testing.assert_almost_equal(res, expected)