def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound,
max_bound):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
dof_mapping = BiMappingList()
dof_mapping.add("tau", [None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_residual_torque=True,
with_contact=True)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL_SHOOTING,
contact_index=1,
)
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL_SHOOTING,
contact_index=2,
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * len(dof_mapping["tau"].to_first) +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * len(dof_mapping["tau"].to_first) +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first) +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints=constraints,
variable_mappings=dof_mapping,
)
def prepare_ocp(
biorbd_model: tuple,
final_time: list,
nb_shooting: list,
markers_ref: list,
grf_ref: list,
q_ref: list,
qdot_ref: list,
M_ref: list,
CoP: list,
nb_threads: int,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model: tuple
Tuple of bioMod (1 bioMod for each phase)
final_time: list
List of the time at the final node.
The length of the list corresponds to the phase number
nb_shooting: list
List of the number of shooting points
markers_ref: list
List of the array of markers trajectories to track
grf_ref: list
List of the array of ground reaction forces to track
q_ref: list
List of the array of joint trajectories.
Those trajectories were computed using Kalman filter
They are used as initial guess
qdot_ref: list
List of the array of joint velocities.
Those velocities were computed using Kalman filter
They are used as initial guess
M_ref: list
List of the array of ground reaction moments to track
CoP: list
List of the array of the measured center of pressure trajectory
nb_threads:int
The number of threads used
Returns
-------
The OptimalControlProgram ready to be solved
"""
# Problem parameters
nb_phases = len(biorbd_model)
nb_q = biorbd_model[0].nbQ()
nb_qdot = biorbd_model[0].nbQdot()
nb_tau = biorbd_model[0].nbGeneralizedTorque()
nb_mus = biorbd_model[0].nbMuscleTotal()
min_bound, max_bound = 0, np.inf
torque_min, torque_max, torque_init = -1000, 1000, 0
activation_min, activation_max, activation_init = 1e-3, 1.0, 0.1
# Add objective functions
markers_pelvis = [0, 1, 2, 3]
markers_anat = [4, 9, 10, 11, 12, 17, 18]
markers_tissus = [5, 6, 7, 8, 13, 14, 15, 16]
markers_foot = [19, 20, 21, 22, 23, 24, 25]
objective_functions = ObjectiveList()
for p in range(nb_phases):
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=1,
index=range(nb_q),
target=q_ref[p],
phase=p,
quadratic=True)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=1000,
index=markers_anat,
target=markers_ref[p][:, markers_anat, :],
phase=p,
quadratic=True,
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100000,
index=markers_pelvis,
target=markers_ref[p][:, markers_pelvis, :],
phase=p,
quadratic=True,
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100000,
index=markers_foot,
target=markers_ref[p][:, markers_foot, :],
phase=p,
quadratic=True,
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100,
index=markers_tissus,
target=markers_ref[p][:, markers_tissus, :],
phase=p,
quadratic=True,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=0.001,
index=(10),
phase=p,
quadratic=True)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=1,
index=(6, 7, 8, 9, 11),
phase=p,
quadratic=True)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10,
phase=p,
quadratic=True)
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE,
weight=0.1,
phase=p,
quadratic=True)
# --- track contact forces for the stance phase ---
for p in range(nb_phases - 1):
objective_functions.add(
track_sum_contact_forces, # track contact forces
grf=grf_ref[p],
custom_type=ObjectiveFcn.Lagrange,
node=Node.ALL,
weight=0.1,
quadratic=True,
phase=p,
)
for p in range(1, nb_phases - 1):
objective_functions.add(
track_sum_contact_moments,
CoP=CoP[p],
M_ref=M_ref[p],
custom_type=ObjectiveFcn.Lagrange,
node=Node.ALL,
weight=0.01,
quadratic=True,
phase=p,
)
# Dynamics
dynamics = DynamicsList()
for p in range(nb_phases - 1):
dynamics.add(
DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT,
phase=p)
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN, phase=3)
# Constraints
constraints = ConstraintList()
constraints.add( # null speed for the first phase --> non sliding contact point
ConstraintFcn.TRACK_MARKERS_VELOCITY,
node=Node.START,
index=26,
phase=0,
)
# --- phase flatfoot ---
constraints.add( # positive vertical forces
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=(1, 2, 5),
phase=1,
)
constraints.add( # non slipping y
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2, 5),
tangential_component_idx=4,
static_friction_coefficient=0.2,
phase=1,
)
constraints.add( # non slipping x m5
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(2, 5),
tangential_component_idx=3,
static_friction_coefficient=0.2,
phase=1,
)
constraints.add( # non slipping x heel
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.2,
phase=1,
)
constraints.add( # forces heel at zeros at the end of the phase
get_last_contact_force_null,
node=Node.ALL,
contact_name="Heel_r",
phase=1,
)
# --- phase forefoot ---
constraints.add( # positive vertical forces
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=(2, 4, 5),
phase=2,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(2, 4, 5),
tangential_component_idx=1,
static_friction_coefficient=0.2,
phase=2,
)
constraints.add( # non slipping x m1
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=2,
tangential_component_idx=0,
static_friction_coefficient=0.2,
phase=2,
)
constraints.add(
get_last_contact_force_null,
node=Node.ALL,
contact_name="all",
phase=2,
)
# Phase Transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=0)
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
# Path constraint
x_bounds = BoundsList()
u_bounds = BoundsList()
for p in range(nb_phases):
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[p]))
u_bounds.add(
[torque_min] * nb_tau + [activation_min] * nb_mus,
[torque_max] * nb_tau + [activation_max] * nb_mus,
)
# Initial guess
x_init = InitialGuessList()
u_init = InitialGuessList()
n_shoot = 0
for p in range(nb_phases):
init_x = np.zeros((nb_q + nb_qdot, nb_shooting[p] + 1))
init_x[:nb_q, :] = q_ref[p]
init_x[nb_q:nb_q + nb_qdot, :] = qdot_ref[p]
x_init.add(init_x, interpolation=InterpolationType.EACH_FRAME)
init_u = [torque_init] * nb_tau + [activation_init] * nb_mus
u_init.add(init_u)
n_shoot += nb_shooting[p]
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
nb_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
phase_transitions=phase_transitions,
n_threads=nb_threads,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, min_bound,
max_bound):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
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(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model_path = str(
PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK
# --- Options --- #
nb_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(
Constraint.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, 2))
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_bound=min_g,
max_bound=max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:nb_phases],
dynamics,
ns,
final_time[:nb_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
def prepare_ocp(biorbd_model_path="HandSpinner.bioMod"):
end_crank_idx = 0
hand_marker_idx = 18
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Problem parameters
number_shooting_points = 30
final_time = 1.0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, index=hand_marker_idx)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.ALIGN_MARKERS, first_marker_idx=hand_marker_idx, second_marker_idx=end_crank_idx, node=Node.ALL
)
constraints.add(
Constraint.TRACK_STATE,
node=Node.ALL,
index=0,
target=np.linspace(0, 2 * np.pi, number_shooting_points + 1),
)
state_transitions = StateTransitionList()
state_transitions.add(state_transition_function, phase_pre_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Initial guess
x_init = InitialGuessList()
x_init.add([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [0] * biorbd_model.nbQdot())
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[
[tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(),
]
)
u_init = 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,
constraints,
state_transitions=state_transitions,
)
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_TORQUE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# 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 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,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK4(),
weight: float = 1,
min_time=0,
max_time=np.inf,
) -> 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
ode_solver: OdeSolver
The ode solver to use
weight: float
The weighting of the minimize time objective function
min_time: float
The minimum time allowed for the final node
max_time: float
The maximum time allowed for the final node
Returns
-------
The OptimalControlProgram ready to be solved
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
# A weight of -1 will maximize time
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME,
weight=weight,
min_bound=min_time,
max_bound=max_time)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
n_shooting: int
The number of shooting points
final_time: float
The time at the final node
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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
index=1,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque(
) # biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
# Initial guesses
# TODO put this in a function defined before and explain what it does, and what are the variables
x = np.vstack((np.zeros(
(biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2))))
Arm_init_D = np.zeros((3, 2))
Arm_init_D[1, 0] = 0
Arm_init_D[1, 1] = -np.pi + 0.01
Arm_init_G = np.zeros((3, 2))
Arm_init_G[1, 0] = 0
Arm_init_G[1, 1] = np.pi - 0.01
for i in range(2):
Arm_Quat_D = eul2quat(Arm_init_D[:, i])
Arm_Quat_G = eul2quat(Arm_init_G[:, i])
x[6:9, i] = Arm_Quat_D[1:]
x[12, i] = Arm_Quat_D[0]
x[9:12, i] = Arm_Quat_G[1:]
x[13, i] = Arm_Quat_G[0]
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
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,
final_time,
number_shooting_points,
use_SX=True):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
data_to_track = np.zeros((number_shooting_points + 1, n_q + n_qdot))
data_to_track[:, 1] = 3.14
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100.0,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1.0)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
weight=50000.0,
target=data_to_track[-1:, :].T,
node=Node.END,
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([torque_min] * n_tau, [torque_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
)
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,
objective_functions=objective_functions,
)
# --- Solve the program --- #
sol = ocp.solve(show_online_optim=True)
# --- Show results --- #
result = ShowResult(ocp, sol)
result.animate()
def prepare_ocp_phase_transitions(
biorbd_model_path: str,
with_constraints: bool,
with_mayer: bool,
with_lagrange: bool,
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
Returns
-------
The ocp ready to be solved
"""
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
n_shooting = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
if with_lagrange:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=3)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY,
weight=0,
phase=3)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
weight=0,
phase=3,
marker_index=[0, 1])
if with_mayer:
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION,
phase=0,
node=1)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
weight=0,
phase=3,
marker_index=[0, 1])
objective_functions.add(
minimize_difference,
custom_type=ObjectiveFcn.Mayer,
node=Node.TRANSITION,
weight=100,
phase=1,
quadratic=True,
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
if with_constraints:
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0,
list_index=1,
)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=2,
first_marker="m0",
second_marker="m1",
phase=0,
list_index=2)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=0,
list_index=3,
)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=1,
list_index=4,
)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=2,
list_index=5,
)
constraints.add(custom_func_track_markers,
node=Node.ALL,
first_marker="m0",
second_marker="m1",
phase=3,
list_index=6)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -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()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = 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())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# Define phase transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=2)
phase_transitions.add(PhaseTransitionFcn.CYCLIC)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
phase_transitions=phase_transitions,
)
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 prepare_ocp(
biorbd_model_path: str = "cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
all_generalized_mapping = BiMapping([0, 1, 2, -2], [0, 1, 2])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model, all_generalized_mapping))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * all_generalized_mapping.to_first.len * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * all_generalized_mapping.to_first.len, [tau_max] * all_generalized_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * all_generalized_mapping.to_first.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
)
import time
import sys
from bioptim import OptimalControlProgram, ShowResult, Data
from up_and_down_bow import xia_model_dynamic, xia_model_configuration, xia_model_fibers, xia_initial_fatigue_at_zero
file_path = "/home/theophile/Documents/programmation/ViolinOptimalControl/optimal_control_python/results/2020_7_29_upDown.bo"
if len(sys.argv) > 1:
file_path = str(sys.argv[1])
if not isinstance(file_path, str):
t = time.localtime(time.time())
file_path = f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_upDown.bo"
ocp, sol = OptimalControlProgram.load(file_path)
d = Data.get_data(ocp, sol)
result = ShowResult(ocp, sol)
result.graphs()
def prepare_ocp(biorbd_model,
final_time,
x0,
nbGT,
number_shooting_points,
use_SX=False,
nb_threads=8,
use_torque=True):
# --- 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
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsTypeList()
if use_activation and use_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif use_activation is not True and use_torque:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
elif use_activation and use_torque is not True:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
elif use_activation is not True and use_torque is not True:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(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 = 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,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, mu):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
Constraint.CONTACT_FORCE,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=2,
)
constraints.add(
Constraint.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=mu,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
initialize_near_solution: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
constr: bool = True,
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
initialize_near_solution: bool
If the initial guess should be almost the solution (this is merely to reduce the time of the tests)
ode_solver: OdeSolver
The ode solver to use
constr: bool
If the constraint should be applied (this is merely to reduce the time of the tests)
use_sx: bool
If SX CasADi variables should be used
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
if constr:
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=4)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=5)
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker_idx=1,
segment_idx=2,
axis=Axis.X)
else:
constraints = ConstraintList()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0][i, [0, -1]] = 0
x_bounds[0][2, -1] = 1.57
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
x_init[0].init[i] = 1.5
for i in range(4, 6):
x_init[0].init[i] = 0.7
for i in range(6, 8):
x_init[0].init[i] = 0.6
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
def test_double_update_bounds_and_init():
bioptim_folder = TestUtils.bioptim_folder()
biorbd_model = biorbd.Model(bioptim_folder +
"/examples/track/models/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.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, expected)
expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, expected)
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, expected)
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.array([[-2] * (nq * 2) * (ns + 1) + [-4] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, expected)
expected = np.array([[2] * (nq * 2) * (ns + 1) + [4] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, expected)
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.array([[0.25] * (nq * 2) * (ns + 1) + [-0.25] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.init.init, expected)
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 prepare_ocp(
biorbd_model_path,
final_time,
number_shooting_points,
marker_velocity_or_displacement,
marker_in_first_coordinates_system,
control_type,
ode_solver=OdeSolver.RK,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
biorbd_model.markerNames()
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
if marker_in_first_coordinates_system:
# Marker should follow this segment (0 velocity when compare to this one)
coordinates_system_idx = 0
else:
# Marker should be static in global reference frame
coordinates_system_idx = -1
objective_functions = ObjectiveList()
if marker_velocity_or_displacement == "disp":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
coordinates_system_idx=coordinates_system_idx,
index=6,
weight=1000,
)
elif marker_velocity_or_displacement == "velo":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY,
index=6,
weight=1000)
else:
raise RuntimeError(
"Wrong choice of marker_velocity_or_displacement, actual value is "
"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.")
# Make sure the segments actually moves (in order to test the relative speed objective)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=6,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=7,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(nq, 2 * nq):
x_bounds[0].min[i, :] = -10
x_bounds[0].max[i, :] = 10
# Initial guess
x_init = InitialGuessList()
x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6])
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_integration_steps=5,
control_type=control_type,
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/models/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_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
weight: float,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> 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
weight: float
The weight applied to the SUPERIMPOSE_MARKERS final objective function. The bigger this number is, the greater
the model will try to reach the marker. This is in relation with the other objective functions
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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker_idx=0, second_marker_idx=5, weight=weight
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (0.07, 1.4, 0, 0)
# Initial guess
x_init = InitialGuessList()
x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
tau_min, tau_max, tau_init = -1, 1, 0
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,
ode_solver=ode_solver,
)
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 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: str,
final_time: float,
n_shooting: int,
marker_velocity_or_displacement: str,
marker_in_first_coordinates_system: bool,
control_type: ControlType,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare an ocp that targets some marker velocities, either by finite differences or by jacobian
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time of the final node
n_shooting: int
The number of shooting points
marker_velocity_or_displacement: str
which type of tracking: finite difference ('disp') or by jacobian ('velo')
marker_in_first_coordinates_system: bool
If the marker to track should be expressed in the global or local reference frame
control_type: ControlType
The type of controls
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
if marker_in_first_coordinates_system:
# Marker should follow this segment (0 velocity when compare to this one)
coordinates_system_idx = 0
else:
# Marker should be static in global reference frame
coordinates_system_idx = -1
objective_functions = ObjectiveList()
if marker_velocity_or_displacement == "disp":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
coordinates_system_idx=coordinates_system_idx,
index=6,
weight=1000,
)
elif marker_velocity_or_displacement == "velo":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY,
index=6,
weight=1000)
else:
raise RuntimeError(
f"Wrong choice of marker_velocity_or_displacement, actual value is "
f"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.")
# Make sure the segments actually moves (in order to test the relative speed objective)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=6,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=7,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
nq = biorbd_model.nbQ()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(nq, 2 * nq):
x_bounds[0].min[i, :] = -10
x_bounds[0].max[i, :] = 10
# Initial guess
x_init = InitialGuessList()
x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6])
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
control_type=control_type,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
activations_ref: np.ndarray,
q_ref: np.ndarray,
kin_data_to_track: str = "markers",
use_residual_torque: bool = True,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the ocp to solve
Parameters
----------
biorbd_model: biorbd.Model
The loaded biorbd model
final_time: float
The time at final node
n_shooting: int
The number of shooting points
markers_ref: np.ndarray
The marker to track if 'markers' is chosen in kin_data_to_track
activations_ref: np.ndarray
The muscle activation to track
q_ref: np.ndarray
The state to track if 'q' is chosen in kin_data_to_track
kin_data_to_track: str
The type of kin data to track ('markers' or 'q')
use_residual_torque: bool
If residual torque are present or not in the dynamics
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to solve
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MUSCLES_CONTROL,
target=activations_ref)
if use_residual_torque:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
if kin_data_to_track == "markers":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
index=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsList()
if use_residual_torque:
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
nq = biorbd_model.nbQ()
x_bounds[0].min[:nq, :] = -2 * np.pi
x_bounds[0].max[:nq, :] = 2 * np.pi
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
activation_min, activation_max, activation_init = 0, 1, 0.5
u_bounds = BoundsList()
u_init = InitialGuessList()
if use_residual_torque:
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
else:
u_bounds.add([activation_min] * biorbd_model.nbMuscleTotal(),
[activation_max] * biorbd_model.nbMuscleTotal())
u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str = "cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
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, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
constraints.add(ConstraintFcn.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][4:8, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbQ())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
tau_ref: np.ndarray,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model: biorbd.Model
The loaded biorbd model
final_time: float
The time at final node
n_shooting: int
The number of shooting points
markers_ref: np.ndarray
The markers to track
tau_ref: np.ndarray
The generalized forces to track
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
axes=[Axis.Y, Axis.Z],
node=Node.ALL,
weight=100,
target=markers_ref[1:, :, :],
)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL,
key="tau",
target=tau_ref)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuessList()
x_init.add([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 = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path: str = "cube_with_forces.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
# External forces. external_forces is of len 1 because there is only one phase.
# The array inside it is 6x2x30 since there is [Mx, My, Mz, Fx, Fy, Fz] for the two externalforceindex for each node
external_forces = [
np.repeat(
np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], n_shooting, axis=2
)
]
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
external_forces=external_forces,
ode_solver=ode_solver,
)
