def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 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, weight=10)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[1, :] = 0
u_init = InitialGuessOption([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=target_g
)
parameters.add(
"gravity_z", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=parameter_objective_functions, # Objective of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_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 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,
node=Node.END,
min_bound=time_min,
max_bound=time_max)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuessOption([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,
number_shooting_points,
final_time,
x_init,
u_init,
x0,
):
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, idx=0)
objective_functions.add(Objective.Lagrange.ALIGN_SEGMENT_WITH_CUSTOM_RT,
weight=100,
segment_idx=Bow.segment_idx,
rt_idx=violin.rt_on_string,
idx=1)
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, 0] = x0
x_init = InitialGuessOption(x_init,
interpolation=InterpolationType.EACH_FRAME)
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_init = InitialGuessOption(u_init,
interpolation=InterpolationType.EACH_FRAME)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
), x_bounds
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(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[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[torque_min] * n_tau, [torque_max] * n_tau])
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuessOption([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,
number_shooting_points,
final_time,
initial_guess=InterpolationType.CONSTANT,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
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 and control path constraints
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
u_bounds = BoundsOption([[tau_min] * ntau, [tau_max] * ntau])
# Initial guesses
t = None
extra_params_x = {}
extra_params_u = {}
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [tau_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, number_shooting_points + 1))
u = np.random.random((ntau, number_shooting_points))
elif initial_guess == InterpolationType.SPLINE:
# Bound spline assume the first and last point are 0 and final respectively
t = np.hstack((0, np.sort(np.random.random(
(3, )) * final_time), final_time))
x = np.random.random((nq + nqdot, 5))
u = np.random.random((ntau, 5))
elif initial_guess == InterpolationType.CUSTOM:
# The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation
x = custom_init_func
u = custom_init_func
extra_params_x = {
"my_values": np.random.random((nq + nqdot, 2)),
"nb_shooting": number_shooting_points
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"nb_shooting": number_shooting_points
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialGuessOption(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialGuessOption(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(
biorbd_model,
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,
)
[0] * biorbd_model.nbMuscleTotal(),
[0] * 6 + [0.1] * 3 + [0] * 10,
[0] * 6 + [0.2] * 3 + [0] * 10,
[0] * 6 + [0.3] * 3 + [0] * 10,
# [0] * 6 + [0.4] * 3 + [0] * 10
]
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, x0=x0, nbGT=nbGT, number_shooting_points=Ns, use_SX=True)
for co in range(len(excitations_init)):
# for co in range(2, 4):
u_i = excitations_init[co]
u_mi = excitations_min[co]
u_ma = excitations_max
u_init = InitialGuessOption(np.tile(u_i, (ocp.nlp[0].ns, 1)).T, interpolation=InterpolationType.EACH_FRAME)
x_init = InitialGuessOption(np.tile(x0, (ocp.nlp[0].ns+1, 1)).T, interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init=u_init)
u_bounds = BoundsOption([u_mi, u_ma], interpolation=InterpolationType.CONSTANT)
ocp.update_bounds(u_bounds=u_bounds)
objectives = ObjectiveList()
objectives.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=1000)
objectives.add(
Objective.Lagrange.TRACK_STATE,
weight=100000,
target=states[:biorbd_model.nbQ(), :],
states_idx=np.array(range(nbQ))
)
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,
)
if EMG_lvl != 0:
muscles_target = generate_noise(
biorbd_model, q_ref, u_ref,
marker_noise_lvl[marker_lvl],
EMG_noise_lvl[EMG_lvl])[1]
# reload the model with the real markers
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x_ref[:, 0]
ocp.nlp[0].x_bounds.max[:, 0] = x_ref[:, 0]
# set initial guess on state
x_init = InitialGuessOption(
x_ref[:, 0:Ns_mhe * rt_ratio + 1:rt_ratio],
interpolation=InterpolationType.EACH_FRAME)
u0 = muscles_target
u_init = InitialGuessOption(
u0[:, 0:Ns_mhe * rt_ratio:rt_ratio],
interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
w_marker = 10000000
w_control = 100000
objectives = ObjectiveList()
if TRACK_EMG:
w_marker = 100000000
w_torque = 10
objectives.add(
# plt.show()
ocp = prepare_ocp(biorbd_model=biorbd_model,
final_time=T,
x0=x0,
nbGT=nbGT,
number_shooting_points=Ns,
use_torque=use_torque,
use_activation=use_activation,
use_SX=use_ACADOS)
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x0
ocp.nlp[0].x_bounds.max[:, 0] = x0
# set initial guess on state
x_init = InitialGuessOption(x0, interpolation=InterpolationType.CONSTANT)
u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT)
u_init = InitialGuessOption(u0, interpolation=InterpolationType.CONSTANT)
ocp.update_initial_guess(x_init, u_init)
objectives = ObjectiveList()
if use_activation:
objectives.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=10000,
target=muscles_target_real[:, :-1])
else:
objectives.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=1000,
target=muscles_target[:, :-1])
objectives.add(Objective.Lagrange.TRACK_MARKERS,
if EMG_lvl != 0:
muscles_target = generate_noise(
biorbd_model, q_ref, u_ref,
marker_noise_lvl[marker_lvl],
EMG_noise_lvl[EMG_lvl])[1]
# reload the model with the real markers
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x_ref[:, 0]
ocp.nlp[0].x_bounds.max[:, 0] = x_ref[:, 0]
# set initial guess on state
x_init = InitialGuessOption(
x_ref[:, 0:rt_ratio * (Ns_mhe + 1):rt_ratio],
interpolation=InterpolationType.EACH_FRAME)
u0 = muscles_target
u_init = InitialGuessOption(
u0[:, 0:rt_ratio * (Ns_mhe):rt_ratio],
interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
if use_activation:
w_marker = 100000000
w_control = 100000
else:
w_marker = 100000000
w_control = 100000
objectives = ObjectiveList()
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 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)
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))
if EMG_lvl != 0:
muscles_target = generate_noise(
biorbd_model, q_sol, u_sol,
marker_noise_lvl[marker_lvl],
EMG_noise_lvl[EMG_lvl])[1]
# reload the model with the real markers
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x0_ref
ocp.nlp[0].x_bounds.max[:, 0] = x0_ref
# set initial guess on state
x_init = InitialGuessOption(
x0_ref, interpolation=InterpolationType.CONSTANT)
u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT)
u_init = InitialGuessOption(
u0, interpolation=InterpolationType.CONSTANT)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
# w_marker = 500000
# w_state = 100
objectives = ObjectiveList()
if TRACK_EMG:
w_control = 100000
w_torque = 10
objectives.add(
Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=w_control,
]
ocp = prepare_ocp(biorbd_model=biorbd_model,
final_time=T,
number_shooting_points=Ns,
x0=x0,
xT=xT,
use_SX=True)
for i in range(0, len(excitations_init)):
u_i = excitations_init[i]
u_mi = excitations_min[i]
u_ma = excitations_max
# Update u_init and u_bounds
u_init = InitialGuessOption(np.tile(u_i, (ocp.nlp[0].ns, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
x_init = InitialGuessOption(np.tile(
np.concatenate((x0, [0.1] * biorbd_model.nbMuscles())),
(ocp.nlp[0].ns + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init=u_init)
u_bounds = BoundsOption([u_mi, u_ma],
interpolation=InterpolationType.CONSTANT)
ocp.update_bounds(u_bounds=u_bounds)
if use_ACADOS:
sol = ocp.solve(solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_max_iter": 100,
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,
node=Node.MID,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(Constraint.TRACK_STATE, node=Node.MID, index=2)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[2:6, -1] = [1.57, 0, 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())
# ------------- #
# 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,
)
with open(
f"solutions/sim_ac_{int(T * 1000)}ms_{Ns}sn_REACH2_co_level_{co}_tmp.bob",
'rb') as file:
data = pickle.load(file)
states = data['data'][0]
controls = data['data'][1]
q_sol = states['q']
dq_sol = states['q_dot']
a_sol = states['muscles']
u_sol = controls['muscles']
u_co = u_sol
t = np.linspace(0, T, u_co.shape[1])
x_ref = np.hstack(
[q_sol[:, 0], dq_sol[:, 0], [0.3] * biorbd_model.nbMuscles()])
# x_ref = np.concatenate((q_sol, dq_sol, a_sol))
x_init = InitialGuessOption(np.tile(x_ref, (ocp.nlp[0].ns + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
u0 = u_sol[:, :-1]
u_init = InitialGuessOption(u0, interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(
Bounds([0] * biorbd_model.nbMuscles(), [1] * biorbd_model.nbMuscles()))
x_bounds[0].min[:biorbd_model.nbQ() * 2,
0] = x_ref[:biorbd_model.nbQ() * 2]
x_bounds[0].max[:biorbd_model.nbQ() * 2,
0] = x_ref[:biorbd_model.nbQ() * 2]
x_bounds[0].min[biorbd_model.nbQ() * 2:biorbd_model.nbQ() * 2 +
biorbd_model.nbMuscles(),
0] = [0.1] * biorbd_model.nbMuscles()
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,
)