def test_lagrange_neg_monophase_time_constraint():
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=1)
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
objective_functions.add(Objective.Lagrange.MINIMIZE_TIME)
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0])
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_mayer_neg_monophase_time_constraint():
(
biorbd_model,
n_shooting,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(n_phases=1)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0])
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_custom_constraint_mx_fail():
def custom_mx_fail(pn):
if pn.u is None:
return None
u = pn.nlp.controls
return MX.zeros(u.shape), u.cx, MX.zeros(u.shape)
bioptim_folder = TestUtils.bioptim_folder()
model_path = bioptim_folder + "/examples/getting_started/models/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path), Dynamics(DynamicsFcn.TORQUE_DRIVEN), 30, 2, constraints=constraints
)
with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"):
ocp.solve()
def test_acados_one_end_constraints():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import cube as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Mayer.TRACK_STATE, index=0, key="q", weight=100, target=np.array([[1]]), multi_thread=False
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False)
ocp.update_objectives(objective_functions)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[0, 0] = 0
ocp.update_bounds(x_bounds=x_bounds)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2")
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS())
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.array((-2.72727272, 9.81, 0)), decimal=6)
def prepare_nmpc(model_path, cycle_len, cycle_duration, max_torque):
model = biorbd.Model(model_path)
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
x_bound = QAndQDotBounds(model)
u_bound = Bounds([-max_torque] * model.nbQ(), [max_torque] * model.nbQ())
x_init = InitialGuess(
np.zeros(
model.nbQ() * 2,
)
)
u_init = InitialGuess(
np.zeros(
model.nbQ(),
)
)
new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q")
# Rotate the wheel and force the marker of the hand to follow the marker on the wheel
wheel_target = np.linspace(-np.pi, np.pi, cycle_len + 1)[np.newaxis, :]
constraints = ConstraintList()
constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.ALL,
first_marker="wheel",
second_marker="COM_hand",
axes=[Axis.X, Axis.Y],
)
return MyCyclicNMPC(
model,
dynamics,
cycle_len,
cycle_duration,
objective_functions=new_objectives,
constraints=constraints,
x_init=x_init,
u_init=u_init,
x_bounds=x_bound,
u_bounds=u_bound,
)
def test_custom_constraint_mx_fail():
def custom_mx_fail(pn):
return MX(0), vertcat(*pn.u), MX(0)
bioptim_folder = TestUtils.bioptim_folder()
model_path = bioptim_folder + "/examples/getting_started/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path),
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
30,
2,
constraints=constraints,
)
with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"):
sol = ocp.solve(show_online_optim=True)
def test_acados_custom_dynamics(problem_type_custom):
# Load pendulum
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"custom_problem_type_and_dynamics",
str(PROJECT_FOLDER) + "/examples/getting_started/custom_dynamics.py",
)
pendulum = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum)
ocp = pendulum.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
problem_type_custom=problem_type_custom,
ode_solver=OdeSolver.RK,
use_SX=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -1], np.array(
(0, 9.81, -2.27903226)))
def test_custom_constraint_mx_fail():
def custom_mx_fail(ocp, nlp, t, x, u, p):
return MX(0), vertcat(*u), MX(0)
PROJECT_FOLDER = Path(__file__).parent / ".."
model_path = str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path),
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
30,
2,
constraints=constraints,
)
with pytest.raises(
RuntimeError,
match="Ipopt doesn't support SX/MX types in constraints bounds"):
sol = ocp.solve(show_online_optim=True)
def test_acados_constraints_end_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.track import track_marker_on_segment as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m5")
constraints.add(
ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL_SHOOTING, marker="m1", segment="seg_rt", axis=Axis.X
)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS())
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0], np.array([2.01701330, 0, 0, 3.20057865e-01]), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57, 7.85398168e-01)), decimal=6)
np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0.04648408, 9.88616194, 2.24285498, 0.864213)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.array((0.19389194, 9.99905781, -2.37713652, -0.19858311)), decimal=6)
def test_acados_constraints_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.track import track_marker_on_segment as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(
ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=Axis.X
)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS())
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0], np.array([2.28988221, 0, 0, 2.95087911e-01]), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2.28215749, 0, 1.57, 6.62470772e-01)), decimal=6)
np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0.04483914, 9.90739842, 2.24951691, 0.78496612)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.array((0.15945561, 10.03978178, -2.36075327, 0.07267697)), decimal=6)
def test_lagrange_multiphase_time_constraint():
(
biorbd_model,
n_shooting,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(n_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, phase=0)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=2)
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=2)
OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_acados_custom_dynamics(problem_type_custom):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(
bioptim_folder + "/examples/getting_started/custom_dynamics.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/getting_started/models/cube.bioMod",
problem_type_custom=problem_type_custom,
ode_solver=OdeSolver.RK4(),
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -2], np.array(
(0, 9.81, -2.27903226)))
def test_lagrange_multiphase_time_constraint():
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, phase=0)
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=2)
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=2)
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(
biorbd_model_path: str,
problem_type_custom: bool = True,
ode_solver: OdeSolver = OdeSolver.RK4(),
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
problem_type_custom: bool
If the preparation should be done using the user-defined dynamic function or the normal TORQUE_DRIVEN.
They should return the same results
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
if problem_type_custom:
dynamics.add(custom_configure, dynamic_function=custom_dynamic, my_additional_factor=1, expand=expand)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, dynamic_function=custom_dynamic, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2")
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
initialize_near_solution: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
constr: bool = True,
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
initialize_near_solution: bool
If the initial guess should be almost the solution (this is merely to reduce the time of the tests)
ode_solver: OdeSolver
The ode solver to use
constr: bool
If the constraint should be applied (this is merely to reduce the time of the tests)
use_sx: bool
If SX CasADi variables should be used
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
if constr:
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m4")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m5")
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker="m1",
segment="seg_rt",
axis=Axis.X)
else:
constraints = ConstraintList()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0][i, [0, -1]] = 0
x_bounds[0][2, -1] = 1.57
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
x_init[0].init[i] = 1.5
for i in range(4, 6):
x_init[0].init[i] = 0.7
for i in range(6, 8):
x_init[0].init[i] = 0.6
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
def prepare_ocp(
biorbd_model_path: str, ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_track_markers,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(custom_func_track_markers,
node=Node.END,
first_marker="m0",
second_marker="m2")
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path="cube_with_forces.bioMod",
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# External forces
external_forces = [
np.repeat(np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0,
5]]).T[:, :, np.newaxis],
number_shooting_points,
axis=2)
]
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
external_forces=external_forces,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound,
max_bound):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
dof_mapping = BiMappingList()
dof_mapping.add("tau", [None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_torque=True,
with_contact=True)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL_SHOOTING,
contact_index=1,
)
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL_SHOOTING,
contact_index=2,
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * len(dof_mapping["tau"].to_first) +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * len(dof_mapping["tau"].to_first) +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first) +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints=constraints,
variable_mappings=dof_mapping,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
interpolation_type: InterpolationType = InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
) -> OptimalControlProgram:
"""
Prepare the ocp for the specified interpolation type
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
n_shooting: int
The number of shooting point
final_time: float
The movement time
interpolation_type: InterpolationType
The requested InterpolationType
Returns
-------
The OCP fully prepared and ready to be solved
"""
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
# Path constraints
if interpolation_type == InterpolationType.CONSTANT:
x_min = [-100] * (nq + nqdot)
x_max = [100] * (nq + nqdot)
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.CONSTANT)
u_min = [tau_min] * ntau
u_max = [tau_max] * ntau
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.CONSTANT)
elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x_min = np.random.random((6, 3)) * (-10) - 5
x_max = np.random.random((6, 3)) * 10 + 5
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
u_min = np.random.random((3, 3)) * tau_min + tau_min / 2
u_max = np.random.random((3, 3)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
elif interpolation_type == InterpolationType.LINEAR:
x_min = np.random.random((6, 2)) * (-10) - 5
x_max = np.random.random((6, 2)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR)
u_min = np.random.random((3, 2)) * tau_min + tau_min / 2
u_max = np.random.random((3, 2)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR)
elif interpolation_type == InterpolationType.EACH_FRAME:
x_min = np.random.random((nq + nqdot, n_shooting + 1)) * (-10) - 5
x_max = np.random.random((nq + nqdot, n_shooting + 1)) * 10 + 5
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.EACH_FRAME)
u_min = np.random.random((ntau, n_shooting)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, n_shooting)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.EACH_FRAME)
elif interpolation_type == InterpolationType.SPLINE:
spline_time = np.hstack(
(0, np.sort(np.random.random((3, )) * final_time), final_time))
x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5
x_max = np.random.random((nq + nqdot, 5)) * 10 + 5
u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.SPLINE,
t=spline_time)
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.SPLINE,
t=spline_time)
elif interpolation_type == InterpolationType.CUSTOM:
# The custom functions refer to the ones at the beginning of the file.
# For this particular instance, they emulate a Linear interpolation
extra_params_x = {"n_elements": nq + nqdot, "n_shooting": n_shooting}
extra_params_u = {"n_elements": ntau, "n_shooting": n_shooting}
x_bounds = Bounds(custom_x_bounds_min,
custom_x_bounds_max,
interpolation=InterpolationType.CUSTOM,
**extra_params_x)
u_bounds = Bounds(custom_u_bounds_min,
custom_u_bounds_max,
interpolation=InterpolationType.CUSTOM,
**extra_params_u)
else:
raise NotImplementedError("Not implemented yet")
# Initial guess
x_init = InitialGuess([0] * (nq + nqdot))
u_init = InitialGuess([tau_init] * ntau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(
biorbd_model_path,
final_time,
number_shooting_points,
initialize_near_solution,
ode_solver=OdeSolver.RK,
constr=True,
use_SX=False,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
if constr is True:
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=4)
constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5)
constraints.add(
ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=(Axe.X)
)
else:
constraints = ConstraintList()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0][i, [0, -1]] = 0
x_bounds[0][2, -1] = 1.57
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
x_init[0].init[i] = 1.5
for i in range(4, 6):
x_init[0].init[i] = 0.7
for i in range(6, 8):
x_init[0].init[i] = 0.6
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_SX=use_SX,
)
def prepare_ocp(biorbd_model_path,
phase_time,
n_shooting,
min_bound,
max_bound,
mu,
ode_solver=OdeSolver.RK4()):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BiMapping([None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=mu,
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * tau_mapping.to_first.len,
[tau_max] * tau_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.to_first.len)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path,
number_shooting_points,
final_time,
use_actuators=False,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
x_bounds[0][2, [0, -1]] = [0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp_phase_transitions(
biorbd_model_path: str,
with_constraints: bool,
with_mayer: bool,
with_lagrange: bool,
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
Returns
-------
The ocp ready to be solved
"""
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
n_shooting = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
if with_lagrange:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=3)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY,
weight=0,
phase=3,
axis=None)
if with_mayer:
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION,
phase=0,
node=1)
objective_functions.add(
minimize_difference,
custom_type=ObjectiveFcn.Mayer,
node=Node.TRANSITION,
weight=100,
phase=1,
get_all_nodes_at_once=True,
quadratic=True,
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
if with_constraints:
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=2,
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.add(custom_func_track_markers,
node=Node.ALL,
first_marker="m0",
second_marker="m1",
phase=3)
# 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]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -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()))
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_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())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# Define phase transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=2,
idx_1=1,
idx_2=3)
phase_transitions.add(PhaseTransitionFcn.CYCLIC)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
phase_transitions=phase_transitions,
)
def prepare_ocp(model_path,
phase_time,
number_shooting_points,
min_bound,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=2,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
nb_mus = biorbd_model.nbMuscleTotal()
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(
Bounds([activation_min] * nb_mus, [activation_max] * nb_mus))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[torque_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([torque_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
tau_mapping=tau_mapping,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
phase_time: float,
n_shooting: int,
use_actuators: bool = False,
ode_solver: OdeSolver = OdeSolver.RK4(),
objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT",
com_constraints: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
phase_time: float
The time at the final node
n_shooting: int
The number of shooting points
use_actuators: bool
If torque or torque activation should be used for the dynamics
ode_solver: OdeSolver
The ode solver to use
objective_name: str
The objective function to run ('MINIMIZE_PREDICTED_COM_HEIGHT',
'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY')
com_constraints: bool
If a constraint on the COM should be applied
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
dof_mapping = BiMappingList()
dof_mapping.add("tau", [None, None, None, 0], [3])
# Add objective functions
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_POSITION":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION, node=Node.ALL, axes=Axis.Z, weight=-1)
elif objective_name == "MINIMIZE_COM_VELOCITY":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, node=Node.ALL, axes=Axis.Z, weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1 / 100)
# Dynamics
dynamics = DynamicsList()
if use_actuators:
dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=True)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True)
# Constraints
constraints = ConstraintList()
if com_constraints:
constraints.add(
ConstraintFcn.TRACK_COM_VELOCITY,
node=Node.ALL,
min_bound=np.array([-100, -100, -100]),
max_bound=np.array([100, 100, 100]),
)
constraints.add(
ConstraintFcn.TRACK_COM_POSITION,
node=Node.ALL,
min_bound=np.array([-1, -1, -1]),
max_bound=np.array([1, 1, 1]),
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * len(dof_mapping["tau"].to_first), [tau_max] * len(dof_mapping["tau"].to_first))
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first))
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints=constraints,
variable_mappings=dof_mapping,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
actuator_type: int = None,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
n_shooting: int
The number of shooting points
final_time: float
The time at final node
actuator_type: int
The type of actuator to use: 1 (torque activations) or 2 (torque max constraints)
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
if actuator_type and actuator_type == 1:
tau_min, tau_max, tau_init = -1, 1, 0
else:
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)
# Dynamics
dynamics = DynamicsList()
if actuator_type:
if actuator_type == 1:
dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN)
elif actuator_type == 2:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
else:
raise ValueError(
"actuator_type is 1 (torque activations) or 2 (torque max constraints)"
)
else:
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
if actuator_type == 2:
constraints.add(ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT,
node=Node.ALL_SHOOTING,
min_torque=7.5)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
x_bounds[0][2, [0, -1]] = [0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
biorbd_model_path = TestUtils.bioptim_folder(
) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK4()
# --- Options --- #
n_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2)
# 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.add(
ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2
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())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
new_gravity = MX.zeros(3, 1)
new_gravity[2] = value + extra_value
biorbd_model.setGravity(new_gravity)
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:n_phases],
dynamics,
ns,
final_time[:n_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
def prepare_ocp(
biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
n_shooting = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
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)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=3)
# 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)
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.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=3)
# 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]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -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()))
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_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())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
"""
By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3)
are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous,
as for example IMPACT or CUSTOM can be used as below.
"phase_pre_idx" corresponds to the index of the phase preceding the transition.
IMPACT will cause an impact related discontinuity when defining one or more contact points in the model.
CUSTOM will allow to call the custom function previously presented in order to have its own phase transition.
Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity)
you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx.
If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than
zero. It will thereafter be treated as a Mayer objective function with the specified weight.
"""
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping(range(3), range(3)))
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5)
phase_transitions.add(PhaseTransitionFcn.CYCLIC)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
loop_from_constraint: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
loop_from_constraint: bool
If the looping cost should be a constraint [True] or an objective [False]
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.MID,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# First node is free but mid and last are constrained to be exactly at a certain point.
# The cyclic penalty ensures that the first node and the last node are the same.
x_bounds[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A phase transition loop constraint is treated as
# hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or
# as a soft penalty (objective) otherwise
phase_transitions = PhaseTransitionList()
if loop_from_constraint:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0)
else:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
Objective.Lagrange.MINIMIZE_TORQUE,
instant=Instant.ALL,
state_idx=bow.hair_idx,
weight=1,
idx=3
) # permet de réduire le nombre d'itérations avant la convergence
new_objectives.add(
Objective.Lagrange.MINIMIZE_TORQUE_DERIVATIVE,
instant=Instant.ALL,
state_idx=bow.hair_idx,
# weight=1,
idx=4
) # rajoute des itérations et ne semble riuen changer au mouvement...
ocp.update_objectives(new_objectives)
new_constraints = ConstraintList()
for j in range(1, 5):
new_constraints.add(Constraint.ALIGN_MARKERS,
instant=j,
min_bound=0,
max_bound=0,
first_marker_idx=Bow.contact_marker,
second_marker_idx=violin.bridge_marker)
for j in range(5, nb_shooting_pts_window + 1):
new_constraints.add(
Constraint.ALIGN_MARKERS,
instant=j,
# min_bound=-1, #-10**(j-14) donne 25 itérations
# max_bound=1, # (j-4)/10 donne 21 itérations
first_marker_idx=Bow.contact_marker,
second_marker_idx=violin.bridge_marker)