def prepare_ocp(model_path, phase_time, number_shooting_points):
# --- 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]), Mapping([3]))
# Add objective functions
objective_functions = (
{
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 1 / 100
},
)
# 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]
# 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 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
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": 0, "second_marker": 1,},
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2,},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model, all_generalized_mapping)
for i in range(3, 6):
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] * 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,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
show_online_optim=show_online_optim,
)
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, use_actuators=False, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
if use_actuators:
torque_min, torque_max, torque_init = -1, 1, 0
else:
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100}
# Dynamics
if use_actuators:
problem_type = ProblemType.torque_activations_driven
else:
problem_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)
X_bounds.min[3:6, [0, -1]] = 0
X_bounds.max[3:6, [0, -1]] = 0
X_bounds.min[2, [0, -1]] = [0, 1.57]
X_bounds.max[2, [0, -1]] = [0, 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, final_time, number_shooting_points,
nb_threads):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -1000000, 1000000, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = {
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 100
}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, 0] = 0
X_bounds.max[:, 0] = 0
X_bounds.min[:n_q, -1] = 1
X_bounds.max[:n_q, -1] = 1
X_bounds.min[n_q:, -1] = 0
X_bounds.max[n_q:, -1] = 0
# 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)
# Control initial guess
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,
nb_threads=nb_threads,
)
def test_initial_condition_constant():
nb_elements = 6
nb_shoot = 10
init_val = np.random.random(nb_elements, )
init = InitialConditions(init_val,
interpolation=InterpolationType.CONSTANT)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
expected_val = init_val
for i in range(nb_shoot):
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points,
time_min, time_max):
# --- 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 = {"type": Objective.Lagrange.MINIMIZE_TORQUE}
# Dynamics
problem_type = ProblemType.torque_driven
# Constraints
constraints = ({
"type": Constraint.TIME_CONSTRAINT,
"minimum": time_min,
"maximum": time_max,
}, )
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, [0, -1]] = 0
X_bounds.max[:, [0, -1]] = 0
X_bounds.min[n_q - 1, -1] = 3.14
X_bounds.max[n_q - 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 test_initial_condition_linear():
nb_elements = 6
nb_shoot = 10
init_val = np.random.random((nb_elements, 2))
init = InitialConditions(init_val, interpolation=InterpolationType.LINEAR)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
expected_val = init_val[:, 0] + (init_val[:, 1] -
init_val[:, 0]) * i / nb_shoot
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_initial_condition_each_frame():
nb_elements = 6
nb_shoot = 10
init_val = np.random.random((nb_elements, nb_shoot + 1))
init = InitialConditions(init_val,
interpolation=InterpolationType.EACH_FRAME)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
expected_val = init_val[:, i]
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -1, 1, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = (
{"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1},
{"type": Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, "weight": 1},
{"type": Objective.Mayer.ALIGN_MARKERS, "first_marker_idx": 0, "second_marker_idx": 5, "weight": 1,},
)
# Dynamics
problem_type = ProblemType.muscle_activations_and_torque_driven
# Constraints
constraints = ()
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
# Set the initial position
X_bounds.min[:, 0] = (0.07, 1.4, 0, 0)
X_bounds.max[:, 0] = (0.07, 1.4, 0, 0)
# Initial guess
X_init = InitialConditions([1.57] * biorbd_model.nbQ() + [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,
)
def test_initial_condition_constant_with_first_and_last_different():
nb_elements = 6
nb_shoot = 10
init_val = np.random.random((nb_elements, 3))
init = InitialConditions(
init_val,
interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
if i == 0:
expected_val = init_val[:, 0]
elif i == nb_shoot:
expected_val = init_val[:, 2]
else:
expected_val = init_val[:, 1]
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_simulate_from_initial_single_shoot():
# Load pendulum
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"pendulum",
str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.py")
pendulum = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum)
ocp = pendulum.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/pendulum.bioMod",
final_time=2,
number_shooting_points=10,
nb_threads=4,
)
X = InitialConditions([-1, -2, 1, 0.5])
U = InitialConditions(np.array([[-0.1, 0], [1, 2]]).T,
interpolation=InterpolationType.LINEAR)
sol_simulate_single_shooting = Simulate.from_controls_and_initial_states(
ocp, X, U, single_shoot=True)
# Check some of the results
states, controls = Data.get_data(ocp, sol_simulate_single_shooting["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(-0.59371229, 2.09731719)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5)))
np.testing.assert_almost_equal(qdot[:, -1],
np.array((1.38153013, -0.60425128)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((1.0, 2.0)))
def test_initial_condition_custom():
nb_elements = 6
nb_shoot = 10
def custom_bound_func(current_shooting, val, total_shooting):
# Linear interpolation created with custom bound function
return val[:, 0] + (val[:, 1] -
val[:, 0]) * current_shooting / total_shooting
init_val = np.random.random((nb_elements, 2))
init = InitialConditions(
custom_bound_func,
interpolation=InterpolationType.CUSTOM,
val=init_val,
total_shooting=nb_shoot,
)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
expected_val = init_val[:, 0] + (init_val[:, 1] -
init_val[:, 0]) * i / nb_shoot
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_initial_condition_spline():
nb_shoot = 10
spline_time = np.hstack((0.0, 1.0, 2.2, 6.0))
init_val = np.array([
[0.5, 0.6, 0.2, 0.8],
[0.4, 0.6, 0.8, 0.2],
[0.0, 0.3, 0.2, 0.5],
[0.5, 0.2, 0.9, 0.4],
[0.5, 0.6, 0.2, 0.8],
[0.5, 0.6, 0.2, 0.8],
])
nb_elements = init_val.shape[0]
# Raise if time is not sent
with pytest.raises(RuntimeError):
InitialConditions(init_val, interpolation=InterpolationType.SPLINE)
init = InitialConditions(init_val,
t=spline_time,
interpolation=InterpolationType.SPLINE)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
time_to_test = [0, nb_shoot // 3, nb_shoot // 2, nb_shoot]
expected_matrix = np.array([
[0.5, 0.4, 0.0, 0.5, 0.5, 0.5],
[
0.33333333, 0.73333333, 0.23333333, 0.66666667, 0.33333333,
0.33333333
],
[
0.32631579, 0.67368421, 0.26315789, 0.79473684, 0.32631579,
0.32631579
],
[0.8, 0.2, 0.5, 0.4, 0.8, 0.8],
]).T
for i, t in enumerate(time_to_test):
expected_val = expected_matrix[:, i]
np.testing.assert_almost_equal(init.init.evaluate_at(t), expected_val)
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,
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,
final_time,
number_shooting_points,
ode_solver,
marker_velocity_or_displacement,
marker_in_first_coordinates_system,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
biorbd_model.markerNames()
# Problem parameters
torque_min, torque_max, torque_init = -100, 100, 0
# Add objective functions
if marker_in_first_coordinates_system:
coordinates_system_idx = 0
else:
coordinates_system_idx = -1
if marker_velocity_or_displacement == "disp":
objective_functions = (
{
"type": Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
"coordinates_system_idx": coordinates_system_idx,
"markers_idx": 6,
"weight": 1000,
},
{
"type": Objective.Lagrange.MINIMIZE_STATE,
"states_idx": 6,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_STATE,
"states_idx": 7,
"weight": -1
},
)
elif marker_velocity_or_displacement == "velo":
objective_functions = (
{
"type": Objective.Lagrange.MINIMIZE_MARKERS_VELOCITY,
"markers_idx": 6,
"weight": 1000
},
{
"type": Objective.Lagrange.MINIMIZE_STATE,
"states_idx": 6,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_STATE,
"states_idx": 7,
"weight": -1
},
)
else:
raise RuntimeError(
"Wrong choice of marker_velocity_or_displacement, actual value is "
"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.")
# Dynamics
problem_type = ProblemType.torque_driven
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model)
for i in range(nq, 2 * nq):
X_bounds.min[i, :] = -10
X_bounds.max[i, :] = 10
# Initial guess
X_init = InitialConditions([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
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,
ode_solver=ode_solver,
)
def prepare_ocp(
show_online_optim=False,
use_symmetry=True,
):
# --- Options --- #
# Model path
biorbd_model = (
biorbd.Model("jumper2contacts.bioMod"),
biorbd.Model("jumper1contacts.bioMod"),
)
nb_phases = len(biorbd_model)
torque_min, torque_max, torque_init = -1000, 1000, 0
# Problem parameters
number_shooting_points = [8, 8]
phase_time = [0.4, 0.2]
if use_symmetry:
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]),
Mapping([0, 1, 2, 4, 7, 8, 9]))
q_mapping = q_mapping, q_mapping
tau_mapping = BidirectionalMapping(
Mapping([-1, -1, -1, -1, 0, -1, 0, 1, 2, 3, 1, 2, 3], [5]),
Mapping([4, 7, 8, 9]))
tau_mapping = tau_mapping, tau_mapping
else:
q_mapping = BidirectionalMapping(
Mapping([i for i in range(biorbd_model[0].nbQ())]),
Mapping([i for i in range(biorbd_model[0].nbQ())]),
)
q_mapping = q_mapping, q_mapping
tau_mapping = q_mapping
# Add objective functions
objective_functions = (
(),
(
{
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_ALL_CONTROLS,
"weight": 1 / 100
},
),
)
# Dynamics
problem_type = (
ProblemType.torque_driven_with_contact,
ProblemType.torque_driven_with_contact,
)
constraints_first_phase = []
constraints_second_phase = []
contact_axes = (1, 2, 4, 5)
for i in contact_axes:
constraints_first_phase.append({
"type": Constraint.CONTACT_FORCE_GREATER_THAN,
"instant": Instant.ALL,
"idx": i,
"boundary": 0,
})
contact_axes = (1, 3)
for i in contact_axes:
constraints_second_phase.append({
"type": Constraint.CONTACT_FORCE_GREATER_THAN,
"instant": Instant.ALL,
"idx": i,
"boundary": 0,
})
constraints_first_phase.append({
"type": Constraint.NON_SLIPPING,
"instant": Instant.ALL,
"normal_component_idx": (1, 2, 4, 5),
"tangential_component_idx": (0, 3),
"static_friction_coefficient": 0.5,
})
constraints_second_phase.append({
"type": Constraint.NON_SLIPPING,
"instant": Instant.ALL,
"normal_component_idx": (1, 3),
"tangential_component_idx": (0, 2),
"static_friction_coefficient": 0.5,
})
if not use_symmetry:
first_dof = (3, 4, 7, 8, 9)
second_dof = (5, 6, 10, 11, 12)
coeff = (-1, 1, 1, 1, 1)
for i in range(len(first_dof)):
constraints_first_phase.append({
"type": Constraint.PROPORTIONAL_STATE,
"instant": Instant.ALL,
"first_dof": first_dof[i],
"second_dof": second_dof[i],
"coef": coeff[i],
})
for i in range(len(first_dof)):
constraints_second_phase.append({
"type": Constraint.PROPORTIONAL_STATE,
"instant": Instant.ALL,
"first_dof": first_dof[i],
"second_dof": second_dof[i],
"coef": coeff[i],
})
constraints = (constraints_first_phase, constraints_second_phase)
# Path constraint
if use_symmetry:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47]
else:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [
0,
0,
-0.5336,
0,
1.4,
0,
1.4,
0.8,
-0.9,
0.47,
0.8,
-0.9,
0.47,
]
# Initialize X_bounds
X_bounds = [
QAndQDotBounds(biorbd_model[i], all_generalized_mapping=q_mapping[i])
for i in range(nb_phases)
]
X_bounds[0].first_node_min = pose_at_first_node + [0] * nb_qdot
X_bounds[0].first_node_max = pose_at_first_node + [0] * nb_qdot
# Initial guess
X_init = [
InitialConditions(pose_at_first_node + [0] * nb_qdot),
InitialConditions(pose_at_first_node + [0] * nb_qdot),
]
# Define control path constraint
U_bounds = [
Bounds(min_bound=[torque_min] * tau_m.reduce.len,
max_bound=[torque_max] * tau_m.reduce.len)
for tau_m in tau_mapping
]
U_init = [
InitialConditions([torque_init] * tau_m.reduce.len)
for tau_m in tau_mapping
]
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
show_online_optim=show_online_optim,
)
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_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, 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_ocp(biorbd_model_path="../models/BrasViolon.bioMod"):
"""
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.
"""
optimal_initial_values = False
nb_phases = 2
biorbd_model = []
objective_functions = ObjectiveList()
dynamics = DynamicsTypeList()
constraints = ConstraintList()
x_bounds = BoundsList()
u_bounds = BoundsList()
x_init = InitialConditionsList()
u_init = InitialConditionsList()
# --- Options --- #
number_shooting_points = [20] * nb_phases
final_time = [1] * nb_phases
muscle_activated_init, muscle_fatigued_init, muscle_resting_init = 0, 0, 1
torque_min, torque_max, torque_init = -10, 10, 0
muscle_states_min, muscle_states_max = 0, 1
# --- Aliasing --- #
model_tp = biorbd.Model(biorbd_model_path)
n_q = model_tp.nbQ()
n_qdot = model_tp.nbQdot()
n_tau = model_tp.nbGeneralizedTorque()
n_mus = model_tp.nbMuscles()
violon_string = Violin("G")
inital_bow_side = Bow("frog")
# --- External forces --- #
external_forces = [
np.repeat(violon_string.external_force[:, np.newaxis], number_shooting_points[0], axis=1)
] * nb_phases
for idx_phase in range(nb_phases):
biorbd_model.append(biorbd.Model(biorbd_model_path))
# --- Objective --- #
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=1, phase=idx_phase)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1, phase=idx_phase)
objective_functions.add(
Objective.Lagrange.ALIGN_SEGMENT_WITH_CUSTOM_RT,
weight=1,
segment_idx=Bow.segment_idx,
rt_idx=violon_string.rt_on_string,
phase=idx_phase,
)
objective_functions.add(
Objective.Lagrange.MINIMIZE_TORQUE, controls_idx=[0, 1, 2, 3], weight=10, phase=idx_phase
)
# --- Dynamics --- #
dynamics.add(xia.xia_model_configuration, dynamic_function=xia.xia_model_dynamic)
# --- Constraints --- #
if idx_phase == 0:
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.START,
min_bound=-0.00001,
max_bound=0.00001,
first_marker_idx=Bow.frog_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
)
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.MID,
min_bound=-0.00001,
max_bound=0.00001,
first_marker_idx=Bow.tip_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
)
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.END,
min_bound=-0.00001,
max_bound=0.00001,
first_marker_idx=Bow.frog_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
)
# constraints.add(
# Constraint.ALIGN_SEGMENT_WITH_CUSTOM_RT,
# instant=Instant.ALL,
# min_bound=-0.00001,
# max_bound=0.00001,
# segment_idx=Bow.segment_idx,
# rt_idx=violon_string.rt_on_string,
# phase=idx_phase,
# )
# constraints.add(
# Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS,
# instant=Instant.ALL,
# min_bound=-0.00001,
# max_bound=0.00001,
# marker_idx=violon_string.bridge_marker,
# segment_idx=Bow.segment_idx,
# axis=(Axe.Y),
# phase=idx_phase,
# )
constraints.add(
Constraint.ALIGN_MARKERS,
instant=Instant.ALL,
first_marker_idx=Bow.contact_marker,
second_marker_idx=violon_string.bridge_marker,
phase=idx_phase,
# TODO: add constraint about velocity in a marker of bow (start and end instant)
)
# --- Path constraints --- #
x_bounds.add(QAndQDotBounds(biorbd_model[0]), phase=idx_phase)
# Start and finish with zero velocity
if idx_phase == 0:
x_bounds[idx_phase][n_q :, 0] = 0
if idx_phase == nb_phases - 1:
x_bounds[idx_phase][n_q :, -1] = 0
muscle_states_bounds = Bounds(
[muscle_states_min] * n_mus * 3,
[muscle_states_max] * n_mus * 3,
)
if idx_phase == 0:
# fatigued_fibers = activated_fibers = 0 and resting_fibers = 1 at start
muscle_states_bounds[:2 * n_mus, 0] = 0
muscle_states_bounds[2 * n_mus:, 0] = 1
x_bounds[idx_phase].concatenate(muscle_states_bounds)
u_bounds.add(
[[torque_min] * n_tau + [muscle_states_min] * n_mus, [torque_max] * n_tau + [muscle_states_max] * n_mus],
phase=idx_phase,
)
# --- Initial guess --- #
if optimal_initial_values:
# TODO: Fix this part (avoid using .bio)
raise NotImplementedError("optimal_initial_values = True should be reviewed")
if idx_phase == 0:
with open(f"utils/optimal_init_{number_shooting_points[0]}_nodes_constr_first.bio", "rb") as file:
dict = pickle.load(file)
else:
with open(f"utils/optimal_init_{number_shooting_points[0]}_nodes_constr_first.bio", "rb") as file:
dict = pickle.load(file)
x_init.add(dict["states"], interpolation=InterpolationType.EACH_FRAME, phase=idx_phase)
u_init.add(dict["controls"], interpolation=InterpolationType.EACH_FRAME, phase=idx_phase)
else:
# TODO: x_init could be a LINEAR from frog to tip
x_init.add(
violon_string.initial_position()[inital_bow_side.side] + [0] * n_qdot,
interpolation=InterpolationType.CONSTANT,
phase=idx_phase,
)
muscle_states_init = InitialConditions(
[muscle_activated_init] * n_mus + [muscle_fatigued_init] * n_mus + [muscle_resting_init] * n_mus,
interpolation=InterpolationType.CONSTANT,
)
x_init[idx_phase].concatenate(muscle_states_init)
u_init.add(
[torque_init] * n_tau + [muscle_activated_init] * n_mus,
interpolation=InterpolationType.CONSTANT,
phase=idx_phase,
)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
external_forces=external_forces,
ode_solver=OdeSolver.RK,
nb_threads=4,
)
def prepare_ocp(
model_path,
phase_time,
number_shooting_points,
muscle_activations_ref,
contact_forces_ref,
):
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
# Add objective functions
objective_functions = (
{
"type": Objective.Lagrange.TRACK_MUSCLES_CONTROL,
"weight": 1,
"data_to_track": muscle_activations_ref
},
{
"type": Objective.Lagrange.TRACK_CONTACT_FORCES,
"weight": 1,
"data_to_track": contact_forces_ref
},
)
# Dynamics
problem_type = ProblemType.muscles_activations_and_torque_driven_with_contact
# Constraints
constraints = ()
# 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 = 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] * biorbd_model.nbGeneralizedTorque() +
[activation_min] * biorbd_model.nbMuscleTotal(),
max_bound=[torque_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
]
U_init = [
InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
]
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions=objective_functions,
constraints=constraints,
)
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,
param=False):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.3
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# -- Force iso ipopt pour acados
# if param is not True:
# fiso = []
# for k in range(nb_mus):
# fiso.append(biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# mat_content = sio.loadmat("./results/param_f_iso_flex.mat")
# f_iso = mat_content["f_iso"] * mat_content["f_iso_global"]
# for k in range(biorbd_model.nbMuscles()):
# biorbd_model.muscle(k).characteristics().setForceIsoMax(
# f_iso[k] * fiso[k]
# )
# 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())
# Define the parameter to optimize
bound_p_iso = Bounds(
# min_bound=np.repeat(0.75, nb_mus), max_bound=np.repeat(3, nb_mus), interpolation=InterpolationType.CONSTANT)
min_bound=[0.5] * nb_mus + [0.75],
max_bound=[3.5] * nb_mus + [3],
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([1] * nb_mus + [2])
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(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]), 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 = (
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": "GREATER_THAN",
"instant": Instant.ALL,
"contact_force_idx": 1,
"boundary": 0,
},
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": "GREATER_THAN",
"instant": Instant.ALL,
"contact_force_idx": 2,
"boundary": 0,
},
{
"type": Constraint.NON_SLIPPING,
"instant": Instant.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 = 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 prepare_ocp(model_path, phase_time, number_shooting_points, direction,
boundary):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ({
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
}, )
# Dynamics
problem_type = ProblemType.muscle_excitations_and_torque_driven_with_contact
# Constraints
constraints = (
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": direction,
"instant": Instant.ALL,
"contact_force_idx": 1,
"boundary": boundary,
},
{
"type": 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
nb_mus = biorbd_model.nbMuscleTotal()
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize X_bounds
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.concatenate(
Bounds([activation_min] * nb_mus, [activation_max] * nb_mus))
X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
# Initial guess
X_init = [
InitialConditions(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus)
]
# Define control path constraint
U_bounds = [
Bounds(
min_bound=[torque_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
max_bound=[torque_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
]
U_init = [
InitialConditions([torque_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
]
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions=objective_functions,
constraints=constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g,
max_g, target_g):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=10)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.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[1, :] = 0
u_bounds.max[1, :] = 0
u_init = InitialConditionsOption([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_bound=min_g,
max_bound=max_g,
interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialConditions((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", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=
parameter_objective_functions, # Objective of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
def prepare_ocp(biorbd_model_path="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_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,
number_shooting_points,
final_time,
initial_guess=InterpolationType.CONSTANT):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
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": 1,
},
{
"type": Constraint.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
# Define control path constraint
U_bounds = Bounds(
[torque_min] * ntau,
[torque_max] * ntau,
)
# Initial guesses
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [torque_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, number_shooting_points + 1))
u = np.random.random((ntau, number_shooting_points))
else:
raise RuntimeError("Initial guess not implemented yet")
X_init = InitialConditions(x, interpolation_type=initial_guess)
U_init = InitialConditions(u, interpolation_type=initial_guess)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
)