Skip to content

Commit

Permalink
FIX: example which had an unexpected behavior, and a new test that th…
Browse files Browse the repository at this point in the history 
…e new implementation should pass
  • Loading branch information
@Mart1t1
Mart1t1 committed Jan 19, 2024
1 parent d65e846 commit 7166060
Show file tree
Hide file tree
Showing 2 changed files with 275 additions and 35 deletions.
158 changes: 129 additions & 29 deletions bioptim/examples/getting_started/example_continuity_as_objective.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
Solution,
PenaltyController,
PhaseDynamics,
SolutionMerge,
)


Expand Down Expand Up @@ -137,14 +136,61 @@ def prepare_ocp_first_pass(
u_init.add_noise(bounds=u_bounds, magnitude=0.01, n_shooting=n_shooting)

constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="marker_2", second_marker="target_2")
constraints.add(out_of_sphere, y=-0.45, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=0.05, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="marker_2",
second_marker="target_2",
)
constraints.add(
out_of_sphere,
y=-0.45,
z=0,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=0.05,
z=0,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
# for another good example, comment out this line below here and in second pass (see HERE)
constraints.add(out_of_sphere, y=0.55, z=-0.85, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=0.75, z=0.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=1.4, z=0.5, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=2, z=1.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(
out_of_sphere,
y=0.55,
z=-0.85,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=0.75,
z=0.2,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=1.4,
z=0.5,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=2,
z=1.2,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)

return OptimalControlProgram(
bio_model,
Expand All @@ -165,7 +211,6 @@ def prepare_ocp_first_pass(

def prepare_ocp_second_pass(
biorbd_model_path: str,
ns: int,
solution: Solution,
ode_solver: OdeSolverBase = OdeSolver.RK4(),
use_sx: bool = True,
Expand Down Expand Up @@ -208,10 +253,11 @@ def prepare_ocp_second_pass(
x_bounds["qdot"][:, 0] = 0

# Initial guess
states = solution.decision_states(to_merge=SolutionMerge.NODES)
x_init = InitialGuessList()
x_init.add("q", states["q"], interpolation=InterpolationType.EACH_FRAME)
x_init.add("qdot", states["qdot"], interpolation=InterpolationType.EACH_FRAME)
x_init.add("q", solution.states["q"], interpolation=InterpolationType.EACH_FRAME)
x_init.add(
"qdot", solution.states["qdot"], interpolation=InterpolationType.EACH_FRAME
)

# Define control path constraint
n_tau = bio_model.nb_tau
Expand All @@ -220,26 +266,75 @@ def prepare_ocp_second_pass(
u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau
u_bounds["tau"][1, :] = 0 # Prevent the model from actively rotate

controls = solution.decision_controls(to_merge=SolutionMerge.NODES)
u_init = InitialGuessList()
u_init.add("tau", controls["tau"], interpolation=InterpolationType.EACH_FRAME)
u_init.add(
"tau",
solution.controls["tau"][:, :-1],
interpolation=InterpolationType.EACH_FRAME,
)

constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="marker_2", second_marker="target_2")
constraints.add(out_of_sphere, y=-0.45, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=0.05, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="marker_2",
second_marker="target_2",
)
constraints.add(
out_of_sphere,
y=-0.45,
z=0,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=0.05,
z=0,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
# HERE (referenced in first pass)
constraints.add(out_of_sphere, y=0.55, z=-0.85, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=0.75, z=0.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=1.4, z=0.5, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(out_of_sphere, y=2, z=1.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING)
constraints.add(
out_of_sphere,
y=0.55,
z=-0.85,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=0.75,
z=0.2,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=1.4,
z=0.5,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)
constraints.add(
out_of_sphere,
y=2,
z=1.2,
min_bound=0.35,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
)

final_time = float(solution.decision_time(to_merge=SolutionMerge.NODES)[-1, 0])
return OptimalControlProgram(
bio_model,
dynamics,
ns,
final_time,
solution.ns,
solution.phase_time[-1],
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
Expand All @@ -260,19 +355,21 @@ def main():
# --- First pass --- #
# --- Prepare the ocp --- #
np.random.seed(123456)
n_shooting = 500
ocp_first = prepare_ocp_first_pass(
biorbd_model_path="models/pendulum_maze.bioMod",
final_time=5,
n_shooting=n_shooting,
n_shooting=500,
# change the weight to observe the impact on the continuity of the solution
# or comment to see how the constrained program would fare
state_continuity_weight=1_000_000,
n_threads=3,
)
# ocp_first.print(to_console=True)

solver_first = Solver.IPOPT(show_online_optim=platform.system() == "Linux", show_options=dict(show_bounds=True))
solver_first = Solver.IPOPT(
show_online_optim=platform.system() == "Linux",
show_options=dict(show_bounds=True),
)
# change maximum iterations to affect the initial solution
# it doesn't mather if it exits before the optimal solution, only that there is an initial guess
solver_first.set_maximum_iterations(500)
Expand All @@ -286,11 +383,14 @@ def main():

# # --- Second pass ---#
# # --- Prepare the ocp --- #
solver_second = Solver.IPOPT(show_online_optim=platform.system() == "Linux", show_options=dict(show_bounds=True))
solver_second = Solver.IPOPT(
show_online_optim=platform.system() == "Linux",
show_options=dict(show_bounds=True),
)
solver_second.set_maximum_iterations(10000)

ocp_second = prepare_ocp_second_pass(
biorbd_model_path="models/pendulum_maze.bioMod", ns=n_shooting, solution=sol_first, n_threads=3
biorbd_model_path="models/pendulum_maze.bioMod", solution=sol_first, n_threads=3
)

# Custom plots
Expand Down
Loading

0 comments on commit 7166060

Please sign in to comment.