def test_mayer_neg_two_objectives():
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=1)
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_TIME, phase=0)
objective_functions.add(Objective.Mayer.MINIMIZE_TIME, phase=0)
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
def 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)
# 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].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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 prepare_ocp(biorbd_model, final_time, number_shooting_points, markers_ref,
tau_ref):
# --- 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(Objective.Lagrange.TRACK_MARKERS,
axis_tot_track=[Axe.Y, Axe.Z],
weight=100,
target=markers_ref)
objective_functions.add(Objective.Lagrange.TRACK_TORQUE, target=tau_ref)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = 0
x_bounds[0].max[:, 0] = 0
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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_lagrange2_neg_multiphase_time_constraint():
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(Objective.Lagrange.MINIMIZE_TIME, phase=2)
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1,
phase=2)
constraints.add(Constraint.TIME_CONSTRAINT,
instant=Instant.END,
minimum=time_min[0],
maximum=time_max[0],
phase=2)
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path, 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].min[:, [0, -1]] = 0
x_bounds[0].max[:, [0, -1]] = 0
x_bounds[0].min[n_q - 1, -1] = 3.14
x_bounds[0].max[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialConditionsList()
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].min[n_tau - 1, :] = 0
u_bounds[0].max[n_tau - 1, :] = 0
u_init = InitialConditionsList()
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 prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
use_SX=False):
# --- 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(Objective.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(Objective.Mayer.ALIGN_MARKERS,
first_marker_idx=0,
second_marker_idx=5)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = (0.07, 1.4, 0, 0)
x_bounds[0].max[:, 0] = (0.07, 1.4, 0, 0)
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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,
use_SX=use_SX,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, nb_threads, use_SX=False):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
data_to_track = np.zeros((number_shooting_points + 1, n_q + n_qdot))
data_to_track[:, 1] = 3.14
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100.0)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=1.0)
objective_functions.add(
Objective.Mayer.MINIMIZE_STATE, weight=50000.0, target=data_to_track.T, instant=Instant.END,
)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = 0
x_bounds[0].max[:, 0] = 0
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[[torque_min] * n_tau, [torque_max] * n_tau,]
)
u_bounds[0].min[n_tau - 1, :] = 0
u_bounds[0].max[n_tau - 1, :] = 0
u_init = InitialConditionsList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
)
def prepare_ocp(biorbd_model_path="cube_with_forces.bioMod", ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=2)
# External forces
external_forces = [
np.repeat(
np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], number_shooting_points, axis=2
)
]
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[3:6, [0, -1]] = 0
x_bounds[0].max[3:6, [0, -1]] = 0
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
external_forces=external_forces,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path="cube.bioMod",
ode_solver=OdeSolver.RK,
long_optim=False):
# --- Options --- #
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
if long_optim:
number_shooting_points = (100, 300, 100)
else:
number_shooting_points = (20, 30, 20)
final_time = (2, 5, 4)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(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
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
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="HandSpinner.bioMod"):
end_crank_idx = 0
hand_marker_idx = 18
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Problem parameters
number_shooting_points = 30
final_time = 1.0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
markers_idx=hand_marker_idx)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
first_marker_idx=hand_marker_idx,
second_marker_idx=end_crank_idx,
instant=Instant.ALL)
constraints.add(
Constraint.TRACK_STATE,
instant=Instant.ALL,
states_idx=0,
target=np.linspace(0, 2 * np.pi, number_shooting_points + 1),
)
state_transitions = StateTransitionList()
state_transitions.add(state_transition_function, phase_pre_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Initial guess
x_init = InitialConditionsList()
x_init.add([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [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 = InitialConditionsList()
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,
constraints,
state_transitions=state_transitions,
)
def prepare_ocp(
biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_ekf,
use_residual_torque,
kin_data_to_track,
nb_threads,
use_SX=True,
):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.5
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=0.001,
target=excitations_ref)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
weight=1,
target=marker_ref)
elif kin_data_to_track == "q":
objective_functions.add(Objective.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
states_idx=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Add muscle to the bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
x_bounds[0].min[:, 0] = 0 # state_ekf[:, 0]
x_bounds[0].max[:, 0] = 0 # state_ekf[:, 0]
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[0] * biorbd_model.nbMuscles())
# x_init.add(state_ekf, interpolation=InterpolationType.EACH_FRAME)
# Add muscle to the bounds
u_bounds = BoundsList()
u_init = InitialConditionsList()
init_residual_torque = np.concatenate((np.ones(
(biorbd_model.nbGeneralizedTorque(), n_shooting_points)) * 0.5,
excitations_ref))
if use_residual_torque:
u_bounds.add([
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
# u_init.add([torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(init_residual_torque,
interpolation=InterpolationType.EACH_FRAME)
else:
u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(),
[excitation_max] * biorbd_model.nbMuscleTotal()])
u_init.add([0] * biorbd_model.nbMuscleTotal())
# u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME)
# Get initial isometric forces
fiso = []
for k in range(nb_mus):
fiso.append(
biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# Define the parameter to optimize
bound_p_iso = Bounds(min_bound=np.repeat(0.5, nb_mus + 1),
max_bound=np.repeat(3.5, nb_mus + 1),
interpolation=InterpolationType.CONSTANT)
bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus),
max_bound=np.repeat(0, nb_mus),
interpolation=InterpolationType.CONSTANT)
p_iso_init = InitialConditions(np.repeat(1, nb_mus + 1))
initial_guess_A = InitialConditions([-3] * nb_mus)
parameters = ParameterList()
parameters.add(
"p_iso", # The name of the parameter
modify_isometric_force, # The function that modifies the biorbd model
p_iso_init,
bound_p_iso, # The bounds
size=nb_mus +
1, # The number of elements this particular parameter vector has
fiso_init=fiso,
)
# parameters.add(
# "shape_factor", # The name of the parameter
# modify_shape_factor,
# initial_guess_A,
# bound_shape_factor, # The bounds
# size=nb_mus, # The number of elements this particular parameter vector has
# )
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
# parameters=parameters,
)
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
use_SX=False,
nb_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -5, 5, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=100)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Add muscle to the bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# Following values are taken from Belaise's matlab code
x_bounds[0].min[:,
0] = (-0.2, 0.1, -0.25, 0.1, 0, -0, -0.2, 0.05, -0.15,
-0.02, 0, 0.28) + (0, ) * biorbd_model.nbMuscles()
x_bounds[0].max[:,
0] = (-0.2, 0.1, -0.25, 0.1, 0, -0, -0.2, 0.05, -0.15,
-0.02, 0, 0.28) + (0, ) * biorbd_model.nbMuscles()
x_bounds[0].min[:biorbd_model.nbQ() * 2,
-1] = (-0.03, 0.1, -0.1, 0.2, -0.76, 1., 2., -1.5, -0.17,
-0.62, 1.4, -0.57)
x_bounds[0].max[:biorbd_model.nbQ() * 2,
-1] = (-0.03, 0.1, -0.1, 0.2, -0.76, 1., 2., -1.5, -0.17,
-0.62, 1.4, -0.57)
# Initial guess
x_init = InitialConditionsList()
x_init.add([1.] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot() +
[0] * biorbd_model.nbMuscles())
# Define control path constraint
u_bounds = BoundsList()
u_init = InitialConditionsList()
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())
# ------------- #
return OptimalControlProgram(biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads)
def prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
activations_ref,
q_ref,
kin_data_to_track="markers",
use_residual_torque=True,
use_SX=False,
):
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
nq = biorbd_model.nbQ()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
target=activations_ref)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(Objective.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
states_idx=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
x_bounds[0].min[:nq, :] = -2 * np.pi
x_bounds[0].max[:nq, :] = 2 * np.pi
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_init = InitialConditionsList()
if use_residual_torque:
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.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
else:
u_bounds.add([[activation_min] * biorbd_model.nbMuscleTotal(),
[activation_max] * biorbd_model.nbMuscleTotal()])
u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
nb_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
)
def prepare_ocp(model_path,
phase_time,
number_shooting_points,
use_actuators=False):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1 / 100)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize X_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialConditionsList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
tau_mapping=tau_mapping,
)
def prepare_nlp(biorbd_model_path="../models/Bras.bioMod"):
"""
Mix .bioMod and users data to call OptimalControlProgram constructor.
:param biorbd_model_path: path to the .bioMod file.
:param show_online_optim: bool which active live plot function.
:return: OptimalControlProgram object.
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
muscle_activated_init, muscle_fatigued_init, muscle_resting_init = 0, 0, 1
torque_min, torque_max, torque_init = -10, 10, 0
muscle_states_ratio_min, muscle_states_ratio_max = 0, 1
number_shooting_points = 30
final_time = 0.5
# --- Objective --- #
objective_functions = ObjectiveList()
# objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10)
# objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE_DERIVATIVE,
weight=100)
# objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, controls_idx=[0, 1, 2, 3], weight=2000)
# --- Dynamics --- #
dynamics = DynamicsTypeOption(xia.xia_model_configuration,
dynamic_function=xia.xia_model_dynamic)
# --- Path constraints --- #
X_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
X_bounds[biorbd_model.nbQ():, 0] = 0
X_bounds[biorbd_model.nbQ():, 2] = -1.5
muscle_states_bounds = BoundsOption([
[muscle_states_ratio_min] * biorbd_model.nbMuscleTotal() * 3,
[muscle_states_ratio_max] * biorbd_model.nbMuscleTotal() * 3,
])
muscle_states_bounds.min[:, 0] = (
[muscle_activated_init] * biorbd_model.nbMuscleTotal() +
[muscle_fatigued_init] * biorbd_model.nbMuscleTotal() +
[muscle_resting_init] * biorbd_model.nbMuscleTotal())
muscle_states_bounds.max[:, 0] = (
[muscle_activated_init] * biorbd_model.nbMuscleTotal() +
[muscle_fatigued_init] * biorbd_model.nbMuscleTotal() +
[muscle_resting_init] * biorbd_model.nbMuscleTotal())
X_bounds.bounds.concatenate(muscle_states_bounds.bounds)
U_bounds = BoundsOption([
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_states_ratio_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_states_ratio_max] * biorbd_model.nbMuscleTotal(),
])
# --- Initial guess --- #
X_init = InitialConditionsOption(
[0] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot(),
InterpolationType.CONSTANT,
)
U_init = InitialConditionsOption(
[torque_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_activated_init] * biorbd_model.nbMuscleTotal(),
InterpolationType.CONSTANT,
)
muscle_states_init = InitialConditionsOption(
[muscle_activated_init] * biorbd_model.nbMuscleTotal() +
[muscle_fatigued_init] * biorbd_model.nbMuscleTotal() +
[muscle_resting_init] * biorbd_model.nbMuscleTotal(),
InterpolationType.CONSTANT,
)
X_init.initial_condition.concatenate(muscle_states_init.initial_condition)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions=objective_functions,
nb_threads=4,
)
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,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(Constraint.PROPORTIONAL_STATE,
instant=Instant.ALL,
first_dof=2,
second_dof=3,
coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[4:8, [0, -1]] = 0
x_bounds[0].max[4:8, [0, -1]] = 0
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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,
)
sol = ocp.solve(solver=Solver.ACADOS, solver_options=options_acados)
data_sol = Data.get_data(ocp, sol)
X0, U0, X_out = warm_start_mhe(data_sol)
X_est[:, 0] = X_out
t0 = time.time()
# Reduce ipopt tol for moving estimation
options_ipopt["max_iter"] = 5
options_ipopt["tol"] = 1e-1
# TODO: The following loop should be move in a MHE module that yields after iteration so the user can change obj
for i in range(1, N - N_mhe):
Y_i = Y_N_[:, :, i:i + N_mhe + 1]
new_objectives = ObjectiveList()
new_objectives.add(Objective.Lagrange.MINIMIZE_MARKERS,
weight=1000,
target=Y_i,
idx=0)
new_objectives.add(Objective.Lagrange.MINIMIZE_STATE,
weight=1000,
target=X0,
phase=0,
idx=1)
ocp.update_objectives(new_objectives)
# sol = ocp.solve(solver_options=options_ipopt)
sol = ocp.solve(solver=Solver.ACADOS, solver_options=options_acados)
data_sol = Data.get_data(ocp, sol, concatenate=False)
X0, U0, X_out = warm_start_mhe(data_sol)
X_est[:, i] = X_out
t1 = time.time()
print(f"Window size of MHE : {Tf_mhe} s.")
def prepare_ocp(biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_init,
use_residual_torque,
activation_driven,
kin_data_to_track,
nb_threads,
use_SX=True,
param=False):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.3
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# -- Force iso ipopt pour acados
# if param is not True:
# fiso = []
# for k in range(nb_mus):
# fiso.append(biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# mat_content = sio.loadmat("./results/param_f_iso_flex.mat")
# f_iso = mat_content["f_iso"] * mat_content["f_iso_global"]
# for k in range(biorbd_model.nbMuscles()):
# biorbd_model.muscle(k).characteristics().setForceIsoMax(
# f_iso[k] * fiso[k]
# )
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=10,
target=excitations_ref)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.01)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=1)
if kin_data_to_track == "markers":
objective_functions.add(
Objective.Lagrange.TRACK_MARKERS,
weight=1000,
target=marker_ref[:, -biorbd_model.nbMarkers():, :])
elif kin_data_to_track == "q":
objective_functions.add(
Objective.Lagrange.TRACK_STATE,
weight=100,
# target=q_ref,
# states_idx=range(biorbd_model.nbQ())
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif activation_driven:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Constraints
constraints = ()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
if use_SX:
x_bounds[0].min[:, 0] = np.concatenate(
(state_init[6:biorbd_model.nbQ() + 6,
0], state_init[biorbd_model.nbQ() + 12:-nb_mus,
0]))
x_bounds[0].max[:, 0] = np.concatenate(
(state_init[6:biorbd_model.nbQ() + 6,
0], state_init[biorbd_model.nbQ() + 12:-nb_mus,
0]))
# Add muscle to the bounds
if activation_driven is not True:
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# Initial guess
x_init = InitialConditionsList()
if activation_driven:
# state_base = np.ndarray((12, n_shooting_points+1))
# for i in range(n_shooting_points+1):
# state_base[:, i] = np.concatenate((state_init[:6, 0], np.zeros((6))))
x_init.add(np.concatenate(
(state_init[6:biorbd_model.nbQ() + 6, :],
state_init[biorbd_model.nbQ() + 12:-nb_mus, :])),
interpolation=InterpolationType.EACH_FRAME)
# x_init.add(state_init[:-nb_mus, :], interpolation=InterpolationType.EACH_FRAME)
else:
# x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())
x_init.add(state_init[biorbd_model.nbQ():, :],
interpolation=InterpolationType.EACH_FRAME)
# Add muscle to the bounds
u_bounds = BoundsList()
u_init = InitialConditionsList()
nb_tau = 6
# init_residual_torque = np.concatenate((np.ones((biorbd_model.nbGeneralizedTorque(), n_shooting_points))*0.5,
# excitations_ref))
if use_residual_torque:
u_bounds.add([
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
u_init.add([torque_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscleTotal())
# u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME)
else:
u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(),
[excitation_max] * biorbd_model.nbMuscleTotal()])
if activation_driven:
# u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref,
interpolation=InterpolationType.EACH_FRAME)
else:
# u_init.add([excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref,
interpolation=InterpolationType.EACH_FRAME)
# Get initial isometric forces
fiso = []
for k in range(nb_mus):
fiso.append(
biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# Define the parameter to optimize
bound_p_iso = Bounds(
# min_bound=np.repeat(0.75, nb_mus), max_bound=np.repeat(3, nb_mus), interpolation=InterpolationType.CONSTANT)
min_bound=[0.5] * nb_mus + [0.75],
max_bound=[3.5] * nb_mus + [3],
interpolation=InterpolationType.CONSTANT)
bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus),
max_bound=np.repeat(0, nb_mus),
interpolation=InterpolationType.CONSTANT)
p_iso_init = InitialConditions([1] * nb_mus + [2])
initial_guess_A = InitialConditions([-3] * nb_mus)
parameters = ParameterList()
parameters.add(
"p_iso", # The name of the parameter
modify_isometric_force, # The function that modifies the biorbd model
p_iso_init,
bound_p_iso, # The bounds
size=nb_mus +
1, # The number of elements this particular parameter vector has
fiso_init=fiso,
)
# parameters.add(
# "shape_factor", # The name of the parameter
# modify_shape_factor,
# initial_guess_A,
# bound_shape_factor, # The bounds
# size=nb_mus, # The number of elements this particular parameter vector has
# )
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
# parameters=parameters
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
max_torque,
X0,
U0,
target=None,
interpolation=InterpolationType.EACH_FRAME,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -max_torque, max_torque, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS,
weight=1000,
target=target)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=1000,
target=X0)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = -np.inf
x_bounds[0].max[:, 0] = np.inf
# X_bounds[0].min[:biorbd_model.nbQ(), 0] = X0[:biorbd_model.nbQ(),0]
# X_bounds[0].max[:biorbd_model.nbQ(), 0] = X0[:biorbd_model.nbQ(),0]
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min, 0.0], [tau_max, 0.0]])
# Initial guesses
x = X0
u = U0
x_init = InitialConditionsList()
x_init.add(x, interpolation=interpolation)
u_init = InitialConditionsList()
u_init.add(u, interpolation=interpolation)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=4,
use_SX=True,
)
def prepare_ocp(biorbd_model_path,
number_shooting_points,
final_time,
use_actuators=False,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[3:6, [0, -1]] = 0
x_bounds[0].max[3:6, [0, -1]] = 0
x_bounds[0].min[2, [0, -1]] = [0, 1.57]
x_bounds[0].max[2, [0, -1]] = [0, 1.57]
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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,
final_time,
number_shooting_points,
marker_velocity_or_displacement,
marker_in_first_coordinates_system,
control_type,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
biorbd_model.markerNames()
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
if marker_in_first_coordinates_system:
coordinates_system_idx = 0
else:
coordinates_system_idx = -1
objective_functions = ObjectiveList()
if marker_velocity_or_displacement == "disp":
objective_functions.add(
Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
coordinates_system_idx=coordinates_system_idx,
markers_idx=6,
weight=1000,
)
elif marker_velocity_or_displacement == "velo":
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_VELOCITY,
markers_idx=6,
weight=1000)
else:
raise RuntimeError(
"Wrong choice of marker_velocity_or_displacement, actual value is "
"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.")
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
states_idx=6,
weight=-1)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
states_idx=7,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
for i in range(nq, 2 * nq):
x_bounds[0].min[i, :] = -10
x_bounds[0].max[i, :] = 10
# Initial guess
x_init = InitialConditionsList()
x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6])
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialConditionsList()
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,
nb_integration_steps=5,
control_type=control_type,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points,
initialize_near_solution):
# --- 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(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=4)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=5)
constraints.add(Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS,
instant=Instant.ALL,
marker_idx=1,
segment_idx=2,
axis=(Axe.X))
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0].min[i, [0, -1]] = 0
x_bounds[0].max[i, [0, -1]] = 0
x_bounds[0].min[2, -1] = 1.57
x_bounds[0].max[2, -1] = 1.57
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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,
)
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = nqdot # biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_MARKERS,
markers_idx=1,
weight=-1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * ntau, [tau_max] * ntau])
# Initial guesses
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 = InitialConditionsList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
u_init = InitialConditionsList()
u_init.add([tau_init] * ntau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
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
all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]),
Mapping([0, 1, 2]))
# 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,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model, all_generalized_mapping))
x_bounds[0].min[3:6, [0, -1]] = 0
x_bounds[0].max[3:6, [0, -1]] = 0
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * all_generalized_mapping.reduce.len * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * all_generalized_mapping.reduce.len,
[tau_max] * all_generalized_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_init] * all_generalized_mapping.reduce.len)
# ------------- #
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,
all_generalized_mapping=all_generalized_mapping,
)
def prepare_ocp(
biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_init,
use_residual_torque,
activation_driven,
kin_data_to_track,
nb_threads,
use_SX=True,
):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.3
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, weight=10, target=excitations_ref)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.01)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1)
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS, weight=1000,
target=marker_ref[:, -biorbd_model.nbMarkers():, :]
)
elif kin_data_to_track == "q":
objective_functions.add(
Objective.Lagrange.TRACK_STATE, weight=100,
# target=q_ref,
# states_idx=range(biorbd_model.nbQ())
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif activation_driven:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Constraints
constraints = ()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
if use_SX:
x_bounds[0].min[:, 0] = np.concatenate((state_init[6:biorbd_model.nbQ() + 6, 0],
state_init[biorbd_model.nbQ() + 12:-nb_mus, 0]))
x_bounds[0].max[:, 0] = np.concatenate((state_init[6:biorbd_model.nbQ() + 6, 0],
state_init[biorbd_model.nbQ() + 12:-nb_mus, 0]))
# Add muscle to the bounds
if activation_driven is not True:
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
# Initial guess
x_init = InitialConditionsList()
if activation_driven:
# state_base = np.ndarray((12, n_shooting_points+1))
# for i in range(n_shooting_points+1):
# state_base[:, i] = np.concatenate((state_init[:6, 0], np.zeros((6))))
x_init.add(np.concatenate((state_init[6:biorbd_model.nbQ() + 6, :],
state_init[biorbd_model.nbQ() + 12:-nb_mus, :])),
interpolation=InterpolationType.EACH_FRAME)
# x_init.add(state_init[:-nb_mus, :], interpolation=InterpolationType.EACH_FRAME)
else:
# x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())
x_init.add(state_init[biorbd_model.nbQ():, :], interpolation=InterpolationType.EACH_FRAME)
# Add muscle to the bounds
u_bounds = BoundsList()
u_init = InitialConditionsList()
nb_tau = 6
# init_residual_torque = np.concatenate((np.ones((biorbd_model.nbGeneralizedTorque(), n_shooting_points))*0.5,
# excitations_ref))
if use_residual_torque:
u_bounds.add(
[
[torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(),
]
)
u_init.add(
[torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal())
# u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME)
else:
u_bounds.add(
[[excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()])
if activation_driven:
# u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME)
else:
# u_init.add([excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME)
# Get initial isometric forces
fiso = []
for k in range(nb_mus):
fiso.append(biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
# parameters=parameters
)
def prepare_ocp(biorbd_model_path="../models/BrasViolon.bioMod"):
"""
Mix .bioMod and users data to call OptimalControlProgram constructor.
:param biorbd_model_path: path to the .bioMod file.
:param show_online_optim: bool which active live plot function.
:return: OptimalControlProgram object.
"""
optimal_initial_values = False
nb_phases = 2
biorbd_model = []
objective_functions = ObjectiveList()
dynamics = DynamicsTypeList()
constraints = ConstraintList()
x_bounds = BoundsList()
u_bounds = BoundsList()
x_init = InitialConditionsList()
u_init = InitialConditionsList()
# --- Options --- #
number_shooting_points = [20] * nb_phases
final_time = [1] * nb_phases
muscle_activated_init, muscle_fatigued_init, muscle_resting_init = 0, 0, 1
torque_min, torque_max, torque_init = -10, 10, 0
muscle_states_min, muscle_states_max = 0, 1
# --- Aliasing --- #
model_tp = biorbd.Model(biorbd_model_path)
n_q = model_tp.nbQ()
n_qdot = model_tp.nbQdot()
n_tau = model_tp.nbGeneralizedTorque()
n_mus = model_tp.nbMuscles()
violon_string = Violin("G")
inital_bow_side = Bow("frog")
# --- External forces --- #
external_forces = [
np.repeat(violon_string.external_force[:, np.newaxis], number_shooting_points[0], axis=1)
] * nb_phases
for idx_phase in range(nb_phases):
biorbd_model.append(biorbd.Model(biorbd_model_path))
# --- Objective --- #
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=1, phase=idx_phase)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1, phase=idx_phase)
objective_functions.add(
Objective.Lagrange.ALIGN_SEGMENT_WITH_CUSTOM_RT,
weight=1,
segment_idx=Bow.segment_idx,
rt_idx=violon_string.rt_on_string,
phase=idx_phase,
)
objective_functions.add(
Objective.Lagrange.MINIMIZE_TORQUE, controls_idx=[0, 1, 2, 3], weight=10, phase=idx_phase
)
# --- Dynamics --- #
dynamics.add(xia.xia_model_configuration, dynamic_function=xia.xia_model_dynamic)
# --- Constraints --- #
if idx_phase == 0:
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.START,
min_bound=-0.00001,
max_bound=0.00001,
first_marker_idx=Bow.frog_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
)
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.MID,
min_bound=-0.00001,
max_bound=0.00001,
first_marker_idx=Bow.tip_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
)
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.END,
min_bound=-0.00001,
max_bound=0.00001,
first_marker_idx=Bow.frog_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
)
# constraints.add(
# Constraint.ALIGN_SEGMENT_WITH_CUSTOM_RT,
# instant=Instant.ALL,
# min_bound=-0.00001,
# max_bound=0.00001,
# segment_idx=Bow.segment_idx,
# rt_idx=violon_string.rt_on_string,
# phase=idx_phase,
# )
# constraints.add(
# Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS,
# instant=Instant.ALL,
# min_bound=-0.00001,
# max_bound=0.00001,
# marker_idx=violon_string.bridge_marker,
# segment_idx=Bow.segment_idx,
# axis=(Axe.Y),
# phase=idx_phase,
# )
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.ALL,
first_marker_idx=Bow.contact_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
# TODO: add constraint about velocity in a marker of bow (start and end instant)
)
# --- Path constraints --- #
x_bounds.add(QAndQDotBounds(biorbd_model[0]), phase=idx_phase)
# Start and finish with zero velocity
if idx_phase == 0:
x_bounds[idx_phase][n_q :, 0] = 0
if idx_phase == nb_phases - 1:
x_bounds[idx_phase][n_q :, -1] = 0
muscle_states_bounds = Bounds(
[muscle_states_min] * n_mus * 3,
[muscle_states_max] * n_mus * 3,
)
if idx_phase == 0:
# fatigued_fibers = activated_fibers = 0 and resting_fibers = 1 at start
muscle_states_bounds[:2 * n_mus, 0] = 0
muscle_states_bounds[2 * n_mus:, 0] = 1
x_bounds[idx_phase].concatenate(muscle_states_bounds)
u_bounds.add(
[[torque_min] * n_tau + [muscle_states_min] * n_mus, [torque_max] * n_tau + [muscle_states_max] * n_mus],
phase=idx_phase,
)
# --- Initial guess --- #
if optimal_initial_values:
# TODO: Fix this part (avoid using .bio)
raise NotImplementedError("optimal_initial_values = True should be reviewed")
if idx_phase == 0:
with open(f"utils/optimal_init_{number_shooting_points[0]}_nodes_constr_first.bio", "rb") as file:
dict = pickle.load(file)
else:
with open(f"utils/optimal_init_{number_shooting_points[0]}_nodes_constr_first.bio", "rb") as file:
dict = pickle.load(file)
x_init.add(dict["states"], interpolation=InterpolationType.EACH_FRAME, phase=idx_phase)
u_init.add(dict["controls"], interpolation=InterpolationType.EACH_FRAME, phase=idx_phase)
else:
# TODO: x_init could be a LINEAR from frog to tip
x_init.add(
violon_string.initial_position()[inital_bow_side.side] + [0] * n_qdot,
interpolation=InterpolationType.CONSTANT,
phase=idx_phase,
)
muscle_states_init = InitialConditions(
[muscle_activated_init] * n_mus + [muscle_fatigued_init] * n_mus + [muscle_resting_init] * n_mus,
interpolation=InterpolationType.CONSTANT,
)
x_init[idx_phase].concatenate(muscle_states_init)
u_init.add(
[torque_init] * n_tau + [muscle_activated_init] * n_mus,
interpolation=InterpolationType.CONSTANT,
phase=idx_phase,
)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
external_forces=external_forces,
ode_solver=OdeSolver.RK,
nb_threads=4,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, direction,
boundary):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_ini = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE_INEQUALITY,
direction=direction,
instant=Instant.ALL,
contact_force_idx=1,
boundary=boundary,
)
constraints.add(
Constraint.CONTACT_FORCE_INEQUALITY,
direction=direction,
instant=Instant.ALL,
contact_force_idx=2,
boundary=boundary,
)
# 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].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialConditionsList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_ini] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(model_path,
phase_time,
number_shooting_points,
use_symmetry=True,
use_actuators=True):
# --- Options --- #
# Model path
biorbd_model = [biorbd.Model(elt) for elt in model_path]
nb_phases = len(biorbd_model)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
if use_symmetry:
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]),
Mapping([0, 1, 2, 4, 7, 8, 9]))
q_mapping = q_mapping, q_mapping
tau_mapping = BidirectionalMapping(
Mapping([-1, -1, -1, -1, 0, -1, 0, 1, 2, 3, 1, 2, 3], [5]),
Mapping([4, 7, 8, 9]))
tau_mapping = tau_mapping, tau_mapping
else:
q_mapping = BidirectionalMapping(
Mapping([i for i in range(biorbd_model[0].nbQ())]),
Mapping([i for i in range(biorbd_model[0].nbQ())]))
q_mapping = q_mapping, q_mapping
tau_mapping = q_mapping
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1,
phase=1)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# --- Constraints --- #
constraints = ConstraintList()
# Positivity constraints of the normal component of the reaction forces
contact_axes = (1, 2, 4, 5)
for i in contact_axes:
constraints.add(Constraint.CONTACT_FORCE_INEQUALITY,
phase=0,
direction="GREATER_THAN",
instant=Instant.ALL,
contact_force_idx=i,
boundary=0)
contact_axes = (1, 3)
for i in contact_axes:
constraints.add(Constraint.CONTACT_FORCE_INEQUALITY,
phase=1,
direction="GREATER_THAN",
instant=Instant.ALL,
contact_force_idx=i,
boundary=0)
# Non-slipping constraints
# N.B.: Application on only one of the two feet is sufficient, as the slippage cannot occurs on only one foot.
constraints.add(Constraint.NON_SLIPPING,
phase=0,
instant=Instant.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5)
constraints.add(Constraint.NON_SLIPPING,
phase=1,
instant=Instant.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.5)
# Custom constraints for contact forces at transitions
# constraints_second_phase.append(
# {"type": Constraint.CUSTOM, "function": from_2contacts_to_1, "instant": Instant.START}
# )
# Custom constraints for positivity of CoM_dot on z axis just before the take-off
constraints.add(CoM_dot_Z_last_node_positivity,
phase=1,
instant=Instant.END,
min_bound=0,
max_bound=np.inf)
# TODO: Make it works also with no symmetry (adapt to refactor)
# if not use_symmetry:
# first_dof = (3, 4, 7, 8, 9)
# second_dof = (5, 6, 10, 11, 12)
# coef = (-1, 1, 1, 1, 1)
# for i in range(len(first_dof)):
# for elt in [
# constraints_first_phase,
# constraints_second_phase,
# ]:
# elt.append(
# {
# "type": Constraint.PROPORTIONAL_STATE,
# "instant": Instant.ALL,
# "first_dof": first_dof[i],
# "second_dof": second_dof[i],
# "coef": coef[i],
# }
# )
# constraints = (
# constraints_first_phase,
# constraints_second_phase,
# )
# # Time constraint
# for i in range(nb_phases):
# constraints.add(Constraint.TIME_CONSTRAINT, phase=i, minimum=time_min[i], maximum=time_max[i])
# --- Path constraints --- #
if use_symmetry:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47]
else:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [
0, 0, -0.5336, 0, 1.4, 0, 1.4, 0.8, -0.9, 0.47, 0.8, -0.9, 0.47
]
# Initialize x_bounds (Interpolation type is CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
x_bounds = BoundsList()
for i in range(nb_phases):
x_bounds.add(
QAndQDotBounds(biorbd_model[i],
all_generalized_mapping=q_mapping[i]))
x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# # Initial guess for states (Interpolation type is CONSTANT)
# x_init = InitialConditionsList()
# for i in range(nb_phases):
# x_init.add(pose_at_first_node + [0] * nb_qdot)
# Initial guess for states (Interpolation type is CONSTANT for 1st phase and SPLINE with 3 key positions for 2nd phase)
x_init = InitialConditionsList()
x_init.add(pose_at_first_node +
[0] * nb_qdot) # x_init phase 0 type CONSTANT
t_spline = np.hstack((0, 0.34, phase_time[1]))
p0 = np.array([pose_at_first_node + [0] * nb_qdot]).T
p_flex = np.array(
[[-0.12, -0.23, -1.10, 1.85, 2.06, -1.67, 0.55, 0, 0, 0, 0, 0, 0,
0]]).T
p_end = p0
key_positions = np.hstack((p0, p_flex, p_end))
x_init.add(
key_positions, t=t_spline,
interpolation=InterpolationType.SPLINE) # x_init phase 1 type SPLINE
# Define control path constraint
u_bounds = BoundsList()
for tau_m in tau_mapping:
u_bounds.add(
[[tau_min] * tau_m.reduce.len, [tau_max] * tau_m.reduce.len],
interpolation=InterpolationType.CONSTANT
) # This precision of the CONSTANT type is for informative purposes only
u_init = InitialConditionsList()
for tau_m in tau_mapping:
u_init.add(
[tau_init] *
tau_m.reduce.len) # Interpolation type is CONSTANT (default value)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
nb_threads=2,
)
def prepare_ocp(
final_time, time_min, time_max, number_shooting_points, biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK
):
# --- Options --- #
nb_phases = len(number_shooting_points)
if nb_phases != 1 and nb_phases != 3:
raise RuntimeError("Number of phases must be 1 to 3")
# 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(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0)
if nb_phases == 3:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=0)
if nb_phases == 3:
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.START, first_marker_idx=0, second_marker_idx=1, phase=0)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=2, phase=0)
constraints.add(Constraint.TIME_CONSTRAINT, instant=Instant.END, minimum=time_min[0], maximum=time_max[0], phase=0)
if nb_phases == 3:
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=1, phase=1)
constraints.add(
Constraint.TIME_CONSTRAINT, instant=Instant.END, minimum=time_min[1], maximum=time_max[1], phase=1
)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=2, phase=2)
constraints.add(
Constraint.TIME_CONSTRAINT, instant=Instant.END, minimum=time_min[2], maximum=time_max[2], phase=2
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 0
if nb_phases == 3:
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 2
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 == 3:
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if nb_phases == 3:
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()])
if nb_phases == 3:
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 = InitialConditionsList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
if nb_phases == 3:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model[:nb_phases],
dynamics,
number_shooting_points,
final_time[:nb_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK):
# --- Options --- #
# 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
number_shooting_points = (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(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=3)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=1,
phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds[0].min[[1, 3, 4, 5], 0] = 0
x_bounds[0].max[[1, 3, 4, 5], 0] = 0
x_bounds[-1].min[[1, 3, 4, 5], -1] = 0
x_bounds[-1].max[[1, 3, 4, 5], -1] = 0
x_bounds[0].min[2, 0] = 0.0
x_bounds[0].max[2, 0] = 0.0
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialConditionsList()
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 = InitialConditionsList()
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 state 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) state 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 state transition.
Finally, if you want a state transition (continuous or not) between the last and the first phase (cyclicity)
you can use the dedicated StateTransition.Cyclic or use a continuous set at the lase phase_pre_idx.
If for some reason, you don't want the state 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.
"""
state_transitions = StateTransitionList()
state_transitions.add(StateTransition.IMPACT, phase_pre_idx=1)
state_transitions.add(custom_state_transition,
phase_pre_idx=2,
idx_1=1,
idx_2=3)
state_transitions.add(StateTransition.CYCLIC)
# ------------- #
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,
state_transitions=state_transitions,
)