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 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_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 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 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, 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 test_double_update_bounds_and_init():
bioptim_folder = TestUtils.bioptim_folder()
biorbd_model = biorbd.Model(bioptim_folder +
"/examples/track/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0)
x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1)))
u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1)))
ocp.update_bounds(x_bounds, u_bounds)
expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, expected)
expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, expected)
x_init = InitialGuess(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.init.init, expected)
x_bounds = Bounds(-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1)))
u_bounds = Bounds(-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1)))
ocp.update_bounds(x_bounds=x_bounds)
ocp.update_bounds(u_bounds=u_bounds)
expected = np.array([[-2] * (nq * 2) * (ns + 1) + [-4] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, expected)
expected = np.array([[2] * (nq * 2) * (ns + 1) + [4] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, expected)
x_init = InitialGuess(0.25 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-0.25 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.array([[0.25] * (nq * 2) * (ns + 1) + [-0.25] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.init.init, expected)
with pytest.raises(
RuntimeError,
match=
"x_init should be built from a InitialGuess or InitialGuessList"):
ocp.update_initial_guess(x_bounds, u_bounds)
with pytest.raises(
RuntimeError,
match="x_bounds should be built from a Bounds or BoundsList"):
ocp.update_bounds(x_init, u_init)
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 test_update_bounds_and_init_with_param():
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
biorbd_model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/track/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
g_min, g_max, g_init = -10, -6, -8
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(g_min, g_max, interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = Objective(
my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=-8
)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0, parameters=parameters)
x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1)))
u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1)))
ocp.update_bounds(x_bounds, u_bounds)
expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, np.append(expected, [g_min])[:, np.newaxis])
expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, np.append(expected, [g_max])[:, np.newaxis])
x_init = InitialGuess(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.init.init, np.append(expected, [g_init])[:, np.newaxis])
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 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 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_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(
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,
n_shooting: int,
final_time: float,
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
n_shooting: int
The number of shooting points
final_time: float
The time at the final node
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
index=1,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque(
) # biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
# Initial guesses
# TODO put this in a function defined before and explain what it does, and what are the variables
x = np.vstack((np.zeros(
(biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2))))
Arm_init_D = np.zeros((3, 2))
Arm_init_D[1, 0] = 0
Arm_init_D[1, 1] = -np.pi + 0.01
Arm_init_G = np.zeros((3, 2))
Arm_init_G[1, 0] = 0
Arm_init_G[1, 1] = np.pi - 0.01
for i in range(2):
Arm_Quat_D = eul2quat(Arm_init_D[:, i])
Arm_Quat_G = eul2quat(Arm_init_G[:, i])
x[6:9, i] = Arm_Quat_D[1:]
x[12, i] = Arm_Quat_D[0]
x[9:12, i] = Arm_Quat_G[1:]
x[13, i] = Arm_Quat_G[0]
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
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,
ode_solver: OdeSolver = OdeSolver.RK4(),
weight: float = 1,
) -> OptimalControlProgram:
"""
Prepare the optimal control program
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the final time
n_shooting: int
The number of shooting points
ode_solver: OdeSolver
The ode solver to use
weight: float
The weighting of the minimize time objective function
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
# A weight of -1 will maximize time
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weight=weight)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
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
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
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,
n_shooting,
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, 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 = "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,
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_test_ocp(with_muscles=False,
with_contact=False,
with_actuator=False):
bioptim_folder = TestUtils.bioptim_folder()
if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator:
raise RuntimeError(
"With muscles and with contact and with_actuator together is not defined"
)
elif with_muscles:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/muscle_driven_ocp/models/arm26.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles()
elif with_contact:
biorbd_model = biorbd.Model(
bioptim_folder +
"/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod"
)
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True,
expand=False)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
elif with_actuator:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/torque_driven_ocp/models/cube.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
else:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/track/models/cube_and_line.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess(np.zeros((nx, 1)))
u_init = InitialGuess(np.zeros((nu, 1)))
x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1)))
u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1)))
ocp = OptimalControlProgram(biorbd_model,
dynamics,
10,
1.0,
x_init,
u_init,
x_bounds,
u_bounds,
use_sx=True)
ocp.nlp[0].J = [[]]
ocp.nlp[0].g = [[]]
return ocp
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: 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,
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 partial_ocp_parameters(nb_phases):
if nb_phases != 1 and nb_phases != 3:
raise RuntimeError("nb_phases should be 1 or 3")
biorbd_model_path = str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod"
biorbd_model = biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)
number_shooting_points = (2, 2, 2)
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
tau_min, tau_max, tau_init = -100, 100, 0
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
if nb_phases > 1:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
if nb_phases > 1:
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds.min[i, [0, -1]] = 0
bounds.max[i, [0, -1]] = 0
x_bounds[0].min[2, 0] = 0.0
x_bounds[0].max[2, 0] = 0.0
if nb_phases > 1:
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if nb_phases > 1:
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
if nb_phases > 1:
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())
if nb_phases > 1:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
return (
biorbd_model[:nb_phases],
number_shooting_points[:nb_phases],
final_time[:nb_phases],
time_min[:nb_phases],
time_max[:nb_phases],
tau_min,
tau_max,
tau_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
)
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(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,
final_time: float,
n_shooting: int,
use_sx: bool = True) -> 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
use_sx: bool
If the ocp should be built with SX. Please note that ACADOS requires SX
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
data_to_track = np.zeros((n_shooting + 1, n_q + n_qdot))
data_to_track[:, 1] = 3.14
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100.0,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1.0)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
weight=50000.0,
target=data_to_track[-1:, :].T,
node=Node.END,
)
# 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
n_tau = biorbd_model.nbGeneralizedTorque()
torque_min, torque_max, torque_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([torque_min] * n_tau, [torque_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
)
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
excitations_ref: np.ndarray,
q_ref: np.ndarray,
use_residual_torque: bool,
kin_data_to_track: str = "markers",
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
) -> OptimalControlProgram:
"""
Prepare the ocp to solve
Parameters
----------
biorbd_model: biorbd.Model
The loaded biorbd model
final_time: float
The time at final node
n_shooting: int
The number of shooting points
markers_ref: np.ndarray
The marker to track if 'markers' is chosen in kin_data_to_track
excitations_ref: np.ndarray
The muscle excitation (EMG) to track
q_ref: np.ndarray
The state to track if 'q' is chosen in kin_data_to_track
kin_data_to_track: str
The type of kin data to track ('markers' or 'q')
use_residual_torque: bool
If residual torque are present or not in the dynamics
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to solve
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL,
key="muscles",
target=excitations_ref)
if use_residual_torque:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
if kin_data_to_track == "markers":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
node=Node.ALL,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
key="q",
weight=100,
node=Node.ALL,
target=q_ref,
index=range(biorbd_model.nbQ()),
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_excitations=True,
with_residual_torque=use_residual_torque)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
x_bounds[0].min[[0, 1], :] = -2 * np.pi
x_bounds[0].max[[0, 1], :] = 2 * np.pi
# Add muscle to the bounds
activation_min, activation_max, activation_init = 0, 1, 0.5
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()):,
0] = excitations_ref[:, 0]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[0] * biorbd_model.nbMuscles())
# Define control path constraint
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
u_bounds = BoundsList()
u_init = InitialGuessList()
if use_residual_torque:
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscles(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscles(),
)
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscles())
else:
u_bounds.add([excitation_min] * biorbd_model.nbMuscles(),
[excitation_max] * biorbd_model.nbMuscles())
u_init.add([excitation_init] * biorbd_model.nbMuscles())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
