def prepare_nmpc(model_path, cycle_len, cycle_duration, n_cycles_simultaneous, n_cycles_to_advance, max_torque):
model = biorbd.Model(model_path)
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
x_bound = QAndQDotBounds(model)
x_bound.min[0, :] = -2 * np.pi * n_cycles_simultaneous # Allow the wheel to spin as much as needed
x_bound.max[0, :] = 0
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(-2 * np.pi * n_cycles_simultaneous, 0, cycle_len * n_cycles_simultaneous + 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_len,
cycle_duration=cycle_duration,
n_cycles_simultaneous=n_cycles_simultaneous,
n_cycles_to_advance=n_cycles_to_advance,
objective_functions=new_objectives,
constraints=constraints,
x_init=x_init,
u_init=u_init,
x_bounds=x_bound,
u_bounds=u_bound,
)
def set_bounds(self):
for p in range(self.n_phases):
self.x_bounds.add(bounds=QAndQDotBounds(self.models[p]))
self.u_bounds.add([self.torque_min] * self.nb_tau +
[self.activation_min] * self.nb_mus,
[self.torque_max] * self.nb_tau +
[self.activation_max] * self.nb_mus)
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 prepare_ocp(biorbd_model_path,
n_shooting,
tf,
ode_solver=OdeSolver.RK4(),
use_sx=True):
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
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,
tf,
x_init,
u_init,
x_bounds,
u_bounds,
ode_solver=ode_solver,
use_sx=use_sx,
)
def prepare_ocp(biorbd_model_path, 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 = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_align_markers,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# 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,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[1, :] = 0
u_init = InitialGuessOption([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=target_g
)
parameters.add(
"gravity_z", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=parameter_objective_functions, # Objective of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
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_CONTROL, key="muscles", target=muscle_activations_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=0.001)
# objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="torque", weight=0.001)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True, with_contact=True)
# 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 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):
# --- 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,
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: str, final_time: float,
n_shooting: int) -> OptimalControlProgram:
"""
The initialization of an ocp
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
final_time: float
The time in second required to perform the task
n_shooting: int
The number of shooting points to define int the direct multiple shooting program
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, muscle_activations_ref, contact_forces_ref):
# Model path
biorbd_model = biorbd.Model(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(Objective.Lagrange.TRACK_MUSCLES_CONTROL, target=muscle_activations_ref)
objective_functions.add(Objective.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.001)
objective_functions.add(Objective.Lagrange.MINIMIZE_ALL_CONTROLS, weight=0.001)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_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,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
)
def _set_bounds(self):
self.x_bounds = QAndQDotBounds(self.model)
self.x_bounds[:self.n_q, 0] = self.violin.q(self.bow_starting)
self.x_bounds[self.n_q:, 0] = 0
# self.x_bounds.min[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) - 0.01
# self.x_bounds.max[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) + 0.01
u_min = [self.tau_min] * self.n_tau + [0] * self.n_mus
u_max = [self.tau_max] * self.n_tau + [1] * self.n_mus
self.u_bounds = Bounds(u_min, u_max)
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 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_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 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 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_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)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points,
time_min, time_max):
# --- 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 = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintOption(Constraint.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min,
max_bound=time_max)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuessOption([tau_init] * n_tau)
# ------------- #
return 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, final_time: float,
n_shooting: int) -> 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
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
torque_min, torque_max, torque_init = -100, 100, 0
n_tau = biorbd_model.nbGeneralizedTorque()
u_bounds = Bounds([torque_min] * n_tau, [torque_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([torque_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
)
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_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_acados_one_end_constraints():
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,
)
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.SUPERIMPOSE_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
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[:, -1],
np.array((-2.72727272, 9.81, 0)),
decimal=6)
def prepare_ocp(
biorbd_model_path, final_time, number_shooting_points, nb_threads, use_SX=False, ode_solver=OdeSolver.RK
):
# --- 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
ode_solver=ode_solver,
)
def _set_boundary_conditions(self):
for i in range(self.n_phases):
# Path constraints
self.x_bounds.add(
bounds=QAndQDotBounds(self.models[i],
q_mapping=self.q_mapping[i],
qdot_mapping=self.qdot_mapping[i]))
self.u_bounds.add([-500] * self.n_tau, [500] * self.n_tau)
# Enforce the initial pose and velocity
self.x_bounds[0][:, 0] = self.initial_states[:, 0]
# Target the final pose (except for translation) and velocity
self.objective_functions.add(
ObjectiveFcn.Mayer.TRACK_STATE,
phase=self.n_phases - 1,
index=range(2, self.n_q + self.n_qdot),
target=self.initial_states[2:, :],
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
x_init,
u_init,
x0,
):
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, idx=0)
objective_functions.add(Objective.Lagrange.ALIGN_SEGMENT_WITH_CUSTOM_RT,
weight=100,
segment_idx=Bow.segment_idx,
rt_idx=violin.rt_on_string,
idx=1)
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, 0] = x0
x_init = InitialGuessOption(x_init,
interpolation=InterpolationType.EACH_FRAME)
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_init = InitialGuessOption(u_init,
interpolation=InterpolationType.EACH_FRAME)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
), x_bounds
def prepare_ocp(biorbd_model_path,
nbs,
tf,
ode_solver=OdeSolver.RK,
use_SX=True):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# 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,
nbs,
tf,
x_init,
u_init,
x_bounds,
u_bounds,
ode_solver=ode_solver,
use_SX=use_SX,
)
def prepare_mhe(biorbd_model, window_len, window_duration, max_torque, x_init,
u_init):
new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
node=Node.ALL,
weight=1000,
list_index=0)
return MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
objective_functions=new_objectives,
x_init=InitialGuess(x_init,
interpolation=InterpolationType.EACH_FRAME),
u_init=InitialGuess(u_init,
interpolation=InterpolationType.EACH_FRAME),
x_bounds=QAndQDotBounds(biorbd_model),
u_bounds=Bounds([-max_torque, 0.0], [max_torque, 0.0]),
n_threads=4,
)
