Skip to content

Commit

Permalink
Merge pull request #798 from Ipuch/minmax_scoot
Browse files Browse the repository at this point in the history
Udpate min max + test
  • Loading branch information
pariterre committed Nov 8, 2023
2 parents aada553 + 39b3a92 commit 88695f1
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 25 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@
BiMappingList,
ParameterList,
InterpolationType,
Bounds,
InitialGuess,
BiorbdModel,
PenaltyController,
ParameterObjectiveList,
Expand Down Expand Up @@ -55,23 +53,24 @@ def prepare_ocp(

# Mapping
tau_mappings = BiMappingList()
tau_mappings.add("tau", [None, 0], [1], phase=0)
tau_mappings.add("tau", [None, 0], [1], phase=1)
tau_mappings.add("tau", to_second=[None, 0], to_first=[1], phase=0)
tau_mappings.add("tau", to_second=[None, 0], to_first=[1], phase=1)

# Define the parameter to optimize
parameters = ParameterList()

parameter_initial_guess = InitialGuess(0)
parameter_bounds = Bounds(0, tau_max, interpolation=InterpolationType.CONSTANT)

parameters.add(
"max_tau", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
parameter_initial_guess, # The initial guess
parameter_bounds, # The bounds
size=1,
)

parameter_bounds = BoundsList()
parameter_init = InitialGuessList()

parameter_init["max_tau"] = 0
parameter_bounds.add("max_tau", min_bound=0, max_bound=tau_max, interpolation=InterpolationType.CONSTANT)

# Add phase independant objective functions
parameter_objectives = ParameterObjectiveList()
parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=1000, quadratic=True)
Expand All @@ -98,38 +97,43 @@ def prepare_ocp(

# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=bio_model[0].bounds_from_ranges(["q", "qdot"]))
x_bounds.add(bounds=bio_model[0].bounds_from_ranges(["q", "qdot"]))
x_bounds.add(key="q", bounds=bio_model[0].bounds_from_ranges("q"), phase=0)
x_bounds.add(key="qdot", bounds=bio_model[0].bounds_from_ranges("qdot"), phase=0)
x_bounds.add(key="q", bounds=bio_model[1].bounds_from_ranges("q"), phase=1)
x_bounds.add(key="qdot", bounds=bio_model[1].bounds_from_ranges("qdot"), phase=1)

# change model bound for -pi, pi
for i in range(len(bio_model)):
x_bounds[i].min[1, :] = -np.pi
x_bounds[i].max[1, :] = np.pi
x_bounds[i]["q"].min[1, :] = -np.pi
x_bounds[i]["q"].max[1, :] = np.pi

# Phase 0
x_bounds[0][0, 0] = np.pi
x_bounds[0][1, 0] = 0
x_bounds[0].min[1, -1] = 6 * np.pi / 8 - 0.1
x_bounds[0].max[1, -1] = 6 * np.pi / 8 + 0.1
x_bounds[0]["q"][0, 0] = np.pi
x_bounds[0]["q"][1, 0] = 0
x_bounds[0]["q"].min[1, -1] = 6 * np.pi / 8 - 0.1
x_bounds[0]["q"].max[1, -1] = 6 * np.pi / 8 + 0.1

# Phase 1
x_bounds[1][0, -1] = 3 * np.pi
x_bounds[1][1, -1] = 0
x_bounds[1]["q"][0, -1] = 3 * np.pi
x_bounds[1]["q"][1, -1] = 0

# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (bio_model[0].nb_q + bio_model[0].nb_qdot))
x_init.add([1] * (bio_model[1].nb_q + bio_model[1].nb_qdot))
x_init.add(key="q", initial_guess=[0] * bio_model[0].nb_q, phase=0)
x_init.add(key="qdot", initial_guess=[0] * bio_model[0].nb_qdot, phase=0)
x_init.add(key="q", initial_guess=[1] * bio_model[1].nb_q, phase=1)
x_init.add(key="qdot", initial_guess=[1] * bio_model[1].nb_qdot, phase=1)

# Define control path constraint
n_tau = len(tau_mappings[0]["tau"].to_first)
u_bounds = BoundsList()
u_bounds.add([tau_min] * len(tau_mappings[0]["tau"].to_first), [tau_max] * len(tau_mappings[0]["tau"].to_first))
u_bounds.add([tau_min] * len(tau_mappings[1]["tau"].to_first), [tau_max] * len(tau_mappings[1]["tau"].to_first))
u_bounds.add(key="tau", min_bound=[tau_min] * n_tau, max_bound=[tau_max] * n_tau, phase=0)
u_bounds.add(key="tau", min_bound=[tau_min] * n_tau, max_bound=[tau_max] * n_tau, phase=1)

# Control initial guess
u_init = InitialGuessList()
u_init.add([tau_init] * len(tau_mappings[0]["tau"].to_first))
u_init.add([tau_init] * len(tau_mappings[1]["tau"].to_first))
u_init.add(key="tau", initial_guess=[tau_init] * n_tau, phase=0)
u_init.add(key="tau", initial_guess=[tau_init] * n_tau, phase=1)

return OptimalControlProgram(
bio_model,
Expand All @@ -143,6 +147,8 @@ def prepare_ocp(
objective_functions=objective_functions,
parameter_objectives=parameter_objectives,
parameter_constraints=parameter_constraints,
parameter_bounds=parameter_bounds,
parameter_init=parameter_init,
constraints=constraints,
variable_mappings=tau_mappings,
parameters=parameters,
Expand Down
12 changes: 12 additions & 0 deletions tests/shard1/test_all_examples.py
Original file line number Diff line number Diff line change
Expand Up @@ -1479,3 +1479,15 @@ def test__contact_and_muscle_forces_example_excitation():
min_bound=50,
expand_dynamics=False,
)


def test_min_max_example():
from bioptim.examples.torque_driven_ocp import (
minimize_maximum_torque_by_extra_parameter as ocp_module,
)

bioptim_folder = os.path.dirname(ocp_module.__file__)

ocp_module.prepare_ocp(
bio_model_path=bioptim_folder + "/models/double_pendulum.bioMod",
)

0 comments on commit 88695f1

Please sign in to comment.