def test_multinode_fail_second_node(node):
# Constraints
multinode_constraints = MultinodeConstraintList()
# hard constraint
with pytest.raises(
NotImplementedError,
match="Multi Node Constraint only works with Node.START, Node.MID, Node.PENULTIMATE, Node.END or a int.",
):
multinode_constraints.add(
MultinodeConstraintFcn.EQUALITY,
phase_first_idx=0,
phase_second_idx=2,
first_node=Node.START,
second_node=node,
)
def prepare_ocp(
biorbd_model_path: str = "models/cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solve to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
n_shooting = (100, 300, 100)
final_time = (2, 5, 4)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=2)
# Constraints
multinode_constraints = MultinodeConstraintList()
# hard constraint
multinode_constraints.add(
MultinodeConstraintFcn.EQUALITY,
phase_first_idx=0,
phase_second_idx=2,
first_node=Node.START,
second_node=Node.START,
)
# Objectives with the weight as an argument
multinode_constraints.add(
MultinodeConstraintFcn.EQUALITY,
phase_first_idx=0,
phase_second_idx=2,
first_node=2,
second_node=Node.MID,
weight=2,
)
# Objectives with the weight as an argument
multinode_constraints.add(
MultinodeConstraintFcn.EQUALITY,
phase_first_idx=0,
phase_second_idx=1,
first_node=Node.MID,
second_node=Node.END,
weight=0.1,
)
# Objectives with the weight as an argument
multinode_constraints.add(
custom_multinode_constraint,
phase_first_idx=0,
phase_second_idx=1,
first_node=Node.MID,
second_node=Node.PENULTIMATE,
weight=0.1,
coef=2,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
multinode_constraints=multinode_constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, phase_1, phase_2) -> OptimalControlProgram:
biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path))
# Problem parameters
n_shooting = (100, 300, 100)
final_time = (2, 5, 4)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
multinode_constraints = MultinodeConstraintList()
# hard constraint
multinode_constraints.add(
MultinodeConstraintFcn.EQUALITY,
phase_first_idx=phase_1,
phase_second_idx=phase_2,
first_node=Node.START,
second_node=Node.START,
)
multinode_constraints.add(
MultinodeConstraintFcn.COM_EQUALITY,
phase_first_idx=phase_1,
phase_second_idx=phase_2,
first_node=Node.START,
second_node=Node.START,
)
multinode_constraints.add(
MultinodeConstraintFcn.COM_VELOCITY_EQUALITY,
phase_first_idx=phase_1,
phase_second_idx=phase_2,
first_node=Node.START,
second_node=Node.START,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
multinode_constraints=multinode_constraints,
ode_solver=OdeSolver.RK4(),
)