def test_acados_one_mayer(cost_type):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"cube",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
cube = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cube)
ocp = cube.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=10,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE,
index=[0],
target=np.array([[1.0]]).T)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod")
q = np.array(sol["qqdot"])[:model.nbQ()]
np.testing.assert_almost_equal(q[0, -1], 1.0)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_one_lagrange(cost_type):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"cube",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
cube = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cube)
nbs = 10
target = np.expand_dims(np.arange(0, nbs + 1), axis=0)
target[0, -1] = nbs - 2
ocp = cube.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=nbs,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=10,
index=[0],
target=target)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod")
q = np.array(sol["qqdot"])[:model.nbQ()]
np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_pendulum_max_time_lagrange_constrained(ode_solver):
# Load pendulum_min_time_Lagrange
biorbd_model_path = (TestUtils.bioptim_folder() +
"/examples/optimal_time_ocp/pendulum.bioMod", )
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path[0])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME,
weigth=-1,
max_bound=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# ------------- #
with pytest.raises(
RuntimeError,
match=
"ObjectiveFcn.Lagrange.MINIMIZE_TIME cannot have max_bound. Please either use MAYER or constraint",
):
OptimalControlProgram(biorbd_model,
dynamics,
10,
2,
objective_functions=objective_functions)
def test_acados_one_lagrange(cost_type):
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
n_shooting = 10
target = np.expand_dims(np.arange(0, n_shooting + 1), axis=0)
target[0, -1] = n_shooting - 2
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod",
n_shooting=n_shooting,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=10,
index=[0],
target=target)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
ode_solver=OdeSolver.RK):
# --- Options --- #nq
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
# 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
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_SEGMENT_WITH_CUSTOM_RT,
node=Node.ALL,
segment_idx=2,
rt_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][2, [0, -1]] = [-1.57, 1.57]
x_bounds[0][nq:, [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,
constraints,
ode_solver=ode_solver,
)
def test_acados_one_mayer(cost_type):
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod",
n_shooting=10,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE,
index=[0],
target=np.array([[1.0]]).T)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], 1.0)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_mayer_neg_two_objectives():
(
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(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=0)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=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,
)
def test_pendulum_max_time_lagrange_constrained(ode_solver):
# Load pendulum_min_time_Lagrange
biorbd_model_path = (TestUtils.bioptim_folder() +
"/examples/optimal_time_ocp/models/pendulum.bioMod", )
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path[0])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME,
weight=-1,
max_bound=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# ------------- #
with pytest.raises(
TypeError,
match=re.escape(
"minimize_time() got an unexpected keyword argument 'max_bound'"
)):
OptimalControlProgram(biorbd_model,
dynamics,
10,
2,
objective_functions=objective_functions)
def test_acados_one_mayer(cost_type):
if platform == "win32":
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,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0], target=np.array([[1.0]]).T)
ocp.update_objectives(objective_functions)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], 1.0)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_one_parameter():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.getting_started import custom_parameters as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
final_time=1,
n_shooting=100,
optim_gravity=True,
optim_mass=False,
min_g=np.array([-1, -1, -10]),
max_g=np.array([1, 1, -5]),
min_m=10,
max_m=30,
target_g=np.array([0, 0, -9.81]),
target_m=20,
use_sx=True,
)
model = ocp.nlp[0].model
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="q", target=np.array([[0, 3.14]]).T, weight=100000)
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="qdot", target=np.array([[0, 0]]).T, weight=100)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=1, weight=10, multi_thread=False)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.000000010, multi_thread=False)
ocp.update_objectives(objectives)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[[0, 1, 2, 3], 0] = 0
u_bounds = Bounds([-300] * model.nbQ(), [300] * model.nbQ())
ocp.update_bounds(x_bounds, u_bounds)
solver = Solver.ACADOS()
solver.set_nlp_solver_tol_eq(1e-3)
sol = ocp.solve(solver=solver)
# Check some of the results
q, qdot, tau, gravity = sol.states["q"], sol.states["qdot"], sol.controls["tau"], sol.parameters["gravity_xyz"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14)), decimal=6)
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)), decimal=6)
# parameters
np.testing.assert_almost_equal(gravity[-1, :], np.array([-9.81]), decimal=6)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def prepare_ocp(
biorbd_model_path,
final_time,
number_shooting_points,
ode_solver=OdeSolver.RK,
weight=1,
min_time=0,
max_time=np.inf,
):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
# A weight of -1 will maximize time
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=weight, min_bound=min_time, max_bound=max_time)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path, phase_time, n_shooting, muscle_activations_ref, contact_forces_ref, ode_solver=OdeSolver.RK4()
):
# 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
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MUSCLES_CONTROL, target=muscle_activations_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_ALL_CONTROLS, weight=0.001)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# 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] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
ode_solver=ode_solver,
)
def __init__(
self,
model_path: str,
violin: Violin,
bow: Bow,
n_cycles: int,
bow_starting: BowPosition.TIP,
init_file: str = None,
use_muscles: bool = True,
time_per_cycle: float = 1,
n_shooting_per_cycle: int = 30,
solver: Solver = Solver.IPOPT,
n_threads: int = 8,
):
self.model_path = model_path
self.model = biorbd.Model(self.model_path)
self.n_q = self.model.nbQ()
self.n_tau = self.model.nbGeneralizedTorque()
self.use_muscles = use_muscles
self.n_mus = self.model.nbMuscles() if self.use_muscles else 0
self.violin = violin
self.bow = bow
self.bow_starting = bow_starting
self.n_cycles = n_cycles
self.n_shooting_per_cycle = n_shooting_per_cycle
self.n_shooting = self.n_shooting_per_cycle * self.n_cycles
self.time_per_cycle = time_per_cycle
self.time = self.time_per_cycle * self.n_cycles
self.solver = solver
self.n_threads = n_threads
if self.use_muscles:
self.dynamics = Dynamics(
DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
self.dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
self.x_bounds = Bounds()
self.u_bounds = Bounds()
self._set_bounds()
self.x_init = InitialGuess()
self.u_init = InitialGuess()
self._set_initial_guess(init_file)
self.objective_functions = ObjectiveList()
self._set_generic_objective_functions()
self.constraints = ConstraintList()
self._set_generic_constraints()
self._set_generic_ocp()
if use_muscles:
online_muscle_torque(self.ocp)
def prepare_ocp(biorbd_model_path="cubeSym.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)
constraints.add(Constraint.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][4:8, [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.nbQ(), [tau_max] * biorbd_model.nbQ()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbQ())
# ------------- #
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(biorbd_model_path, final_time, n_shooting, x_warm=None, use_sx=False, n_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -50, 50, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, weight=100000, first_marker="target", second_marker="COM_hand"
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_residual_torque=True)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (1.0, 1.0, 0, 0)
# Initial guess
if x_warm is None:
x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
else:
x_init = InitialGuess(x_warm, interpolation=InterpolationType.EACH_FRAME)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
n_threads=n_threads,
)
def define_objective(
q: np.array, iteration: int, rt_ratio: int, ns_mhe: int, biorbd_model: biorbd.Model, use_noise=True
):
"""
Define the objective function for the ocp
Parameters
----------
q: np.array
State to track
iteration: int
Current iteration
rt_ratio: int
Value of the reference data ratio to send to the estimator
ns_mhe: int
Size of the windows
biorbd_model: biorbd.Model
biorbd model build with the bioMod
use_noise: bool
True if noisy reference data
Returns
---------
The objective function
"""
objectives = ObjectiveList()
if use_noise is not True:
weight = {"track_state": 1000000, "min_act": 1000, "min_dq": 10, "min_q": 10}
else:
weight = {"track_state": 1000, "min_act": 100, "min_dq": 100, "min_q": 10}
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=weight["min_act"])
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=weight["min_dq"],
index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=weight["min_q"],
index=np.array(range(biorbd_model.nbQ())),
)
q = q[:, ::rt_ratio]
objectives.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
weight=weight["track_state"],
target=q[:, iteration : (ns_mhe + 1 + iteration)],
index=range(biorbd_model.nbQ()),
)
return objectives
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -1, 1, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(ObjectiveFcn.Mayer.ALIGN_MARKERS,
first_marker_idx=0,
second_marker_idx=5)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (0, 0.07, 1.4, 0, 0, 0)
# Initial guess
x_init = InitialGuessList()
x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_TIME)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
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_idx=0,
second_marker_idx=1)
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 prepare_ocp(biorbd_model_path, problem_type_custom=True, ode_solver=OdeSolver.RK, use_SX=False):
# --- 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.Mayer.MINIMIZE_STATE, weight=1000, states_idx=[0, 1], target=np.array([[1., 2.]]).T)
objective_functions.add(Objective.Mayer.MINIMIZE_STATE, weight=10000, states_idx=[2], target=np.array([[3.]]))
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1,)
# Dynamics
dynamics = DynamicsTypeList()
if problem_type_custom:
dynamics.add(custom_configure, dynamic_function=custom_dynamic)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN, dynamic_function=custom_dynamic)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, 0] = 0
# Initial guess
x_init = InitialGuessOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption(
[[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]
)
u_init = InitialGuessOption([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,
ode_solver=ode_solver,
use_SX=use_SX
)
def test_acados_control_lagrange_and_state_mayer(cost_type):
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__)
n_shooting = 10
target = np.array([[2]])
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=n_shooting,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", multi_thread=False)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0], target=target, weight=1000, multi_thread=False
)
ocp.update_objectives(objective_functions)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], target.squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_one_lagrange(cost_type):
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__)
n_shooting = 10
target = np.expand_dims(np.arange(0, n_shooting + 1), axis=0)
target[0, -1] = n_shooting - 2
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=n_shooting,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
key="q",
node=Node.ALL,
weight=10,
index=[0],
target=target,
multi_thread=False,
)
ocp.update_objectives(objective_functions)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_control_lagrange_and_state_mayer(cost_type):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
n_shooting = 10
target = np.array([[2]])
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod",
n_shooting=n_shooting,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_ALL_CONTROLS, )
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE,
index=[0],
target=target,
weight=1000)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], target.squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_several_mayer(cost_type):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/acados/models/cube.bioMod",
n_shooting=10,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
index=[0, 1],
target=np.array([[1.0, 2.0]]).T)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
index=[2],
target=np.array([[3.0]]))
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], 1.0)
np.testing.assert_almost_equal(q[1, -1], 2.0)
np.testing.assert_almost_equal(q[2, -1], 3.0)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_pendulum_min_time_lagrange_constrained(ode_solver):
# Load pendulum_min_time_Lagrange
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model_path = (str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum.bioMod",)
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path[0])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, min_bound=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# ------------- #
with pytest.raises(
RuntimeError,
match="ObjectiveFcn.Lagrange.MINIMIZE_TIME cannot have min_bound. Please either use MAYER or constraint",
):
OptimalControlProgram(biorbd_model, dynamics, 10, 2, objective_functions=objective_functions)
def test_acados_one_end_constraints():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/acados/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_ocp(biorbd_model, final_time, number_shooting_points, x0, use_sx=False, n_threads=8):
# --- Options --- #
# Model path
activation_min, activation_max, activation_init = 0, 1, 0.1
excitation_min, excitation_max, excitation_init = 0, 1, 0.2
# Add objective functions
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# add muscle activation bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
# Control path constraint
u_bounds = BoundsList()
u_bounds.add([excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal())
# Initial guesses
x_init = InitialGuess(
np.tile(np.concatenate((x0, [activation_init] * biorbd_model.nbMuscles())), (number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME,
)
u0 = np.array([excitation_init] * biorbd_model.nbMuscles())
u_init = InitialGuess(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
n_threads=n_threads,
)
def prepare_short_ocp(biorbd_model: biorbd.Model, final_time: float, n_shooting: int):
"""
Prepare to build a blank short ocp to use single shooting bioptim function
Parameters
----------
biorbd_model: biorbd.Model
biorbd model build with the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
Returns
-------
The blank OptimalControlProgram
"""
# Add objective functions
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([0] * biorbd_model.nbMuscles(), [1] * biorbd_model.nbMuscles())
x_init = InitialGuess([0] * biorbd_model.nbQ() * 2)
u_init = InitialGuess([0] * biorbd_model.nbMuscles())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=True,
)
def prepare_ocp(biorbd_model,
final_time,
number_shooting_points,
markers_ref,
tau_ref,
ode_solver=OdeSolver.RK):
# --- Options --- #
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
axis_to_track=[Axe.Y, Axe.Z],
weight=100,
target=markers_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_TORQUE, target=tau_ref)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def test_acados_one_end_constraints():
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"constraint",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(constraint)
ocp = constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=0,
weight=100,
target=np.array([[1]]))
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
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.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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[:, -1],
np.array((-2.72727272, 9.81, 0)),
decimal=6)
