def save_and_load(sol, ocp, test_solve_of_loaded=False):
ocp.save(sol, "test.bo")
ocp_load, sol_load = OptimalControlProgram.load("test.bo")
TestUtils.deep_assert(sol, sol_load)
TestUtils.deep_assert(sol_load, sol)
if test_solve_of_loaded:
sol_from_load = ocp_load.solve()
TestUtils.deep_assert(sol, sol_from_load)
TestUtils.deep_assert(sol_from_load, sol)
TestUtils.deep_assert(ocp_load, ocp)
TestUtils.deep_assert(ocp, ocp_load)
os.remove("test.bo")
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_TIME)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, [0, -1]] = 0
x_bounds[0].max[:, [0, -1]] = 0
x_bounds[0].min[n_q - 1, -1] = 3.14
x_bounds[0].max[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[0].min[n_tau - 1, :] = 0
u_bounds[0].max[n_tau - 1, :] = 0
u_init = InitialConditionsList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
def test_mayer2_neg_multiphase_time_constraint():
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(Objective.Mayer.MINIMIZE_TIME, phase=2)
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1,
phase=2)
constraints.add(Constraint.TIME_CONSTRAINT,
instant=Instant.END,
minimum=time_min[0],
maximum=time_max[0],
phase=2)
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model, final_time, number_shooting_points, markers_ref,
tau_ref):
# --- Options --- #
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
axis_tot_track=[Axe.Y, Axe.Z],
weight=100,
target=markers_ref)
objective_functions.add(Objective.Lagrange.TRACK_TORQUE, target=tau_ref)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = 0
x_bounds[0].max[:, 0] = 0
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * n_tau, [tau_max] * n_tau])
u_init = InitialConditionsList()
u_init.add([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ()
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, [0, -1]] = 0
X_bounds.max[:, [0, -1]] = 0
X_bounds.min[1, -1] = 3.14
X_bounds.max[1, -1] = 3.14
# Initial guess
X_init = InitialConditions([0] * (n_q + n_qdot))
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * n_tau,
max_bound=[torque_max] * n_tau)
U_bounds.min[n_tau - 1, :] = 0
U_bounds.max[n_tau - 1, :] = 0
U_init = InitialConditions([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, mu):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0, 0, 0]), Mapping([3]))
# Add objective functions
objective_functions = ({"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1},)
# Dynamics
problem_type = ProblemType.torque_driven_with_contact
# Constraints
constraints = ()
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5, 1, 1]
# Initialize X_bounds
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot
X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len, max_bound=[torque_max] * tau_mapping.reduce.len)
U_init = InitialConditions([torque_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def test_lagrange_multiphase_time_constraint():
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(Objective.Lagrange.MINIMIZE_TIME, phase=0)
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1,
phase=2)
constraints.add(Constraint.TIME_CONSTRAINT,
instant=Instant.END,
minimum=time_min[0],
maximum=time_max[0],
phase=2)
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, nb_threads, use_SX=False):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE_DERIVATIVE)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[:, [0, -1]] = 0
x_bounds.max[:, [0, -1]] = 0
x_bounds.min[1, -1] = 3.14
x_bounds.max[1, -1] = 3.14
# Initial guess
x_init = InitialConditionsOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds.min[n_tau - 1, :] = 0
u_bounds.max[n_tau - 1, :] = 0
u_init = InitialConditionsOption([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[:, [0, -1]] = 0
x_bounds.max[:, [0, -1]] = 0
x_bounds.min[1, -1] = 3.14
x_bounds.max[1, -1] = 3.14
# Initial guess
x_init = InitialConditionsOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[torque_min] * n_tau, [torque_max] * n_tau])
u_bounds.min[n_tau - 1, :] = 0
u_bounds.max[n_tau - 1, :] = 0
u_init = InitialConditionsOption([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
)
ocp = prepare_ocp(biorbd_model_path="pendulum.bioMod",
final_time=3,
number_shooting_points=100,
nb_threads=4)
# --- Solve the program --- #
tic = time()
sol = ocp.solve(show_online_optim=False)
toc = time() - tic
print(f"Time to solve : {toc}sec")
# --- Save result of get_data --- #
ocp.save_get_data(
sol, "pendulum.bob") # you don't have to specify the extension ".bob"
# --- Load result of get_data --- #
with open("pendulum.bob", "rb") as file:
data = pickle.load(file)["data"]
# --- Save the optimal control program and the solution --- #
ocp.save(sol,
"pendulum.bo") # you don't have to specify the extension ".bo"
# --- Load the optimal control program and the solution --- #
ocp_load, sol_load = OptimalControlProgram.load("pendulum.bo")
# --- Show results --- #
result = ShowResult(ocp_load, sol_load)
result.graphs()
result.animate()
def prepare_ocp(biorbd_model_path="HandSpinner.bioMod"):
end_crank_idx = 0
hand_marker_idx = 18
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Problem parameters
number_shooting_points = 30
final_time = 1.0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
markers_idx=hand_marker_idx)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
first_marker_idx=hand_marker_idx,
second_marker_idx=end_crank_idx,
instant=Instant.ALL)
constraints.add(
Constraint.TRACK_STATE,
instant=Instant.ALL,
states_idx=0,
target=np.linspace(0, 2 * np.pi, number_shooting_points + 1),
)
state_transitions = StateTransitionList()
state_transitions.add(state_transition_function, phase_pre_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Initial guess
x_init = InitialConditionsList()
x_init.add([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [0] * biorbd_model.nbQdot())
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialConditionsList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
state_transitions=state_transitions,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, direction,
boundary):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_ini = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE_INEQUALITY,
direction=direction,
instant=Instant.ALL,
contact_force_idx=1,
boundary=boundary,
)
constraints.add(
Constraint.CONTACT_FORCE_INEQUALITY,
direction=direction,
instant=Instant.ALL,
contact_force_idx=2,
boundary=boundary,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize X_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialConditionsList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_ini] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(biorbd_model_path="HandSpinner.bioMod"):
end_crank_idx = 0
hand_marker_idx = 18
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Problem parameters
number_shooting_points = 30
final_time = 1.5
# Add objective functions
objective_functions = (
{
"type": Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
"weight": 1,
"markers_idx": hand_marker_idx
},
{
"type": Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
"weight": 1
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 1
},
)
# Dynamics
problem_type = ProblemType.muscle_activations_and_torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"first_marker_idx": hand_marker_idx,
"second_marker_idx": end_crank_idx,
"instant": Instant.ALL,
},
{
"type": Constraint.TRACK_STATE,
"instant": Instant.ALL,
"states_idx": 0,
"data_to_track": np.linspace(0, 2 * np.pi,
number_shooting_points + 1),
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
# Initial guess
X_init = InitialConditions([0, -0.9, 1.7, 0.9, 2.0, -1.3] +
[0] * biorbd_model.nbQdot())
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
is_cyclic_objective=True,
)
def prepare_ocp(
biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_ekf,
use_residual_torque,
kin_data_to_track,
nb_threads,
use_SX=True,
):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.5
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=0.001,
target=excitations_ref)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
weight=1,
target=marker_ref)
elif kin_data_to_track == "q":
objective_functions.add(Objective.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
states_idx=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Add muscle to the bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
x_bounds[0].min[:, 0] = 0 # state_ekf[:, 0]
x_bounds[0].max[:, 0] = 0 # state_ekf[:, 0]
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[0] * biorbd_model.nbMuscles())
# x_init.add(state_ekf, interpolation=InterpolationType.EACH_FRAME)
# Add muscle to the bounds
u_bounds = BoundsList()
u_init = InitialConditionsList()
init_residual_torque = np.concatenate((np.ones(
(biorbd_model.nbGeneralizedTorque(), n_shooting_points)) * 0.5,
excitations_ref))
if use_residual_torque:
u_bounds.add([
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
# u_init.add([torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(init_residual_torque,
interpolation=InterpolationType.EACH_FRAME)
else:
u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(),
[excitation_max] * biorbd_model.nbMuscleTotal()])
u_init.add([0] * biorbd_model.nbMuscleTotal())
# u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME)
# Get initial isometric forces
fiso = []
for k in range(nb_mus):
fiso.append(
biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# Define the parameter to optimize
bound_p_iso = Bounds(min_bound=np.repeat(0.5, nb_mus + 1),
max_bound=np.repeat(3.5, nb_mus + 1),
interpolation=InterpolationType.CONSTANT)
bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus),
max_bound=np.repeat(0, nb_mus),
interpolation=InterpolationType.CONSTANT)
p_iso_init = InitialConditions(np.repeat(1, nb_mus + 1))
initial_guess_A = InitialConditions([-3] * nb_mus)
parameters = ParameterList()
parameters.add(
"p_iso", # The name of the parameter
modify_isometric_force, # The function that modifies the biorbd model
p_iso_init,
bound_p_iso, # The bounds
size=nb_mus +
1, # The number of elements this particular parameter vector has
fiso_init=fiso,
)
# parameters.add(
# "shape_factor", # The name of the parameter
# modify_shape_factor,
# initial_guess_A,
# bound_shape_factor, # The bounds
# size=nb_mus, # The number of elements this particular parameter vector has
# )
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
# parameters=parameters,
)
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
torque_min, torque_max, torque_init = -100, 100, 0
all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]),
Mapping([0, 1, 2]))
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
variable_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker_idx": 0,
"second_marker_idx": 1,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker_idx": 0,
"second_marker_idx": 2,
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model, all_generalized_mapping)
X_bounds.min[3:6, [0, -1]] = 0
X_bounds.max[3:6, [0, -1]] = 0
# Initial guess
X_init = InitialConditions([0] * all_generalized_mapping.reduce.len * 2)
# Define control path constraint
U_bounds = Bounds(
[torque_min] * all_generalized_mapping.reduce.len,
[torque_max] * all_generalized_mapping.reduce.len,
)
U_init = InitialConditions([torque_init] *
all_generalized_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
)
def prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
excitations_ref,
q_ref,
with_residual_torque,
kin_data_to_track="markers",
):
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
# Add objective functions
objective_functions = [
{"type": Objective.Lagrange.TRACK_MUSCLES_CONTROL, "weight": 1, "data_to_track": excitations_ref},
]
if with_residual_torque:
objective_functions.append({"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1})
if kin_data_to_track == "markers":
objective_functions.append(
{"type": Objective.Lagrange.TRACK_MARKERS, "weight": 100, "data_to_track": markers_ref},
)
elif kin_data_to_track == "q":
objective_functions.append(
{
"type": Objective.Lagrange.TRACK_STATE,
"weight": 100,
"data_to_track": q_ref,
"states_idx": range(biorbd_model.nbQ()),
},
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
if with_residual_torque:
variable_type = ProblemType.muscle_excitations_and_torque_driven
else:
variable_type = ProblemType.muscle_excitations_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
X_bounds.min[[0, 1], :] = -2 * np.pi
X_bounds.max[[0, 1], :] = 2 * np.pi
# Add muscle to the bounds
X_bounds.concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
# Initial guess
X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())
# Define control path constraint
if with_residual_torque:
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(),
[torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(),
)
U_init = InitialConditions(
[torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles()
)
else:
U_bounds = Bounds([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles(),)
U_init = InitialConditions([excitation_init] * biorbd_model.nbMuscles())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
nb_shooting,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
)
def prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
excitations_ref,
q_ref,
kin_data_to_track="markers",
show_online_optim=False,
):
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
# Add objective functions
objective_functions = [
{
"type": Objective.Lagrange.TRACK_MUSCLES_CONTROL,
"weight": 1,
"data_to_track": excitations_ref
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 10000
},
]
if kin_data_to_track == "markers":
objective_functions.append(
{
"type": Objective.Lagrange.TRACK_MARKERS,
"weight": 100,
"data_to_track": markers_ref
}, )
elif kin_data_to_track == "q":
objective_functions.append(
{
"type": Objective.Lagrange.TRACK_STATE,
"weight": 100,
"data_to_track": q_ref,
"states_idx": range(biorbd_model.nbQ()),
}, )
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
variable_type = ProblemType.muscle_excitations_and_torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.first_node_min += [activation_min] * biorbd_model.nbMuscleTotal()
X_bounds.first_node_max += [activation_max] * biorbd_model.nbMuscleTotal()
X_bounds.min += [activation_min] * biorbd_model.nbMuscleTotal()
X_bounds.max += [activation_max] * biorbd_model.nbMuscleTotal()
X_bounds.last_node_min += [activation_min] * biorbd_model.nbMuscleTotal()
X_bounds.last_node_max += [activation_max] * biorbd_model.nbMuscleTotal()
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[activation_init] *
biorbd_model.nbMuscleTotal())
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscleTotal(),
)
U_init = InitialConditions(
[torque_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
nb_shooting,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
show_online_optim=show_online_optim,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points,
ode_solver, initialize_near_solution):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker_idx": 0,
"second_marker_idx": 4,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker_idx": 0,
"second_marker_idx": 5,
},
{
"type": Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS,
"instant": Instant.ALL,
"marker_idx": 1,
"segment_idx": 2,
"axis": (Axe.X),
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
for i in range(1, 8):
if i != 3:
X_bounds.min[i, [0, -1]] = 0
X_bounds.max[i, [0, -1]] = 0
X_bounds.min[2, -1] = 1.57
X_bounds.max[2, -1] = 1.57
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
X_init.init[i] = 1.5
for i in range(4, 6):
X_init.init[i] = 0.7
for i in range(6, 8):
X_init.init[i] = 0.6
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque(),
[torque_max] * biorbd_model.nbGeneralizedTorque(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_nlp(biorbd_model_path="../models/BrasViolon.bioMod",
show_online_optim=True):
"""
Mix .bioMod and users data to call OptimalControlProgram constructor.
:param biorbd_model_path: path to the .bioMod file.
:param show_online_optim: bool which active live plot function.
:return: OptimalControlProgram object.
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
torque_min, torque_max, torque_init = -100, 100, 0
# Problem parameters
number_shooting_points = 31
final_time = 0.5
# Choose the string of the violin
violon_string = Violin("E")
inital_bow_side = Bow("frog")
# Add objective functions
objective_functions = (
{
"type": ObjectiveFunction.minimize_torque,
"weight": 100
},
{
"type": ObjectiveFunction.minimize_muscle,
"weight": 1
},
)
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.Type.MARKERS_TO_PAIR,
"instant": Constraint.Instant.START,
"first_marker": Bow.frog_marker,
"second_marker": violon_string.bridge_marker,
},
{
"type": Constraint.Type.MARKERS_TO_PAIR,
"instant": Constraint.Instant.MID,
"first_marker": Bow.tip_marker,
"second_marker": violon_string.bridge_marker,
},
{
"type": Constraint.Type.MARKERS_TO_PAIR,
"instant": Constraint.Instant.END,
"first_marker": Bow.frog_marker,
"second_marker": violon_string.bridge_marker,
},
{
"type": Constraint.Type.ALIGN_WITH_CUSTOM_RT,
"instant": Constraint.Instant.ALL,
"segment": Bow.segment_idx,
"rt": violon_string.rt_on_string,
},
# TODO: add constraint about velocity in a marker of bow (start and end instant)
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
for i in range(biorbd_model.nbQ(), biorbd_model.nbQdot()):
X_bounds.first_node_min[k] = 0
X_bounds.first_node_max[k] = 0
X_bounds.last_node_min[k] = 0
X_bounds.last_node_max[k] = 0
# Initial guess
X_init = InitialConditions(
violon_string.initial_position()[inital_bow_side.side] +
[0] * biorbd_model.nbQdot())
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
show_online_optim=show_online_optim,
)
def prepare_ocp(
final_time, time_min, time_max, number_shooting_points, biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK
):
# --- Options --- #
nb_phases = len(number_shooting_points)
if nb_phases != 1 and nb_phases != 3:
raise RuntimeError("Number of phases must be 1 to 3")
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0)
if nb_phases == 3:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=0)
if nb_phases == 3:
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.START, first_marker_idx=0, second_marker_idx=1, phase=0)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=2, phase=0)
constraints.add(Constraint.TIME_CONSTRAINT, instant=Instant.END, minimum=time_min[0], maximum=time_max[0], phase=0)
if nb_phases == 3:
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=1, phase=1)
constraints.add(
Constraint.TIME_CONSTRAINT, instant=Instant.END, minimum=time_min[1], maximum=time_max[1], phase=1
)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=2, phase=2)
constraints.add(
Constraint.TIME_CONSTRAINT, instant=Instant.END, minimum=time_min[2], maximum=time_max[2], phase=2
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 0
if nb_phases == 3:
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds.min[i, [0, -1]] = 0
bounds.max[i, [0, -1]] = 0
x_bounds[0].min[2, 0] = 0.0
x_bounds[0].max[2, 0] = 0.0
if nb_phases == 3:
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if nb_phases == 3:
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()])
if nb_phases == 3:
u_bounds.add(
[[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]
)
u_bounds.add(
[[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]
)
u_init = InitialConditionsList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
if nb_phases == 3:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model[:nb_phases],
dynamics,
number_shooting_points,
final_time[:nb_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
import time
import sys
import pickle
from biorbd_optim import OptimalControlProgram, ShowResult, Data, Simulate
from up_and_down_bow import xia_model_dynamic, xia_model_configuration, xia_model_fibers, xia_initial_fatigue_at_zero
file_path = "results/xia 5 phases/2020_7_25_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, Simulate.from_solve(ocp, sol, single_shoot=True))
dict = {"data": d}
with open(file_path[:-3] + "_single.bob", "wb") as file:
pickle.dump(dict, file)
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = nqdot # biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_MARKERS,
markers_idx=1,
weight=-1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * ntau, [tau_max] * ntau])
# Initial guesses
x = np.vstack((np.zeros(
(biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2))))
Arm_init_D = np.zeros((3, 2))
Arm_init_D[1, 0] = 0
Arm_init_D[1, 1] = -np.pi + 0.01
Arm_init_G = np.zeros((3, 2))
Arm_init_G[1, 0] = 0
Arm_init_G[1, 1] = np.pi - 0.01
for i in range(2):
Arm_Quat_D = eul2quat(Arm_init_D[:, i])
Arm_Quat_G = eul2quat(Arm_init_G[:, i])
x[6:9, i] = Arm_Quat_D[1:]
x[12, i] = Arm_Quat_D[0]
x[9:12, i] = Arm_Quat_G[1:]
x[13, i] = Arm_Quat_G[0]
x_init = InitialConditionsList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
u_init = InitialConditionsList()
u_init.add([tau_init] * ntau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
)
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
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.CUSTOM,
"function": custom_func_align_markers,
"instant": Instant.START,
"first_marker_idx": 0,
"second_marker_idx": 1,
},
{
"type": Constraint.CUSTOM,
"function": custom_func_align_markers,
"instant": Instant.END,
"first_marker_idx": 0,
"second_marker_idx": 2,
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[1:6, [0, -1]] = 0
X_bounds.max[1:6, [0, -1]] = 0
X_bounds.min[2, -1] = 1.57
X_bounds.max[2, -1] = 1.57
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque(),
[torque_max] * biorbd_model.nbGeneralizedTorque(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_align_markers,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[1:6, [0, -1]] = 0
x_bounds.max[1:6, [0, -1]] = 0
x_bounds.min[2, -1] = 1.57
x_bounds.max[2, -1] = 1.57
# Initial guess
x_init = InitialConditionsOption(
[0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialConditionsOption([tau_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path="cubeSym.bioMod",
show_online_optim=False,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
variable_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker": 0,
"second_marker": 1,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker": 0,
"second_marker": 2,
},
{
"type": Constraint.PROPORTIONAL_STATE,
"instant": Instant.ALL,
"first_dof": 2,
"second_dof": 3,
"coef": -1,
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
for i in range(4, 8):
X_bounds.first_node_min[i] = 0
X_bounds.last_node_min[i] = 0
X_bounds.first_node_max[i] = 0
X_bounds.last_node_max[i] = 0
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbQ(),
[torque_max] * biorbd_model.nbQ(),
)
U_init = InitialConditions([torque_init] * biorbd_model.nbQ())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
show_online_optim=show_online_optim,
)
def prepare_ocp(biorbd_model_path,
number_shooting_points,
final_time,
loop_from_constraint,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = [{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}]
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = (
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.MID,
"first_marker_idx": 0,
"second_marker_idx": 2,
},
{
"type": Constraint.TRACK_STATE,
"instant": Instant.MID,
"states_idx": 2,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker_idx": 0,
"second_marker_idx": 1,
},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[2:6, -1] = [1.57, 0, 0, 0]
X_bounds.max[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque(),
[torque_max] * biorbd_model.nbGeneralizedTorque(),
)
U_init = InitialConditions([torque_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
is_cyclic_objective=not loop_from_constraint,
is_cyclic_constraint=loop_from_constraint,
)
def prepare_ocp(biorbd_model_path="cube.bioMod",
show_online_optim=False,
ode_solver=OdeSolver.RK,
long_optim=False):
# --- Options --- #
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
if long_optim:
number_shooting_points = (100, 1000)
else:
number_shooting_points = (20, 30)
final_time = (2, 5)
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = (
({
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}, ),
({
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}, ),
)
# Dynamics
variable_type = (ProblemType.torque_driven, ProblemType.torque_driven)
# Constraints
constraints = (
(
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.START,
"first_marker": 0,
"second_marker": 1,
},
{
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker": 0,
"second_marker": 2,
},
),
({
"type": Constraint.ALIGN_MARKERS,
"instant": Instant.END,
"first_marker": 0,
"second_marker": 1,
}, ),
)
# Path constraint
X_bounds = [
QAndQDotBounds(biorbd_model[0]),
QAndQDotBounds(biorbd_model[0])
]
for bounds in X_bounds:
for i in range(6):
if i != 0 and i != 2:
bounds.first_node_min[i] = 0
bounds.last_node_min[i] = 0
bounds.first_node_max[i] = 0
bounds.last_node_max[i] = 0
X_bounds[0].first_node_min[2] = 0.0
X_bounds[0].first_node_max[2] = 0.0
X_bounds[1].first_node_min[2] = 0.0
X_bounds[1].first_node_max[2] = 0.0
X_bounds[1].last_node_min[2] = 1.57
X_bounds[1].last_node_max[2] = 1.57
# Initial guess
X_init = InitialConditions(
[0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
U_bounds = [
Bounds(
[torque_min] * biorbd_model[0].nbGeneralizedTorque(),
[torque_max] * biorbd_model[0].nbGeneralizedTorque(),
),
Bounds(
[torque_min] * biorbd_model[0].nbGeneralizedTorque(),
[torque_max] * biorbd_model[0].nbGeneralizedTorque(),
),
]
U_init = InitialConditions([torque_init] *
biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
objective_functions,
(X_init, X_init),
(U_init, U_init),
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
show_online_optim=show_online_optim,
)
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
use_SX=False,
nb_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -5, 5, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=100)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Add muscle to the bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# Following values are taken from Belaise's matlab code
x_bounds[0].min[:,
0] = (-0.2, 0.1, -0.25, 0.1, 0, -0, -0.2, 0.05, -0.15,
-0.02, 0, 0.28) + (0, ) * biorbd_model.nbMuscles()
x_bounds[0].max[:,
0] = (-0.2, 0.1, -0.25, 0.1, 0, -0, -0.2, 0.05, -0.15,
-0.02, 0, 0.28) + (0, ) * biorbd_model.nbMuscles()
x_bounds[0].min[:biorbd_model.nbQ() * 2,
-1] = (-0.03, 0.1, -0.1, 0.2, -0.76, 1., 2., -1.5, -0.17,
-0.62, 1.4, -0.57)
x_bounds[0].max[:biorbd_model.nbQ() * 2,
-1] = (-0.03, 0.1, -0.1, 0.2, -0.76, 1., 2., -1.5, -0.17,
-0.62, 1.4, -0.57)
# Initial guess
x_init = InitialConditionsList()
x_init.add([1.] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot() +
[0] * biorbd_model.nbMuscles())
# Define control path constraint
u_bounds = BoundsList()
u_init = InitialConditionsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscles(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscles(),
])
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscles())
# ------------- #
return OptimalControlProgram(biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads)
OptimalControlProgram,
Data,
ShowResult,
)
mat_contents = sio.loadmat("./data/excitations.mat")
excitations_ref = mat_contents["excitations"]
excitations_ref = excitations_ref[:, :]
mat_contents = sio.loadmat("./data/markers.mat")
marker_ref = mat_contents["markers"]
marker_ref = marker_ref[:, 4:, :]
mat_contents = sio.loadmat("./data/x_init.mat")
x_init = mat_contents["x_init"][: 2 * 6, :]
ocp, sol = OptimalControlProgram.load("activation_driven_ipopt.bo")
states, controls = Data.get_data(ocp, sol["x"])
biorbd_model = biorbd.Model("arm_Belaise.bioMod")
q = states["q"]
qdot = states["q_dot"]
x = vertcat(states["q"], states["q_dot"])
u = controls["muscles"]
nlp = ocp.nlp[0]
result = ShowResult(ocp, sol)
# result.animate()
# result.graphs()
with open("sol_marker_activation_tracking_acados.bob", 'rb' ) as file :
data = pickle.load(file)
states_ac = data['data'][0]
def prepare_ocp(
biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_init,
use_residual_torque,
activation_driven,
kin_data_to_track,
nb_threads,
use_SX=True,
):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.3
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, weight=10, target=excitations_ref)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.01)
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1)
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS, weight=1000,
target=marker_ref[:, -biorbd_model.nbMarkers():, :]
)
elif kin_data_to_track == "q":
objective_functions.add(
Objective.Lagrange.TRACK_STATE, weight=100,
# target=q_ref,
# states_idx=range(biorbd_model.nbQ())
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif activation_driven:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Constraints
constraints = ()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
if use_SX:
x_bounds[0].min[:, 0] = np.concatenate((state_init[6:biorbd_model.nbQ() + 6, 0],
state_init[biorbd_model.nbQ() + 12:-nb_mus, 0]))
x_bounds[0].max[:, 0] = np.concatenate((state_init[6:biorbd_model.nbQ() + 6, 0],
state_init[biorbd_model.nbQ() + 12:-nb_mus, 0]))
# Add muscle to the bounds
if activation_driven is not True:
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
# Initial guess
x_init = InitialConditionsList()
if activation_driven:
# state_base = np.ndarray((12, n_shooting_points+1))
# for i in range(n_shooting_points+1):
# state_base[:, i] = np.concatenate((state_init[:6, 0], np.zeros((6))))
x_init.add(np.concatenate((state_init[6:biorbd_model.nbQ() + 6, :],
state_init[biorbd_model.nbQ() + 12:-nb_mus, :])),
interpolation=InterpolationType.EACH_FRAME)
# x_init.add(state_init[:-nb_mus, :], interpolation=InterpolationType.EACH_FRAME)
else:
# x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())
x_init.add(state_init[biorbd_model.nbQ():, :], interpolation=InterpolationType.EACH_FRAME)
# Add muscle to the bounds
u_bounds = BoundsList()
u_init = InitialConditionsList()
nb_tau = 6
# init_residual_torque = np.concatenate((np.ones((biorbd_model.nbGeneralizedTorque(), n_shooting_points))*0.5,
# excitations_ref))
if use_residual_torque:
u_bounds.add(
[
[torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(),
]
)
u_init.add(
[torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal())
# u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME)
else:
u_bounds.add(
[[excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()])
if activation_driven:
# u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME)
else:
# u_init.add([excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME)
# Get initial isometric forces
fiso = []
for k in range(nb_mus):
fiso.append(biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
# parameters=parameters
)
