def prepare_ocp(model_path, phase_time, number_shooting_points):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = (
{
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 1 / 100
},
)
# Dynamics
problem_type = ProblemType.torque_driven_with_contact
# Constraints
constraints = ()
# 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 = QAndQDotBounds(biorbd_model)
X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot
X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len,
max_bound=[torque_max] * tau_mapping.reduce.len)
U_init = InitialConditions([torque_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
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, final_time, number_shooting_points):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -1, 1, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = (
{"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1},
{"type": Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, "weight": 1},
{"type": Objective.Mayer.ALIGN_MARKERS, "first_marker_idx": 0, "second_marker_idx": 5, "weight": 1,},
)
# Dynamics
problem_type = ProblemType.muscle_activations_and_torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
# Set the initial position
X_bounds.min[:, 0] = (0.07, 1.4, 0, 0)
X_bounds.max[:, 0] = (0.07, 1.4, 0, 0)
# Initial guess
X_init = InitialConditions([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(),
)
U_init = InitialConditions(
[torque_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()
)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
)
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_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,
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):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -1000000, 1000000, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, 0] = 0
X_bounds.max[:, 0] = 0
X_bounds.min[:n_q, -1] = 1
X_bounds.max[:n_q, -1] = 1
X_bounds.min[n_q:, -1] = 0
X_bounds.max[n_q:, -1] = 0
# Initial guess
X_init = InitialConditions([0] * (n_q + n_qdot))
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * n_tau,
max_bound=[torque_max] * n_tau)
# Control initial guess
U_init = InitialConditions([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
nb_threads=nb_threads,
)
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, final_time, number_shooting_points,
time_min, time_max):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintOption(Constraint.TIME_CONSTRAINT,
instant=Instant.END,
minimum=time_min,
maximum=time_max)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[:, [0, -1]] = 0
x_bounds.max[:, [0, -1]] = 0
x_bounds.min[n_q - 1, -1] = 3.14
x_bounds.max[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialConditionsOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds.min[n_tau - 1, :] = 0
u_bounds.max[n_tau - 1, :] = 0
u_init = InitialConditionsOption([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path, 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, 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 prepare_ocp(biorbd_model_path="cubeSym.bioMod", show_online_optim=False, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
torque_min, torque_max, torque_init = -100, 100, 0
all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]), Mapping([0, 1, 2]))
# Add objective functions
objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100}
# Dynamics
variable_type = ProblemType.torque_driven
# Constraints
constraints = (
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker": 0, "second_marker": 1,},
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2,},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model, all_generalized_mapping)
for i in range(3, 6):
X_bounds.first_node_min[i] = 0
X_bounds.last_node_min[i] = 0
X_bounds.first_node_max[i] = 0
X_bounds.last_node_max[i] = 0
# Initial guess
X_init = InitialConditions([0] * all_generalized_mapping.reduce.len * 2)
# Define control path constraint
U_bounds = Bounds(
[torque_min] * all_generalized_mapping.reduce.len, [torque_max] * all_generalized_mapping.reduce.len,
)
U_init = InitialConditions([torque_init] * all_generalized_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
show_online_optim=show_online_optim,
)
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:
torque_min, torque_max, torque_init = -1, 1, 0
else:
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100}
# Dynamics
if use_actuators:
problem_type = ProblemType.torque_activations_driven
else:
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker_idx": 0, "second_marker_idx": 1,},
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker_idx": 0, "second_marker_idx": 2,},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[3:6, [0, -1]] = 0
X_bounds.max[3:6, [0, -1]] = 0
X_bounds.min[2, [0, -1]] = [0, 1.57]
X_bounds.max[2, [0, -1]] = [0, 1.57]
# Initial guess
X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(),
)
U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
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,
time_min, time_max):
# --- 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()
# Add objective functions
objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = ({
"type": Constraint.TIME_CONSTRAINT,
"minimum": time_min,
"maximum": time_max,
}, )
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, [0, -1]] = 0
X_bounds.max[:, [0, -1]] = 0
X_bounds.min[n_q - 1, -1] = 3.14
X_bounds.max[n_q - 1, -1] = 3.14
# Initial guess
X_init = InitialConditions([0] * (n_q + n_qdot))
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * n_tau,
max_bound=[torque_max] * n_tau)
U_bounds.min[n_tau - 1, :] = 0
U_bounds.max[n_tau - 1, :] = 0
U_init = InitialConditions([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
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)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[:, [0, -1]] = 0
x_bounds.max[:, [0, -1]] = 0
x_bounds.min[1, -1] = 3.14
x_bounds.max[1, -1] = 3.14
# Initial guess
x_init = InitialConditionsOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[torque_min] * n_tau, [torque_max] * n_tau])
u_bounds.min[n_tau - 1, :] = 0
u_bounds.max[n_tau - 1, :] = 0
u_init = InitialConditionsOption([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
)
def prepare_ocp(biorbd_model_path="cube.bioMod",
show_online_optim=False,
ode_solver=OdeSolver.RK,
long_optim=False):
# --- Options --- #
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
if long_optim:
number_shooting_points = (100, 1000)
else:
number_shooting_points = (20, 30)
final_time = (2, 5)
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = (
({
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}, ),
({
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}, ),
)
# Dynamics
variable_type = (ProblemType.torque_driven, ProblemType.torque_driven)
# Constraints
constraints = (
(
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker": 0,
"second_marker": 1,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker": 0,
"second_marker": 2,
},
),
({
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker": 0,
"second_marker": 1,
}, ),
)
# Path constraint
X_bounds = [
QAndQDotBounds(biorbd_model[0]),
QAndQDotBounds(biorbd_model[0])
]
for bounds in X_bounds:
for i in range(6):
if i != 0 and i != 2:
bounds.first_node_min[i] = 0
bounds.last_node_min[i] = 0
bounds.first_node_max[i] = 0
bounds.last_node_max[i] = 0
X_bounds[0].first_node_min[2] = 0.0
X_bounds[0].first_node_max[2] = 0.0
X_bounds[1].first_node_min[2] = 0.0
X_bounds[1].first_node_max[2] = 0.0
X_bounds[1].last_node_min[2] = 1.57
X_bounds[1].last_node_max[2] = 1.57
# Initial guess
X_init = InitialConditions(
[0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
U_bounds = [
Bounds(
[torque_min] * biorbd_model[0].nbGeneralizedTorque(),
[torque_max] * biorbd_model[0].nbGeneralizedTorque(),
),
Bounds(
[torque_min] * biorbd_model[0].nbGeneralizedTorque(),
[torque_max] * biorbd_model[0].nbGeneralizedTorque(),
),
]
U_init = InitialConditions([torque_init] *
biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
objective_functions,
(X_init, X_init),
(U_init, U_init),
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
show_online_optim=show_online_optim,
)
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,
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(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="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="HandSpinner.bioMod"):
end_crank_idx = 0
hand_marker_idx = 18
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Problem parameters
number_shooting_points = 30
final_time = 1.5
# Add objective functions
objective_functions = (
{
"type": Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
"weight": 1,
"markers_idx": hand_marker_idx
},
{
"type": Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
"weight": 1
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 1
},
)
# Dynamics
problem_type = ProblemType.muscle_activations_and_torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"first_marker_idx": hand_marker_idx,
"second_marker_idx": end_crank_idx,
"instant": Instant.ALL,
},
{
"type": Constraint.TRACK_STATE,
"instant": Instant.ALL,
"states_idx": 0,
"data_to_track": np.linspace(0, 2 * np.pi,
number_shooting_points + 1),
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
# Initial guess
X_init = InitialConditions([0, -0.9, 1.7, 0.9, 2.0, -1.3] +
[0] * biorbd_model.nbQdot())
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
is_cyclic_objective=True,
)
def prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
excitations_ref,
q_ref,
kin_data_to_track="markers",
show_online_optim=False,
):
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 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 = [
{
"type": Objective.Lagrange.TRACK_MUSCLES_CONTROL,
"weight": 1,
"data_to_track": excitations_ref
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 10000
},
]
if kin_data_to_track == "markers":
objective_functions.append(
{
"type": Objective.Lagrange.TRACK_MARKERS,
"weight": 100,
"data_to_track": markers_ref
}, )
elif kin_data_to_track == "q":
objective_functions.append(
{
"type": Objective.Lagrange.TRACK_STATE,
"weight": 100,
"data_to_track": q_ref,
"states_idx": range(biorbd_model.nbQ()),
}, )
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
variable_type = ProblemType.muscle_excitations_and_torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.first_node_min += [activation_min] * biorbd_model.nbMuscleTotal()
X_bounds.first_node_max += [activation_max] * biorbd_model.nbMuscleTotal()
X_bounds.min += [activation_min] * biorbd_model.nbMuscleTotal()
X_bounds.max += [activation_max] * biorbd_model.nbMuscleTotal()
X_bounds.last_node_min += [activation_min] * biorbd_model.nbMuscleTotal()
X_bounds.last_node_max += [activation_max] * biorbd_model.nbMuscleTotal()
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[activation_init] *
biorbd_model.nbMuscleTotal())
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscleTotal(),
)
U_init = InitialConditions(
[torque_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
nb_shooting,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
show_online_optim=show_online_optim,
)
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,
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, final_time, number_shooting_points,
ode_solver, initialize_near_solution):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker_idx": 0,
"second_marker_idx": 4,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker_idx": 0,
"second_marker_idx": 5,
},
{
"type": Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS,
"instant": Instant.ALL,
"marker_idx": 1,
"segment_idx": 2,
"axis": (Axe.X),
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
for i in range(1, 8):
if i != 3:
X_bounds.min[i, [0, -1]] = 0
X_bounds.max[i, [0, -1]] = 0
X_bounds.min[2, -1] = 1.57
X_bounds.max[2, -1] = 1.57
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
X_init.init[i] = 1.5
for i in range(4, 6):
X_init.init[i] = 0.7
for i in range(6, 8):
X_init.init[i] = 0.6
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque(),
[torque_max] * biorbd_model.nbGeneralizedTorque(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
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,
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_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
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = [{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}]
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.MID,
"first_marker_idx": 0,
"second_marker_idx": 2,
},
{
"type": Constraint.TRACK_STATE,
"instant": Instant.MID,
"states_idx": 2,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker_idx": 0,
"second_marker_idx": 1,
},
)
# Path constraint
X_bounds = 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 = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque(),
[torque_max] * biorbd_model.nbGeneralizedTorque(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
is_cyclic_objective=not loop_from_constraint,
is_cyclic_constraint=loop_from_constraint,
)
def prepare_ocp(biorbd_model_path="cubeSym.bioMod",
show_online_optim=False,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
variable_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker": 0,
"second_marker": 1,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker": 0,
"second_marker": 2,
},
{
"type": Constraint.PROPORTIONAL_STATE,
"instant": Instant.ALL,
"first_dof": 2,
"second_dof": 3,
"coef": -1,
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
for i in range(4, 8):
X_bounds.first_node_min[i] = 0
X_bounds.last_node_min[i] = 0
X_bounds.first_node_max[i] = 0
X_bounds.last_node_max[i] = 0
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbQ(),
[torque_max] * biorbd_model.nbQ(),
)
U_init = InitialConditions([torque_init] * biorbd_model.nbQ())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
show_online_optim=show_online_optim,
)
def prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
excitations_ref,
q_ref,
with_residual_torque,
kin_data_to_track="markers",
):
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 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 = [
{"type": Objective.Lagrange.TRACK_MUSCLES_CONTROL, "weight": 1, "data_to_track": excitations_ref},
]
if with_residual_torque:
objective_functions.append({"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1})
if kin_data_to_track == "markers":
objective_functions.append(
{"type": Objective.Lagrange.TRACK_MARKERS, "weight": 100, "data_to_track": markers_ref},
)
elif kin_data_to_track == "q":
objective_functions.append(
{
"type": Objective.Lagrange.TRACK_STATE,
"weight": 100,
"data_to_track": q_ref,
"states_idx": range(biorbd_model.nbQ()),
},
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
if with_residual_torque:
variable_type = ProblemType.muscle_excitations_and_torque_driven
else:
variable_type = ProblemType.muscle_excitations_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
X_bounds.min[[0, 1], :] = -2 * np.pi
X_bounds.max[[0, 1], :] = 2 * np.pi
# Add muscle to the bounds
X_bounds.concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
# Initial guess
X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())
# Define control path constraint
if with_residual_torque:
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(),
[torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(),
)
U_init = InitialConditions(
[torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles()
)
else:
U_bounds = Bounds([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles(),)
U_init = InitialConditions([excitation_init] * biorbd_model.nbMuscles())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
nb_shooting,
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,
)