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, number_shooting_points, final_time, loop_from_constraint, ode_solver=OdeSolver.RK):
# --- 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 = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.MID, first_marker_idx=0, second_marker_idx=2)
constraints.add(Constraint.TRACK_STATE, instant=Instant.MID, states_idx=2)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=1)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[2:6, -1] = [1.57, 0, 0, 0]
x_bounds.max[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialConditionsOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption(
[[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]
)
u_init = InitialConditionsOption([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A state transition loop constraint is treated as
# hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or
# as a soft penalty (objective) otherwise
state_transitions = StateTransitionList()
if loop_from_constraint:
state_transitions.add(StateTransition.CYCLIC, weight=0)
else:
state_transitions.add(StateTransition.CYCLIC, weight=10000)
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,
)
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(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(biorbd_model_path, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_align_markers,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[1:6, [0, -1]] = 0
x_bounds.max[1:6, [0, -1]] = 0
x_bounds.min[2, -1] = 1.57
x_bounds.max[2, -1] = 1.57
# Initial guess
x_init = InitialConditionsOption(
[0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialConditionsOption([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,
number_shooting_points,
final_time,
initial_guess=InterpolationType.CONSTANT,
):
# --- 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 = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(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 constraints
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[1:6, [0, -1]] = 0
x_bounds.max[1:6, [0, -1]] = 0
u_bounds = BoundsOption([[tau_min] * ntau, [tau_max] * ntau])
# Initial guesses
t = None
extra_params_x = {}
extra_params_u = {}
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [tau_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, number_shooting_points + 1))
u = np.random.random((ntau, number_shooting_points))
elif initial_guess == InterpolationType.SPLINE:
# Bound spline assume the first and last point are 0 and final respectively
t = np.hstack((0, np.sort(np.random.random(
(3, )) * final_time), final_time))
x = np.random.random((nq + nqdot, 5))
u = np.random.random((ntau, 5))
elif initial_guess == InterpolationType.CUSTOM:
# The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation
x = custom_init_func
u = custom_init_func
extra_params_x = {
"my_values": np.random.random((nq + nqdot, 2)),
"nb_shooting": number_shooting_points
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"nb_shooting": number_shooting_points
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialConditionsOption(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialConditionsOption(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
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="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):
# --- 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,
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
interpolation_type=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
):
# --- 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 = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeOption(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 constraints
if interpolation_type == InterpolationType.CONSTANT:
x_min = [-100] * (nq + nqdot)
x_max = [100] * (nq + nqdot)
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.CONSTANT)
u_min = [tau_min] * ntau
u_max = [tau_max] * ntau
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.CONSTANT)
elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x_min = np.random.random((6, 3)) * (-10) - 5
x_max = np.random.random((6, 3)) * 10 + 5
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
u_min = np.random.random((3, 3)) * tau_min + tau_min / 2
u_max = np.random.random((3, 3)) * tau_max + tau_max / 2
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
elif interpolation_type == InterpolationType.LINEAR:
x_min = np.random.random((6, 2)) * (-10) - 5
x_max = np.random.random((6, 2)) * 10 + 5
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.LINEAR)
u_min = np.random.random((3, 2)) * tau_min + tau_min / 2
u_max = np.random.random((3, 2)) * tau_max + tau_max / 2
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.LINEAR)
elif interpolation_type == InterpolationType.EACH_FRAME:
x_min = np.random.random(
(nq + nqdot, number_shooting_points + 1)) * (-10) - 5
x_max = np.random.random(
(nq + nqdot, number_shooting_points + 1)) * 10 + 5
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.EACH_FRAME)
u_min = np.random.random(
(ntau, number_shooting_points)) * tau_min + tau_min / 2
u_max = np.random.random(
(ntau, number_shooting_points)) * tau_max + tau_max / 2
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.EACH_FRAME)
elif interpolation_type == InterpolationType.SPLINE:
spline_time = np.hstack(
(0, np.sort(np.random.random((3, )) * final_time), final_time))
x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5
x_max = np.random.random((nq + nqdot, 5)) * 10 + 5
u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.SPLINE,
t=spline_time)
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.SPLINE,
t=spline_time)
elif interpolation_type == InterpolationType.CUSTOM:
# The custom functions refer to the ones at the beginning of the file. As for instance, they emulate a Linear interpolation
extra_params_x = {
"n_elements": nq + nqdot,
"nb_shooting": number_shooting_points
}
extra_params_u = {
"n_elements": ntau,
"nb_shooting": number_shooting_points
}
x_bounds = BoundsOption([custom_x_bounds_min, custom_x_bounds_max],
interpolation=InterpolationType.CUSTOM,
**extra_params_x)
u_bounds = BoundsOption([custom_u_bounds_min, custom_u_bounds_max],
interpolation=InterpolationType.CUSTOM,
**extra_params_u)
else:
raise NotImplementedError("Not implemented yet")
# Initial guess
x_init = InitialConditionsOption([0] * (nq + nqdot))
u_init = InitialConditionsOption([tau_init] * ntau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_align_markers_changing_constraints():
# Load align_markers
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_markers",
str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/align_markers.py")
align_markers = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_markers)
ocp = align_markers.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/cube.bioMod",
number_shooting_points=30,
final_time=2,
)
sol = ocp.solve()
# Add a new constraint and reoptimize
new_constraints = ConstraintList()
new_constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.MID,
first_marker_idx=0,
second_marker_idx=2,
idx=2)
ocp.update_constraints(new_constraints)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 20370.211697123825)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (189, 1))
np.testing.assert_almost_equal(g, np.zeros((189, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((1, 0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((4.2641129, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -1],
np.array((1.36088709, 9.81, -2.27903226)))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol, ocp)
# Replace constraints and reoptimize
new_constraints = ConstraintList()
new_constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=2,
idx=0)
new_constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.MID,
first_marker_idx=0,
second_marker_idx=3,
idx=2)
ocp.update_constraints(new_constraints)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 31670.93770220887)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (189, 1))
np.testing.assert_almost_equal(g, np.zeros((189, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((-5.625, 21.06, 2.2790323)))
np.testing.assert_almost_equal(tau[:, -1],
np.array((-5.625, 21.06, -2.27903226)))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol, ocp)
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(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_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,
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="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,
)
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(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(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,
)