def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK4(),
use_sx: bool = True,
n_threads: int = 1,
) -> OptimalControlProgram:
"""
The initialization of an ocp
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
final_time: float
The time in second required to perform the task
n_shooting: int
The number of shooting points to define int the direct multiple shooting program
ode_solver: OdeSolver = OdeSolver.RK4()
Which type of OdeSolver to use
use_sx: bool
If the SX variable should be used instead of MX (can be extensive on RAM)
n_threads: int
The number of threads to use in the paralleling (1 = no parallel computing)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
ode_solver=ode_solver,
use_sx=use_sx,
n_threads=n_threads,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
n_threads: int,
use_sx: bool = False,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
n_threads: int
The number of threads to use while using multithreading
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
n_threads=n_threads,
use_sx=use_sx,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
fatigue_type: str,
split_controls: bool,
use_sx: bool = True,
) -> OptimalControlProgram:
"""
The initialization of an ocp
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
final_time: float
The time in second required to perform the task
n_shooting: int
The number of shooting points to define int the direct multiple shooting program
fatigue_type: str
The type of dynamics to apply ("xia" or "michaud")
split_controls: bool
If the tau should be split into minus and plus or a if_else should be used
use_sx: bool
If the program should be built from SX (True) or MX (False)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", expand=True)
# Fatigue parameters
fatigue_dynamics = FatigueList()
for i in range(n_tau):
if fatigue_type == "xia":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_min),
XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_max),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "xia_stabilized":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_min),
XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_max),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudTauFatigue(
MichaudFatigue(
LD=100,
LR=100,
F=0.005,
R=0.005,
effort_threshold=0.2,
effort_factor=0.001,
stabilization_factor=10,
scaling=tau_min,
),
MichaudFatigue(
LD=100,
LR=100,
F=0.005,
R=0.005,
effort_threshold=0.2,
effort_factor=0.001,
stabilization_factor=10,
scaling=tau_max,
),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "effort":
fatigue_dynamics.add(
TauEffortPerception(
EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_min),
EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_max),
split_controls=split_controls,
)
)
else:
raise ValueError("fatigue_type not implemented")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, fatigue=fatigue_dynamics, expand=True)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True))
if fatigue_type != "effort":
x_bounds[[5, 11], 0] = 0 # The rotation dof is passive (fatigue_ma = 0)
if fatigue_type == "xia":
x_bounds[[7, 13], 0] = 1 # The rotation dof is passive (fatigue_mr = 1)
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
x_init.concatenate(FatigueInitialGuess(fatigue_dynamics))
# Define control path constraint
u_bounds = FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS)
if split_controls:
u_bounds[[1, 3], :] = 0 # The rotation dof is passive
else:
u_bounds[1, :] = 0 # The rotation dof is passive
u_init = FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
use_sx=use_sx,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
time_min: float,
time_max: float,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the optimal control program
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the final time
n_shooting: int
The number of shooting points
time_min: float
The minimal time the phase can have
time_max: float
The maximal time the phase can have
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = Constraint(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min,
max_bound=time_max)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def test_update_bounds_and_init_with_param():
def my_parameter_function(biorbd_model, value, extra_value):
new_gravity = MX.zeros(3, 1)
new_gravity[2] = value + extra_value
biorbd_model.setGravity(new_gravity)
def my_target_function(ocp, value, target_value):
return value + target_value
biorbd_model = biorbd.Model(TestUtils.bioptim_folder() +
"/examples/track/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
g_min, g_max, g_init = -10, -6, -8
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(g_min,
g_max,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.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 = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1)))
u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1)))
ocp.update_bounds(x_bounds, u_bounds)
expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min,
np.append(expected, [g_min])[:, np.newaxis])
expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max,
np.append(expected, [g_max])[:, np.newaxis])
x_init = InitialGuess(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T
np.testing.assert_almost_equal(
ocp.v.init.init,
np.append(expected, [g_init])[:, np.newaxis])
def prepare_test_ocp(with_muscles=False,
with_contact=False,
with_actuator=False,
implicit=False,
use_sx=True):
bioptim_folder = TestUtils.bioptim_folder()
if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator:
raise RuntimeError(
"With muscles and with contact and with_actuator together is not defined"
)
if with_muscles and implicit or implicit and with_actuator:
raise RuntimeError(
"With muscles and implicit and with_actuator together is not defined"
)
elif with_muscles:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/muscle_driven_ocp/models/arm26.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles()
elif with_contact:
biorbd_model = biorbd.Model(
bioptim_folder +
"/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod"
)
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True,
expand=False,
implicit_dynamics=implicit)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
elif with_actuator:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/torque_driven_ocp/models/cube.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
else:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/track/models/cube_and_line.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess(np.zeros((nx, 1)))
mod = 2 if implicit else 1
u_init = InitialGuess(np.zeros((nu * mod, 1)))
x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1)))
u_bounds = Bounds(np.zeros((nu * mod, 1)), np.zeros((nu * mod, 1)))
ocp = OptimalControlProgram(biorbd_model,
dynamics,
10,
1.0,
x_init,
u_init,
x_bounds,
u_bounds,
use_sx=use_sx)
ocp.nlp[0].J = [[]]
ocp.nlp[0].g = [[]]
return ocp
def prepare_ocp(
biorbd_model_path: str,
problem_type_custom: bool = True,
ode_solver: OdeSolver = OdeSolver.RK4(),
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
problem_type_custom: bool
If the preparation should be done using the user-defined dynamic function or the normal TORQUE_DRIVEN.
They should return the same results
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
if problem_type_custom:
dynamics.add(custom_configure, dynamic_function=custom_dynamic, my_additional_factor=1, expand=expand)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, dynamic_function=custom_dynamic, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2")
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK4()) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(
custom_func_track_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.START,
quadratic=True,
first_marker="m0",
second_marker="m1",
weight=1000,
)
objective_functions.add(
custom_func_track_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.END,
quadratic=True,
first_marker="m0",
second_marker="m2",
weight=1000,
)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(
custom_func_align_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.START,
quadratic=True,
first_marker_idx=0,
second_marker_idx=1,
weight=1000,
)
objective_functions.add(
custom_func_align_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.END,
quadratic=True,
first_marker_idx=0,
second_marker_idx=2,
weight=1000,
)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([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,
)
def prepare_ocp(
biorbd_model,
final_time,
x0,
nbGT,
number_shooting_points,
use_SX=False,
nb_threads=8,
use_torque=False,
use_activation=False,
):
# --- Options --- #
# Model path
biorbd_model = biorbd_model
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
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsList()
if use_activation and use_torque:
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif use_activation is not True and use_torque:
dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
elif use_activation and use_torque is not True:
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN)
elif use_activation is not True and use_torque is not True:
dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
if use_activation is not True:
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# 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 = InitialGuess(np.tile(x0, (number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT)
u_init = InitialGuess(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,
n_threads=nb_threads,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
fatigue_type: str,
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
torque_level: int = 0,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
fatigue_type: str
The type of dynamics to apply ("xia" or "michaud")
ode_solver: OdeSolver
The ode solver to use
torque_level: int
0 no residual torque, 1 with residual torque, 2 with fatigable residual torque
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
n_muscles = biorbd_model.nbMuscleTotal()
tau_min, tau_max = -10, 10
# Define fatigue parameters for each muscle and residual torque
fatigue_dynamics = FatigueList()
for i in range(n_muscles):
if fatigue_type == "xia":
fatigue_dynamics.add(XiaFatigue(LD=10, LR=10, F=0.01, R=0.002), state_only=False)
elif fatigue_type == "xia_stabilized":
fatigue_dynamics.add(
XiaFatigueStabilized(LD=10, LR=10, F=0.01, R=0.002, stabilization_factor=10), state_only=False
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudFatigue(
LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10
),
state_only=True,
)
elif fatigue_type == "effort":
fatigue_dynamics.add(EffortPerception(effort_threshold=0.2, effort_factor=0.001))
else:
raise ValueError("fatigue_type not implemented")
if torque_level >= 2:
for i in range(n_tau):
if fatigue_type == "xia":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_min),
XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_max),
),
state_only=False,
)
elif fatigue_type == "xia_stabilized":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigueStabilized(LD=10, LR=10, F=5, R=10, stabilization_factor=10, scaling=tau_min),
XiaFatigueStabilized(LD=10, LR=10, F=5, R=10, stabilization_factor=10, scaling=tau_max),
),
state_only=False,
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudTauFatigue(
MichaudFatigue(
LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_min
),
MichaudFatigue(
LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_max
),
),
state_only=False,
)
elif fatigue_type == "effort":
fatigue_dynamics.add(
TauEffortPerception(
EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_min),
EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_max),
),
state_only=False,
)
else:
raise ValueError("fatigue_type not implemented")
# Dynamics
dynamics = Dynamics(DynamicsFcn.MUSCLE_DRIVEN, expand=False, fatigue=fatigue_dynamics, with_torque=torque_level > 0)
# Add objective functions
objective_functions = ObjectiveList()
if torque_level > 0:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", weight=0.01
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_FATIGUE, key="muscles", weight=1000)
# Constraint
constraint = Constraint(
ConstraintFcn.SUPERIMPOSE_MARKERS,
first_marker="target",
second_marker="COM_hand",
node=Node.END,
axes=[Axis.X, Axis.Y],
)
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, 0] = (0.07, 1.4, 0, 0)
x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True))
x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
x_init.concatenate(FatigueInitialGuess(fatigue_dynamics))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) if torque_level == 1 else Bounds()
u_bounds.concatenate(FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS))
u_init = InitialGuess([0] * n_tau) if torque_level == 1 else InitialGuess()
u_init.concatenate(FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS))
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraint,
ode_solver=ode_solver,
use_sx=False,
n_threads=8,
)
def prepare_ocp(
biorbd_model_path: str, ode_solver: OdeSolver = OdeSolver.IRK()
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_track_markers,
node=Node.START,
first_marker="m0",
second_marker="m1",
method=0)
constraints.add(custom_func_track_markers,
node=Node.END,
first_marker="m0",
second_marker="m2",
method=1)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
index=1,
weight=-1)
# Dynamics
dynamics = Dynamics(custom_configure, dynamic_function=custom_dynamic)
# Path constraint
x_bounds = QAndQDotBounds(m)
x_bounds[:, 0] = [0] * m.nbQ() + [0] * m.nbQdot()
x_bounds.min[:, 1] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 1] = [1] * m.nbQ() + [100] * m.nbQdot()
x_bounds.min[:, 2] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 2] = [1] * m.nbQ() + [100] * m.nbQdot()
# Initial guess
x_init = InitialGuess([0] * (m.nbQ() + m.nbQdot()))
# Define control path constraint
u_bounds = Bounds([-100] * m.nbGeneralizedTorque(),
[0] * m.nbGeneralizedTorque())
u_init = InitialGuess([0] * m.nbGeneralizedTorque())
ocp = OptimalControlProgram(
m,
dynamics,
number_shooting_points=30,
phase_time=0.5,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
def prepare_ocp_parameters(
biorbd_model_path,
final_time,
n_shooting,
optim_gravity,
optim_mass,
min_g,
max_g,
target_g,
min_m,
max_m,
target_m,
ode_solver=OdeSolver.RK4(),
use_sx=False,
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
optim_gravity: bool
If the gravity should be optimized
optim_mass: bool
If the mass should be optimized
min_g: np.ndarray
The minimal value for the gravity
max_g: np.ndarray
The maximal value for the gravity
target_g: np.ndarray
The target value for the gravity
min_m: float
The minimal value for the mass
max_m: float
The maximal value for the mass
target_m: float
The target value for the mass
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
tau_min, tau_max, tau_init = -300, 300, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# Define the parameter to optimize
parameters = ParameterList()
if optim_gravity:
# Give the parameter some min and max bounds
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
# and an objective function
parameter_objective_functions = Objective(
my_target_function,
weight=1000,
quadratic=False,
custom_type=ObjectiveFcn.Parameter,
target=target_g)
parameters.add(
"gravity_xyz", # 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=
3, # The number of elements this particular parameter vector has
penalty_list=
parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter
scaling=np.array([1, 1, 10.0]),
extra_value=1, # You can define as many extra arguments as you want
)
if optim_mass:
bound_mass = Bounds(min_m,
max_m,
interpolation=InterpolationType.CONSTANT)
initial_mass = InitialGuess((min_m + max_m) / 2)
mass_objective_functions = Objective(
my_target_function,
weight=100,
quadratic=False,
custom_type=ObjectiveFcn.Parameter,
target=target_m)
parameters.add(
"mass", # The name of the parameter
set_mass, # The function that modifies the biorbd model
initial_mass, # The initial guess
bound_mass, # The bounds
size=
1, # The number of elements this particular parameter vector has
penalty_list=
mass_objective_functions, # ObjectiveFcn of constraint for this particular parameter
scaling=np.array([10.0]),
)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
ode_solver=ode_solver,
use_sx=use_sx,
)
def generate_state(model_path, T, Ns, nb_phase, x_phase):
# Parameters of the problem
biorbd_model = biorbd.Model(model_path)
X_est = np.zeros((biorbd_model.nbQ() * 2 + biorbd_model.nbMuscleTotal(), nb_phase * Ns + 1))
U_est = np.zeros((biorbd_model.nbMuscleTotal(), nb_phase * Ns))
# Set the joint angles target for each phase
x0 = x_phase[0, :]
# Build graph
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, number_shooting_points=Ns, x0=x0, use_sx=True)
x0 = np.concatenate((x0, np.array([0.2] * biorbd_model.nbMuscles())))
# Solve for each phase
for phase in range(1, nb_phase + 1):
# impose it as first state of next solve
ocp.nlp[0].x_bounds.min[:, 0] = x0
ocp.nlp[0].x_bounds.max[:, 0] = x0
# update initial guess on states
x_init = InitialGuess(np.tile(x0, (ocp.nlp[0].ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init=x_init)
# Update objectives functions
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10, index=np.array(range(biorbd_model.nbQ())))
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=10,
index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=10,
index=np.array(range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles())),
)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10)
objectives.add(
ObjectiveFcn.Mayer.TRACK_STATE,
weight=10000,
target=np.array([x_phase[phase, : biorbd_model.nbQ() * 2]]).T,
index=np.array(range(biorbd_model.nbQ() * 2)),
)
ocp.update_objectives(objectives)
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
# "nlp_solver_max_iter": 50,
"nlp_solver_tol_comp": 1e-7,
"nlp_solver_tol_eq": 1e-7,
"nlp_solver_tol_stat": 1e-7,
},
)
# get last state of previous solve
x_out, u_out, x0 = switch_phase(ocp, sol)
# Store optimal solution
X_est[:, (phase - 1) * Ns : phase * Ns] = x_out
U_est[:, (phase - 1) * Ns : phase * Ns] = u_out
# Collect last state
X_est[:, -1] = x0
return X_est
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)
biorbd_model_path = TestUtils.bioptim_folder(
) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK4()
# --- Options --- #
n_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(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(
ConstraintFcn.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(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(bounds=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_g,
max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.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[:n_phases],
dynamics,
ns,
final_time[:n_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
interpolation_type: InterpolationType = InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
) -> OptimalControlProgram:
"""
Prepare the ocp for the specified interpolation type
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
n_shooting: int
The number of shooting point
final_time: float
The movement time
interpolation_type: InterpolationType
The requested InterpolationType
Returns
-------
The OCP fully prepared and ready to be solved
"""
# 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2")
# Path constraints
if interpolation_type == InterpolationType.CONSTANT:
x_min = [-100] * (nq + nqdot)
x_max = [100] * (nq + nqdot)
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT)
u_min = [tau_min] * ntau
u_max = [tau_max] * ntau
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT)
elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x_min = np.random.random((6, 3)) * (-10) - 5
x_max = np.random.random((6, 3)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
u_min = np.random.random((3, 3)) * tau_min + tau_min / 2
u_max = np.random.random((3, 3)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
elif interpolation_type == InterpolationType.LINEAR:
x_min = np.random.random((6, 2)) * (-10) - 5
x_max = np.random.random((6, 2)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR)
u_min = np.random.random((3, 2)) * tau_min + tau_min / 2
u_max = np.random.random((3, 2)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR)
elif interpolation_type == InterpolationType.EACH_FRAME:
x_min = np.random.random((nq + nqdot, n_shooting + 1)) * (-10) - 5
x_max = np.random.random((nq + nqdot, n_shooting + 1)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.EACH_FRAME)
u_min = np.random.random((ntau, n_shooting)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, n_shooting)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.EACH_FRAME)
elif interpolation_type == InterpolationType.SPLINE:
spline_time = np.hstack((0, np.sort(np.random.random((3,)) * final_time), final_time))
x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5
x_max = np.random.random((nq + nqdot, 5)) * 10 + 5
u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.SPLINE, t=spline_time)
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.SPLINE, t=spline_time)
elif interpolation_type == InterpolationType.CUSTOM:
# The custom functions refer to the ones at the beginning of the file.
# For this particular instance, they emulate a Linear interpolation
extra_params_x = {"n_elements": nq + nqdot, "n_shooting": n_shooting}
extra_params_u = {"n_elements": ntau, "n_shooting": n_shooting}
x_bounds = Bounds(
custom_x_bounds_min, custom_x_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_x
)
u_bounds = Bounds(
custom_u_bounds_min, custom_u_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_u
)
else:
raise NotImplementedError("Not implemented yet")
# Initial guess
x_init = InitialGuess([0] * (nq + nqdot))
u_init = InitialGuess([tau_init] * ntau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def run_mhe(model_path, ocp, var, conf, fold):
# Set variables
Ns, T, Ns_mhe, rt_ratio = var["Ns"], var["T"], var["Ns_mhe"], var[
"rt_ratio"]
co, marker_lvl, EMG_lvl = var["co"], var["marker_lvl"], var["EMG_lvl"]
X_est, U_est = var["X_est"], var["U_est"]
markers_target, muscles_target = var["markers_target"], var[
"muscles_target"]
marker_noise_lvl, EMG_noise_lvl = var["marker_noise_lvl"], var[
"EMG_noise_lvl"]
x_ref, u_ref = var["x_ref"], var["u_ref"]
biorbd_model = biorbd.Model(model_path)
nbQ, nbMT = biorbd_model.nbQ(), biorbd_model.nbMuscles()
nbGT = biorbd_model.nbGeneralizedTorque() if conf["use_torque"] else 0
q_ref, dq_ref, a_ref = x_ref[:nbQ, :], x_ref[nbQ:nbQ * 2, :], x_ref[nbQ *
2:, :]
if "TRACK_LESS_EMG" in conf.keys():
TRACK_LESS_EMG = conf["TRACK_LESS_EMG"]
idx = var["idx_muscle_track"] if conf["TRACK_LESS_EMG"] else False
else:
idx = None
TRACK_LESS_EMG = False
# Set number of tries
nb_try = var["nb_try"] if conf["use_try"] else 1
# Set variables' shape for all tries
X_est_tries = np.ndarray((nb_try, X_est.shape[0], X_est.shape[1]))
U_est_tries = np.ndarray((nb_try, U_est.shape[0], U_est.shape[1]))
markers_target_tries = np.ndarray(
(nb_try, markers_target.shape[0], markers_target.shape[1], Ns + 1))
muscles_target_tries = np.ndarray(
(nb_try, muscles_target.shape[0], Ns + 1))
force_ref = np.ndarray((biorbd_model.nbMuscles(), Ns))
force_est = np.ndarray(
(nb_try, biorbd_model.nbMuscles(), int(ceil(Ns / rt_ratio) - Ns_mhe)))
err_tries = np.ndarray((nb_try, 10))
# Loop for simulate some tries, generate new random noise to each try
for tries in range(nb_try):
# Print current optimisation configuration
print(
f"- Ns_mhe = {Ns_mhe}; Co_lvl: {co}; Marker_noise: {marker_lvl}; EMG_noise : {EMG_lvl}; nb_try : {tries} -"
)
# Generate data with noise
if conf["use_noise"]:
if marker_lvl != 0:
markers_target = generate_noise(biorbd_model, q_ref, u_ref,
marker_noise_lvl[marker_lvl],
EMG_noise_lvl[EMG_lvl])[0]
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 original markers
biorbd_model = biorbd.Model(model_path)
# Update bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
if conf["use_activation"] is not True:
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, 0] - 0.1
x_bounds[0].max[:biorbd_model.nbQ() * 2,
0] = x_ref[:biorbd_model.nbQ() * 2, 0] + 0.1
ocp.update_bounds(x_bounds)
# Update initial guess
x0 = x_ref[:biorbd_model.nbQ() * 2,
0] if conf["use_activation"] else x_ref[:, 0]
x_init = InitialGuess(x0, interpolation=InterpolationType.CONSTANT)
u0 = muscles_target
u_init = InitialGuess(u0[:, 0],
interpolation=InterpolationType.CONSTANT)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
ocp.update_objectives(
define_objective(
conf["use_activation"],
conf["use_torque"],
conf["TRACK_EMG"],
0,
rt_ratio,
nbGT,
Ns_mhe,
muscles_target,
markers_target,
conf["with_low_weight"],
biorbd_model,
idx=idx,
TRACK_LESS_EMG=TRACK_LESS_EMG,
))
# Initialize the solver options
if co == 0 and marker_lvl == 0 and EMG_lvl == 0 and tries == 0:
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-4,
"nlp_solver_tol_eq": 1e-4,
"nlp_solver_tol_stat": 1e-4,
"integrator_type": "IRK",
"nlp_solver_type": "SQP",
"sim_method_num_steps": 1,
"print_level": 0,
"nlp_solver_max_iter": 15,
},
)
# Update all allowed solver options
else:
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-4,
"nlp_solver_tol_eq": 1e-4,
"nlp_solver_tol_stat": 1e-4,
},
)
# Save status of optimisation
if sol["status"] != 0 and conf["save_status"]:
if conf["TRACK_EMG"]:
f = open(f"{fold}status_track_rt_EMG{conf['TRACK_EMG']}.txt",
"a")
else:
f = open(f"{fold}status_track_rt_EMG{conf['TRACK_EMG']}.txt",
"a")
f.write(f"{Ns_mhe}; {co}; {marker_lvl}; {EMG_lvl}; {tries}; "
f"'init'\n")
f.close()
# Set solutions and set initial guess for next optimisation
x0, u0, X_est[:, 0], U_est[:, 0] = warm_start_mhe(
ocp, sol, use_activation=conf["use_activation"])
tic = time() # Save initial time
for iter in range(1, ceil(Ns / rt_ratio - Ns_mhe)):
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x0[:, 0]
ocp.nlp[0].x_bounds.max[:, 0] = x0[:, 0]
# Update initial guess
x_init = InitialGuess(x0,
interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess(u0,
interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
ocp.update_objectives(
define_objective(
conf["use_activation"],
conf["use_torque"],
conf["TRACK_EMG"],
iter,
rt_ratio,
nbGT,
Ns_mhe,
muscles_target,
markers_target,
conf["with_low_weight"],
biorbd_model,
idx,
TRACK_LESS_EMG=TRACK_LESS_EMG,
))
# Solve problem
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-4,
"nlp_solver_tol_eq": 1e-4,
"nlp_solver_tol_stat": 1e-4,
},
)
# Set solutions and set initial guess for next optimisation
x0, u0, X_est[:, iter], u_out = warm_start_mhe(
ocp, sol, use_activation=conf["use_activation"])
if iter < int((Ns / rt_ratio) - Ns_mhe):
U_est[:, iter] = u_out
# Compute muscular force at each iteration
q_est = X_est[:biorbd_model.nbQ(), :]
dq_est = X_est[biorbd_model.nbQ():biorbd_model.nbQ() * 2, :]
a_est = np.zeros(
(nbMT, Ns)) if conf["use_activation"] else X_est[-nbMT:, :]
for i in range(biorbd_model.nbMuscles()):
for j in [iter]:
force_est[tries, i,
j] = var["get_force"](q_est[:, j], dq_est[:, j],
a_est[:, j],
U_est[nbGT:, j])[i, :]
# Save status of optimisation
if sol["status"] != 0 and conf["save_status"]:
if conf["TRACK_EMG"]:
f = open(
f"{fold}status_track_rt_EMG{conf['TRACK_EMG']}.txt",
"a")
else:
f = open(
f"{fold}status_track_rt_EMG{conf['TRACK_EMG']}.txt",
"a")
f.write(f"{Ns_mhe}; {co}; {marker_lvl}; {EMG_lvl}; {tries}; "
f"{iter}\n")
f.close()
toc = time() - tic # Save total time to solve
# Store data
X_est_tries[tries, :, :], U_est_tries[tries, :, :] = X_est, U_est
markers_target_tries[tries, :, :, :] = markers_target
muscles_target_tries[tries, :, :] = muscles_target
# Compute reference muscular force
get_force = force_func(biorbd_model, use_activation=False)
for i in range(biorbd_model.nbMuscles()):
for k in range(Ns):
force_ref[i, k] = get_force(q_ref[:, k], dq_ref[:, k],
a_ref[:, k], u_ref[nbGT:, k])[i, :]
# Print some informations about optimisations
print(f"nb loops: {iter}")
print(f"Total time to solve with ACADOS : {toc} s")
print(f"Time per MHE iter. : {toc/iter} s")
tau = np.zeros((nbGT, Ns + 1))
# Compute and print RMSE
err_offset = Ns_mhe
err = compute_err_mhe(
var["init_offset"],
var["final_offset"],
err_offset,
X_est,
U_est,
Ns,
biorbd_model,
q_ref,
dq_ref,
tau,
a_ref,
u_ref,
nbGT,
ratio=rt_ratio,
use_activation=conf["use_activation"],
)
err_tries[int(tries), :] = [
Ns_mhe,
rt_ratio,
toc,
toc / iter,
err["q"],
err["q_dot"],
err["tau"],
err["muscles"],
err["markers"],
err["force"],
]
print(err)
if conf["plot"]:
plot_MHE_results(
biorbd_model,
X_est,
q_ref,
Ns,
rt_ratio,
nbQ,
dq_ref,
U_est,
u_ref,
nbGT,
muscles_target,
force_est,
force_ref,
tries,
markers_target,
conf["use_torque"],
)
return err_tries, force_est, force_ref, X_est_tries, U_est_tries, muscles_target_tries, markers_target_tries, toc
from bioptim import InitialGuess, Solution, Shooting, InterpolationType
import numpy as np
import pendulum
if __name__ == "__main__":
# --- Load pendulum --- #
ocp = pendulum.prepare_ocp(
biorbd_model_path="pendulum.bioMod",
final_time=2,
n_shooting=10,
)
# Simulation the Initial Guess
# Interpolation: Constant
X = InitialGuess([0, 0, 0, 0])
U = InitialGuess([-1, 1])
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS)
print(f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}")
# Uncomment the next line to animate the integration
# s.animate()
# Interpolation: Each frame (for instance, values from a previous optimization or from measured data)
U = np.random.rand(2, 11)
U = InitialGuess(U, interpolation=InterpolationType.EACH_FRAME)
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS)
print(f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}")
def prepare_ocp(
biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g, ode_solver=OdeSolver.RK, use_SX=False
):
# --- 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.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, # ObjectiveFcn 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,
ode_solver=ode_solver,
use_SX=use_SX,
)
def generate_table(out):
root_path = "/".join(__file__.split("/")[:-1])
model_path = root_path + "/models/arm_wt_rot_scap.bioMod"
biorbd_model = biorbd.Model(model_path)
# --- Prepare and solve MHE --- #
t = 8
ns = 800
ns_mhe = 7
rt_ratio = 3
t_mhe = t / (ns / rt_ratio) * ns_mhe
# --- Prepare reference data --- #
with open(
f"{root_path}/data/sim_ac_8000ms_800sn_REACH2_co_level_0_step5_ERK.bob",
"rb") as file:
data = pickle.load(file)
states = data["data"][0]
controls = data["data"][1]
q_ref, dq_ref, u_ref = states["q"], states["qdot"], controls["muscles"]
ocp = prepare_ocp(biorbd_model=biorbd_model,
final_time=t_mhe,
n_shooting=ns_mhe)
q_noise = 5
x_ref = np.concatenate((generate_noise(biorbd_model, q_ref,
q_noise), dq_ref))
x_est = np.zeros(
(biorbd_model.nbQ() * 2, x_ref[:, ::rt_ratio].shape[1] - ns_mhe))
u_est = np.zeros(
(biorbd_model.nbMuscles(), u_ref[:, ::rt_ratio].shape[1] - ns_mhe))
# Update bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].min[:biorbd_model.nbQ(),
0] = x_ref[:biorbd_model.nbQ(), 0] - 0.1
x_bounds[0].max[:biorbd_model.nbQ(),
0] = x_ref[:biorbd_model.nbQ(), 0] + 0.1
ocp.update_bounds(x_bounds)
# Update initial guess
x_init = InitialGuess(x_ref[:, :ns_mhe + 1],
interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess([0.2] * biorbd_model.nbMuscles(),
interpolation=InterpolationType.CONSTANT)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
objectives = define_objective(q_ref, 0, rt_ratio, ns_mhe, biorbd_model)
ocp.update_objectives(objectives)
# Initialize the solver options
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-10,
"nlp_solver_tol_eq": 1e-10,
"nlp_solver_tol_stat": 1e-8,
"integrator_type": "IRK",
"nlp_solver_type": "SQP",
"sim_method_num_steps": 1,
"print_level": 0,
"nlp_solver_max_iter": 30,
},
)
# Set solutions and set initial guess for next optimisation
x0, u0, x_est[:, 0], u_est[:, 0] = warm_start_mhe(sol)
tic = time() # Save initial time
for i in range(1, x_est.shape[1]):
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x0[:, 0]
ocp.nlp[0].x_bounds.max[:, 0] = x0[:, 0]
# Update initial guess
x_init = InitialGuess(x0, interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess(u0, interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
objectives = define_objective(q_ref, i, rt_ratio, ns_mhe, biorbd_model)
ocp.update_objectives(objectives)
# Solve problem
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-6,
"nlp_solver_tol_eq": 1e-6,
"nlp_solver_tol_stat": 1e-5
},
)
# Set solutions and set initial guess for next optimisation
x0, u0, x_out, u_out = warm_start_mhe(sol)
x_est[:, i] = x_out
if i < u_est.shape[1]:
u_est[:, i] = u_out
toc = time() - tic
n = x_est.shape[1] - 1
tf = (ns - ns % rt_ratio) / (ns / t)
final_time = tf - (ns_mhe * (tf / (n + ns_mhe)))
short_ocp = prepare_short_ocp(biorbd_model,
final_time=final_time,
n_shooting=n)
x_init_guess = InitialGuess(x_est,
interpolation=InterpolationType.EACH_FRAME)
u_init_guess = InitialGuess(u_est,
interpolation=InterpolationType.EACH_FRAME)
sol = Solution(short_ocp, [x_init_guess, u_init_guess])
out.solver.append(out.Solver("Acados"))
out.nx = x_est.shape[0]
out.nu = u_est.shape[0]
out.ns = n
out.solver[0].n_iteration = "N.A."
out.solver[0].cost = "N.A."
out.solver[0].convergence_time = toc
out.solver[0].compute_error_single_shooting(sol, 1)
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 = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0)
x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1)))
u_bounds = Bounds(-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 = InitialGuess(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-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 = Bounds(-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1)))
u_bounds = Bounds(-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 = InitialGuess(0.25 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-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 InitialGuess or InitialGuessList"):
ocp.update_initial_guess(x_bounds, u_bounds)
with pytest.raises(
RuntimeError,
match="x_bounds should be built from a Bounds or BoundsList"):
ocp.update_bounds(x_init, u_init)
def test_add_wrong_param():
g_min, g_max, g_init = -10, -6, -8
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
parameters = ParameterList()
initial_gravity = InitialGuess(g_init)
bounds_gravity = Bounds(g_min,
g_max,
interpolation=InterpolationType.CONSTANT)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.Parameter,
target_value=-8)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
[],
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
None,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
None,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
penalty_list=parameter_objective_functions,
extra_value=1,
)
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 = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(g_min,
g_max,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.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 = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1)))
u_bounds = Bounds(-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 = InitialGuess(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-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: str,
n_shooting: int,
final_time: float,
loop_from_constraint: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
loop_from_constraint: bool
If the looping cost should be a constraint [True] or an objective [False]
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.MID,
first_marker="m0",
second_marker="m2")
constraints.add(ConstraintFcn.TRACK_STATE, key="q", node=Node.MID, index=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1")
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# First node is free but mid and last are constrained to be exactly at a certain point.
# The cyclic penalty ensures that the first node and the last node are the same.
x_bounds[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A phase 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
phase_transitions = PhaseTransitionList()
if loop_from_constraint:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0)
else:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
def generate_exc_low_bounds(rate, model_path, X_est, low_exc):
# Variable of the problem
biorbd_model = biorbd.Model(model_path)
q_ref = X_est[: biorbd_model.nbQ(), :]
dq_ref = X_est[biorbd_model.nbQ() : biorbd_model.nbQ() * 2, :]
Ns = q_ref.shape[1] - 1
T = Ns / rate
# Set lower bounds for excitations on muscles in co-contraction (here triceps). Depends on co-contraction level
excitations_max = [1] * biorbd_model.nbMuscleTotal()
excitations_init = []
excitations_min = []
for i in range(len(low_exc)):
excitations_init.append([[0.05] * 6 + [low_exc[i]] * 3 + [0.05] * 10])
excitations_min.append([[0] * 6 + [low_exc[i]] * 3 + [0] * 10])
x0 = np.hstack([q_ref[:, 0], dq_ref[:, 0]])
# Build the graph
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, x0=x0, number_shooting_points=Ns, use_sx=True)
X_est_co = np.ndarray((len(excitations_init), biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles(), X_est.shape[1]))
U_est_co = np.ndarray((len(excitations_init), biorbd_model.nbMuscles(), Ns + 1))
# Solve for all co-contraction levels
for co in range(len(excitations_init)):
# Update excitations and to correspond to the co-contraction level
u_i = excitations_init[co]
u_mi = excitations_min[co]
u_ma = excitations_max
# Update initial guess
u_init = InitialGuess(np.tile(u_i, (ocp.nlp[0].ns, 1)).T, interpolation=InterpolationType.EACH_FRAME)
x_init = InitialGuess(
np.tile(X_est[:, 0], (ocp.nlp[0].ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME
)
ocp.update_initial_guess(x_init, u_init=u_init)
# Update bounds
u_bounds = BoundsList()
u_bounds.add(u_mi[0], u_ma, interpolation=InterpolationType.CONSTANT)
ocp.update_bounds(u_bounds=u_bounds)
# Update objectives functions
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=1000)
objectives.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
weight=100000,
target=X_est[: biorbd_model.nbQ(), :],
index=range(biorbd_model.nbQ()),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
index=range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles()),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1000, index=range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)
)
ocp.update_objectives(objectives)
# Solve the OCP
tic = time()
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
# "nlp_solver_max_iter": 50,
"nlp_solver_tol_comp": 1e-5,
"nlp_solver_tol_eq": 1e-5,
"nlp_solver_tol_stat": 1e-5,
},
)
print(f"Time to solve with ACADOS : {time() - tic} s")
toc = time() - tic
print(f"Total time to solve with ACADOS : {toc} s")
# Get optimal solution
data_est = Data.get_data(ocp, sol)
X_est_co[co, :, :] = np.vstack([data_est[0]["q"], data_est[0]["qdot"], data_est[0]["muscles"]])
U_est_co[co, :, :] = data_est[1]["muscles"]
return X_est_co, U_est_co
def prepare_ocp(biorbd_model_path,
final_time,
n_shooting,
x_warm=None,
use_sx=False,
n_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(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="q",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="qdot",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS,
weight=100000,
first_marker="target",
second_marker="COM_hand")
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (1.0, 1.0, 0, 0)
# Initial guess
if x_warm is None:
x_init = InitialGuess([1.57] * biorbd_model.nbQ() +
[0] * biorbd_model.nbQdot())
else:
x_init = InitialGuess(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,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
n_threads=n_threads,
)
def generate_final_data(rate, X_ref, U_ref, save_data, plot):
# Variable of the problem
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
Ns = X_ref.shape[2] - 1
T = int(Ns / rate)
x0 = X_ref[0, : -biorbd_model.nbMuscles(), 0]
# Build the graph
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, number_shooting_points=Ns, x0=x0, use_SX=True)
X_ref_fin = X_ref
U_ref_fin = U_ref
q_init = X_ref[0, : biorbd_model.nbQ(), :]
# Run problem for all co-contraction level
for co in range(X_ref.shape[0]):
# Get reference data for co-contraction level ranging from 1 to 3
q_ref = X_ref[co, : biorbd_model.nbQ(), :]
dq_ref = X_ref[co, biorbd_model.nbQ() : biorbd_model.nbQ() * 2, :]
u_ref = U_ref[co, :, :]
x_ref_0 = np.hstack([q_ref[:, 0], dq_ref[:, 0], [0.3] * biorbd_model.nbMuscles()])
# Update initial guess
x_init = InitialGuess(np.tile(x_ref_0, (Ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess(u_ref[:, :-1], interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update bounds on state
x_bounds = BoundsList()
x_bounds.add(bounds=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_0[: biorbd_model.nbQ() * 2]
x_bounds[0].max[: biorbd_model.nbQ() * 2, 0] = x_ref_0[: 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()
x_bounds[0].max[biorbd_model.nbQ() * 2 : biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles(), 0] = [
1
] * biorbd_model.nbMuscles()
ocp.update_bounds(x_bounds=x_bounds)
# Update Objectives
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10, index=np.array(range(biorbd_model.nbQ()))
)
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)),
)
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
index=np.array(range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles())),
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=100)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MUSCLES_CONTROL,
weight=10000,
target=u_ref[[9, 10, 17, 18], :-1],
index=np.array([9, 10, 17, 18]),
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE, weight=100000, target=q_init, index=np.array(range(biorbd_model.nbQ()))
)
# Solve OCP
ocp.update_objectives(objective_functions)
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
# "nlp_solver_max_iter": 40,
"nlp_solver_tol_comp": 1e-4,
"nlp_solver_tol_eq": 1e-4,
"nlp_solver_tol_stat": 1e-4,
},
)
# Store optimal solutions
states, controls = Data.get_data(ocp, sol)
X_ref_fin[co, :, :] = np.concatenate((states["q"], states["qdot"], states["muscles"]))
U_ref_fin[co, :, :] = controls["muscles"]
# Save results in .bob file if flag is True
if save_data:
ocp.save_get_data(sol, f"solutions/sim_ac_{int(T * 1000)}ms_{Ns}sn_REACH2_co_level_{co}.bob")
if plot:
t = np.linspace(0, T, Ns + 1)
plt.figure("Muscles controls")
for i in range(biorbd_model.nbMuscles()):
plt.subplot(4, 5, i + 1)
for k in range(X_ref_fin.shape[0]):
plt.step(t, U_ref[k, i, :])
plt.figure("Q")
for i in range(biorbd_model.nbQ()):
plt.subplot(2, 3, i + 1)
for k in range(X_ref_fin.shape[0]):
plt.plot(X_ref[k, i, :])
plt.figure("Q")
plt.figure("dQ")
for i in range(biorbd_model.nbQ()):
plt.subplot(2, 3, i + 1)
for k in range(X_ref_fin.shape[0]):
plt.plot(X_ref[k, i + biorbd_model.nbQ(), :])
plt.show()
return X_ref_fin, U_ref_fin
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
initial_guess: InterpolationType = InterpolationType.CONSTANT,
ode_solver=OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
n_shooting: int
The number of shooting points
final_time: float
The time at the final node
initial_guess: InterpolationType
The type of interpolation to use for the initial guesses
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
# Path constraint and control path constraints
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
u_bounds = Bounds([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, n_shooting + 1))
u = np.random.random((ntau, n_shooting))
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)),
"n_shooting": n_shooting
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"n_shooting": n_shooting
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialGuess(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialGuess(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK1(n_integration_steps=1),
use_sx: bool = False,
n_threads: int = 1,
implicit_dynamics: bool = False,
) -> OptimalControlProgram:
"""
The initialization of an ocp
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
final_time: float
The time in second required to perform the task
n_shooting: int
The number of shooting points to define int the direct multiple shooting program
ode_solver: OdeSolver = OdeSolver.RK4()
Which type of OdeSolver to use
use_sx: bool
If the SX variable should be used instead of MX (can be extensive on RAM)
n_threads: int
The number of threads to use in the paralleling (1 = no parallel computing)
implicit_dynamics: bool
implicit
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN,
implicit_dynamics=implicit_dynamics)
# Path constraint
tau_min, tau_max, tau_init = -100, 100, 0
# Be careful to let the accelerations not to much bounded to find the same solution in implicit dynamics
if implicit_dynamics:
qddot_min, qddot_max, qddot_init = -1000, 1000, 0
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_qddot = biorbd_model.nbQddot()
n_tau = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
# There are extra controls in implicit dynamics which are joint acceleration qddot.
if implicit_dynamics:
u_bounds = Bounds([tau_min] * n_tau + [qddot_min] * n_qddot,
[tau_max] * n_tau + [qddot_max] * n_qddot)
else:
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0 # Prevent the model from actively rotate
if implicit_dynamics:
u_init = InitialGuess([0] * (n_tau + n_qddot))
else:
u_init = InitialGuess([0] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
ode_solver=ode_solver,
use_sx=use_sx,
n_threads=n_threads,
)
