def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #nq
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_SEGMENT_WITH_CUSTOM_RT,
node=Node.ALL,
segment_idx=2,
rt_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][2, [0, -1]] = [-1.57, 1.57]
x_bounds[0][nq:, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, muscle_activations_ref, contact_forces_ref):
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, target=muscle_activations_ref)
objective_functions.add(Objective.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.001)
objective_functions.add(Objective.Lagrange.MINIMIZE_ALL_CONTROLS, weight=0.001)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[
[tau_min] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(),
]
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
)
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
constraints.add(Constraint.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][4:8, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbQ())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- 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_WITH_CONTACT)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (0, 0.07, 1.4, 0, 0, 0)
# Initial guess
x_init = InitialGuessList()
x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
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_to_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][:, 0] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * n_tau, [tau_max] * n_tau])
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_TIME)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
def prepare_ocp(biorbd_model_path, problem_type_custom=True, 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 = DynamicsTypeList()
if problem_type_custom:
dynamics.add(custom_configure, dynamic_function=custom_dynamic)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN, dynamic_function=custom_dynamic)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuessOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption(
[[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]
)
u_init = InitialGuessOption([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, problem_type_custom=True, ode_solver=OdeSolver.RK, use_SX=False):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_STATE, weight=1000, states_idx=[0, 1], target=np.array([[1., 2.]]).T)
objective_functions.add(Objective.Mayer.MINIMIZE_STATE, weight=10000, states_idx=[2], target=np.array([[3.]]))
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1,)
# Dynamics
dynamics = DynamicsTypeList()
if problem_type_custom:
dynamics.add(custom_configure, dynamic_function=custom_dynamic)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN, dynamic_function=custom_dynamic)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, 0] = 0
# Initial guess
x_init = InitialGuessOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption(
[[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]
)
u_init = InitialGuessOption([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
use_SX=use_SX
)
def partial_ocp_parameters(nb_phases):
if nb_phases != 1 and nb_phases != 3:
raise RuntimeError("nb_phases should be 1 or 3")
biorbd_model_path = str(
PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod"
biorbd_model = biorbd.Model(biorbd_model_path), biorbd.Model(
biorbd_model_path), biorbd.Model(biorbd_model_path)
number_shooting_points = (2, 2, 2)
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
tau_min, tau_max, tau_init = -100, 100, 0
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
if nb_phases > 1:
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
if nb_phases > 1:
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
if nb_phases > 1:
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if nb_phases > 1:
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
if nb_phases > 1:
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
if nb_phases > 1:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
return (
biorbd_model[:nb_phases],
number_shooting_points[:nb_phases],
final_time[:nb_phases],
time_min[:nb_phases],
time_max[:nb_phases],
tau_min,
tau_max,
tau_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
)
def prepare_test_ocp(with_muscles=False, with_contact=False):
PROJECT_FOLDER = Path(__file__).parent / ".."
if with_muscles and with_contact:
raise RuntimeError(
"With muscles and with contact together is not defined")
elif with_muscles:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod")
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles()
elif with_contact:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) +
"/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod"
)
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
else:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuessOption(np.zeros((nx, 1)))
u_init = InitialGuessOption(np.zeros((nu, 1)))
x_bounds = BoundsOption([np.zeros((nx, 1)), np.zeros((nx, 1))])
u_bounds = BoundsOption([np.zeros((nu, 1)), np.zeros((nu, 1))])
ocp = OptimalControlProgram(biorbd_model, dynamics, 10, 1.0, x_init,
u_init, x_bounds, u_bounds)
ocp.nlp[0].J = [list()]
ocp.nlp[0].g = [list()]
ocp.nlp[0].g_bounds = [list()]
return ocp
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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min[0],
max_bound=time_max[0],
phase=0)
if nb_phases == 3:
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min[1],
max_bound=time_max[1],
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(Constraint.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min[2],
max_bound=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[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
if nb_phases == 3:
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
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 = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
if nb_phases == 3:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model[:nb_phases],
dynamics,
number_shooting_points,
final_time[:nb_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path,
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,
node=Node.END,
)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[torque_min] * n_tau,
[torque_max] * n_tau,
])
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
)
def test_update_bounds_and_init_with_param():
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
g_min, g_max, g_init = -10, -6, -8
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(min_bound=g_min,
max_bound=g_max,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=-8)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
ocp = OptimalControlProgram(biorbd_model,
dynamics,
ns,
1.0,
parameters=parameters)
x_bounds = BoundsOption([-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))])
u_bounds = BoundsOption([-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))])
ocp.update_bounds(x_bounds, u_bounds)
expected = np.append(
np.tile(np.append(-np.ones((nq * 2, 1)), -2.0 * np.ones((nq, 1))), ns),
-np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_bounds.min,
np.append([g_min], expected).reshape(129, 1))
expected = np.append(
np.tile(np.append(np.ones((nq * 2, 1)), 2.0 * np.ones((nq, 1))), ns),
np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_bounds.max,
np.append([[g_max]], expected).reshape(129, 1))
x_init = InitialGuessOption(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuessOption(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.append(
np.tile(np.append(0.5 * np.ones((nq * 2, 1)), -0.5 * np.ones((nq, 1))),
ns), 0.5 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_init.init,
np.append([g_init], expected).reshape(129, 1))
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
x0,
xT,
use_SX=False,
nb_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nbQ = biorbd_model.nbQ()
tau_min, tau_max, tau_init = -10, 10, 0
muscle_min, muscle_max, muscle_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=10000,
states_idx=np.array(range(0, nbQ)))
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10000,
states_idx=np.array(range(nbQ, nbQ * 2)))
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=100)
objective_functions.add(Objective.Mayer.MINIMIZE_STATE,
weight=1000000,
target=np.tile(xT,
(number_shooting_points + 1, 1)).T,
states_idx=np.array(range(0, nbQ)))
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = x0
# 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(),
])
# Initial guesses
x_init = InitialGuessOption(np.tile(x0, (number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
u0 = np.array([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads,
)
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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model, all_generalized_mapping))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
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 = InitialGuessList()
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="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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.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[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
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):
# --- 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,
index=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 = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
u_init = InitialGuessList()
u_init.add([tau_init] * ntau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
def prepare_ocp(model_path, phase_time, ns, time_min, time_max):
# --- Options --- #
# Model path
biorbd_model = [biorbd.Model(elt) for elt in model_path]
nb_phases = len(biorbd_model)
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
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
nq = len(q_mapping.reduce.map_idx)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-100,
phase=0)
# Dynamics
dynamics = DynamicsTypeList()
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,
phase=0,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
# 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,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
# Custom constraints for positivity of CoM_dot on z axis just before the take-off
constraints.add(utils.com_dot_z,
phase=0,
node=Node.END,
min_bound=0,
max_bound=np.inf)
# Constraint arm positivity
constraints.add(Constraint.TRACK_STATE,
phase=0,
node=Node.END,
index=3,
min_bound=1.0,
max_bound=np.inf)
# Torque constraint + minimize_state
for i in range(nb_phases):
constraints.add(utils.tau_actuator_constraints,
phase=i,
node=Node.ALL,
minimal_tau=20)
objective_functions.add(Objective.Mayer.MINIMIZE_TIME,
weight=0.0001,
phase=i,
min_bound=time_min[i],
max_bound=time_max[i])
# Path constraint
nb_q = q_mapping.reduce.len
nb_q_dot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 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))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_q_dot
# Initial guess for states (Interpolation type is CONSTANT)
x_init = InitialGuessList()
for i in range(nb_phases):
x_init.add(pose_at_first_node + [0] * nb_q_dot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[-500] * tau_mapping.reduce.len,
[500] * tau_mapping.reduce.len])
# Define initial guess for controls
u_init = InitialGuessList()
u_init.add([0] * tau_mapping.reduce.len)
# ------------- #
ocp = OptimalControlProgram(
biorbd_model,
dynamics,
ns,
phase_time,
x_init=x_init,
x_bounds=x_bounds,
u_init=u_init,
u_bounds=u_bounds,
objective_functions=objective_functions,
constraints=constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
nb_threads=4,
use_SX=False,
)
return utils.add_custom_plots(ocp, nb_phases, x_bounds, nq, minimal_tau=20)
def prepare_ocp(
biorbd_model,
final_time,
number_shooting_points,
x0,
xT,
use_SX=False,
nb_threads=8,
):
# --- Options --- #
# Model path
biorbd_model = biorbd_model
nbQ = biorbd_model.nbQ()
# tau_min, tau_max, tau_init = -10, 10, 0
activation_min, activation_max, activation_init = 0, 1, 0.1
excitation_min, excitation_max, excitation_init = 0, 1, 0.2
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10,
states_idx=np.array(range(biorbd_model.nbQ())))
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10,
states_idx=np.array(
range(biorbd_model.nbQ(),
biorbd_model.nbQ() * 2)))
objective_functions.add(
Objective.Lagrange.MINIMIZE_STATE,
weight=10,
states_idx=np.array(
range(biorbd_model.nbQ() * 2,
biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles())))
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10)
objective_functions.add(Objective.Mayer.TRACK_STATE,
weight=100000,
target=np.array([xT[:biorbd_model.nbQ()]]).T,
states_idx=np.array(range(biorbd_model.nbQ())))
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# add muscle activation bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
x_bounds[0].min[:nbQ, 0] = [-0.1, -0.3, 0.1, -0.3]
x_bounds[0].max[:nbQ, 0] = [-0.1, 0, 0.3, 0]
# Control path constraint
u_bounds = BoundsList()
u_bounds.add([
[excitation_min] * biorbd_model.nbMuscleTotal(),
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
# Initial guesses
x_init = InitialGuessOption(np.tile(
np.concatenate((x0, [activation_init] * biorbd_model.nbMuscles())),
(number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
u0 = np.array([excitation_init] * biorbd_model.nbMuscles())
u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads,
)
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
x_warm=None,
use_SX=False,
nb_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -50, 50, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=10)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10)
objective_functions.add(Objective.Mayer.ALIGN_MARKERS,
weight=100000,
first_marker_idx=0,
second_marker_idx=1)
# 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][:, 0] = (1.0, 1.0, 0, 0)
# Initial guess
if x_warm is None:
x_init = InitialGuessOption([1.57] * biorbd_model.nbQ() +
[0] * biorbd_model.nbQdot())
else:
x_init = InitialGuessOption(x_warm,
interpolation=InterpolationType.EACH_FRAME)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
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(model_path,
phase_time,
number_shooting_points,
use_actuators=False):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1 / 100)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
tau_mapping=tau_mapping,
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
max_torque,
X0,
U0,
target=None,
interpolation=InterpolationType.EACH_FRAME,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -max_torque, max_torque, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS, weight=1000, target=target)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=100, target=X0)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = -np.inf
x_bounds[0].max[:, 0] = np.inf
# x_bounds[0].min[:biorbd_model.nbQ(), 0] = X0[:biorbd_model.nbQ(),0]
# x_bounds[0].max[:biorbd_model.nbQ(), 0] = X0[:biorbd_model.nbQ(),0]
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min, 0.0], [tau_max, 0.0]])
# Initial guesses
x = X0
u = U0
x_init = InitialGuessList()
x_init.add(x, interpolation=interpolation)
u_init = InitialGuessList()
u_init.add(u, interpolation=interpolation)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=4,
use_SX=True,
)
def prepare_ocp(biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_init,
f_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
# nb_tau = biorbd_model.segment(0).nbQ()
# tau_mapping = BidirectionalMapping(Mapping(range(6)), Mapping(range(nb_tau)))
# 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=1000,
# idx_states=(0, 1, 2, 3, 4, 5, 11, 12, 13, 14, 15, 16))
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=1,
idx_states=(6, 7, 8, 9, 10))
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=1) # controls_idx=[6, 7, 8, 9, 10],
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
weight=1000,
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_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif activation_driven:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Constraints
constraints = ()
# constraints = ConstraintList()
# constraints.add(Constraint.TRACK_TORQUE, instant=Instant.ALL, controls_idx=(6, 7, 8, 9, 10),
# target=np.zeros((biorbd_model.nbQ()*2, number_shooting_points)))
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# if use_SX:
# x_bounds[0].min[biorbd_model.nbQ():, 0] = -10
# x_bounds[0].max[biorbd_model.nbQ():, 0] = 10
# 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 = InitialGuessList()
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(state_init[:-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 = InitialGuessList()
nb_tau = biorbd_model.nbGeneralizedTorque()
init_residual_torque = np.concatenate((np.ones(
(nb_tau, n_shooting_points)) * 0.5, excitations_ref))
if use_residual_torque:
u_bounds.add([
[torque_min] * nb_tau +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * nb_tau +
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
# u_init.add([torque_init] * tau_mapping.reduce.len + [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.01, nb_mus+1), max_bound=np.repeat(7, nb_mus+1), interpolation=InterpolationType.CONSTANT)
min_bound=[0.5] * nb_mus,
max_bound=[3] * nb_mus,
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 = InitialGuess(f_init)
initial_guess_A = InitialGuess([-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, # 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,
# tau_mapping=tau_mapping,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, min_bound):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(
DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
Constraint.CONTACT_FORCE,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=2,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
nb_mus = biorbd_model.nbMuscleTotal()
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(
Bounds([activation_min] * nb_mus, [activation_max] * nb_mus))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[torque_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialGuessList()
u_init.add([torque_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(
biorbd_model,
final_time,
x0,
nbGT,
number_shooting_points,
use_SX=False,
nb_threads=1,
):
# --- Options --- #
# Model path
biorbd_model = biorbd_model
nbQ = biorbd_model.nbQ()
nbGT = nbGT
nbMT = biorbd_model.nbMuscleTotal()
tau_min, tau_max, tau_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
activation_min, activation_max, activation_init = 0, 1, 0.2
# Add objective functions
objectives = ObjectiveList()
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
x_bounds[0].min[:nbQ*2, 0] = x0[:nbQ*2]
x_bounds[0].max[:nbQ*2, 0] = x0[:nbQ*2]
x_bounds[0].min[nbQ * 2:nbQ * 2+nbMT, 0] = [0.1] * nbMT
x_bounds[0].max[nbQ * 2:nbQ * 2+nbMT, 0] = [1] * nbMT
# Control path constraint
u_bounds = BoundsList()
u_bounds.add(
[
[tau_min] * nbGT + [muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * nbGT + [muscle_max] * biorbd_model.nbMuscleTotal(),
]
)
# Initial guesses
x_init = InitialGuessOption(np.tile(x0, (number_shooting_points+1, 1)).T, interpolation=InterpolationType.EACH_FRAME)
u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT)+0.1
u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objectives,
use_SX=use_SX,
nb_threads=nb_threads,
)
def prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
activations_ref,
q_ref,
kin_data_to_track="markers",
use_residual_torque=True,
):
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
nq = biorbd_model.nbQ()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
target=activations_ref)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(Objective.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
index=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
x_bounds[0].min[:nq, :] = -2 * np.pi
x_bounds[0].max[:nq, :] = 2 * np.pi
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_init = InitialGuessList()
if use_residual_torque:
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
])
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
else:
u_bounds.add([[activation_min] * biorbd_model.nbMuscleTotal(),
[activation_max] * biorbd_model.nbMuscleTotal()])
u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
nb_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# 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][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
external_forces=external_forces,
ode_solver=ode_solver,
)
def prepare_ocp(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model_path = str(
PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK
# --- Options --- #
nb_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(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, phase=0)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(
Constraint.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 0
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[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, 2))
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_bound=min_g,
max_bound=max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:nb_phases],
dynamics,
ns,
final_time[:nb_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
x_bounds[0][2, [0, -1]] = [0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def test_double_update_bounds_and_init():
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0)
x_bounds = BoundsOption([-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))])
u_bounds = BoundsOption([-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))])
ocp.update_bounds(x_bounds, u_bounds)
expected = np.append(
np.tile(np.append(-np.ones((nq * 2, 1)), -2.0 * np.ones((nq, 1))), ns),
-np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(ocp.V_bounds.min, expected.reshape(128, 1))
expected = np.append(
np.tile(np.append(np.ones((nq * 2, 1)), 2.0 * np.ones((nq, 1))), ns),
np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(ocp.V_bounds.max, expected.reshape(128, 1))
x_init = InitialGuessOption(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuessOption(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.append(
np.tile(np.append(0.5 * np.ones((nq * 2, 1)), -0.5 * np.ones((nq, 1))),
ns), 0.5 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(ocp.V_init.init, expected.reshape(128, 1))
x_bounds = BoundsOption(
[-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1))])
u_bounds = BoundsOption([-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1))])
ocp.update_bounds(x_bounds=x_bounds)
ocp.update_bounds(u_bounds=u_bounds)
expected = np.append(
np.tile(
np.append(-2.0 * np.ones((nq * 2, 1)), -4.0 * np.ones((nq, 1))),
ns), -2.0 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(ocp.V_bounds.min, expected.reshape(128, 1))
expected = np.append(
np.tile(np.append(2.0 * np.ones((nq * 2, 1)), 4.0 * np.ones((nq, 1))),
ns), 2.0 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(ocp.V_bounds.max, expected.reshape(128, 1))
x_init = InitialGuessOption(0.25 * np.ones((nq * 2, 1)))
u_init = InitialGuessOption(-0.25 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.append(
np.tile(
np.append(0.25 * np.ones((nq * 2, 1)), -0.25 * np.ones((nq, 1))),
ns), 0.25 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(ocp.V_init.init, expected.reshape(128, 1))
with pytest.raises(
RuntimeError,
match=
"x_init should be built from a InitialGuessOption or InitialGuessList"
):
ocp.update_initial_guess(x_bounds, u_bounds)
with pytest.raises(
RuntimeError,
match="x_bounds should be built from a BoundsOption or BoundsList"
):
ocp.update_bounds(x_init, u_init)