def test_penalty_custom(penalty_origin, value):
def custom(ocp, nlp, t, x, u, p, mult):
my_values = DM.zeros((12, 1)) + x[0] * mult
return my_values
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.CUSTOM
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type, index=0)
else:
penalty = ConstraintOption(penalty_type, index=0)
penalty.custom_function = custom
mult = 2
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [], mult=mult)
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(res, np.array([[value * mult]] * 12))
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]] * 12))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]] * 12))
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[1, :] = 0
u_init = InitialGuessOption([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=target_g
)
parameters.add(
"gravity_z", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=parameter_objective_functions, # Objective of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
def test_penalty_custom_fail(penalty_origin, value):
def custom_no_mult(ocp, nlp, t, x, u, p):
my_values = DM.zeros((12, 1)) + x[0]
return my_values
def custom_with_mult(ocp, nlp, t, x, u, p, mult):
my_values = DM.zeros((12, 1)) + x[0] * mult
return my_values
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.CUSTOM
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type)
else:
penalty = ConstraintOption(penalty_type)
with pytest.raises(TypeError):
penalty.custom_function = custom_no_mult
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [], mult=2)
with pytest.raises(TypeError):
penalty.custom_function = custom_with_mult
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
with pytest.raises(TypeError):
keywords = [
"phase",
"list_index",
"name",
"type",
"params",
"node",
"quadratic",
"index",
"target",
"sliced_target",
"min_bound",
"max_bound",
"custom_function",
"weight",
]
for keyword in keywords:
exec(f"""def custom_with_keyword(ocp, nlp, t, x, u, p, {keyword}):
my_values = DM.zeros((12, 1)) + x[index]
return my_values""")
exec("""penalty.custom_function = custom_with_keyword""")
exec(
f"""penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [], {keyword}=0)"""
)
def test_penalty_minimize_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MARKERS_VELOCITY
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
if value == 0.1:
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][6]["val"],
np.array([
[-0.00499167],
[0],
[-0.0497502],
]),
)
else:
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][6]["val"],
np.array([
[2.7201056],
[0],
[-4.1953576],
]),
)
def test_penalty_track_torque(penalty_origin, value):
ocp = prepare_test_ocp()
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_TORQUE
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type, target=np.ones((4, 1)) * value)
else:
penalty = ConstraintOption(penalty_type,
target=np.ones((4, 1)) * value)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [4], [], u, [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value]]).T,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.zeros((4, 1)))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.zeros((4, 1)))
def test_penalty_align_marker_with_segment_axis(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.ALIGN_MARKER_WITH_SEGMENT_AXIS
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type)
else:
penalty = ConstraintOption(penalty_type)
penalty_type.value[0](penalty,
ocp,
ocp.nlp[0], [],
x, [], [],
marker_idx=0,
segment_idx=1,
axis=Axe.X)
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array([[0]]),
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]]))
def test_penalty_track_state(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_STATE
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type, target=np.ones((8, 1)) * value)
else:
penalty = ConstraintOption(penalty_type,
target=np.ones((8, 1)) * value)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [1], x, [], [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
expected = np.array([[value]] * 8)
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]] * 8))
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]] * 8),
)
def test_penalty_track_all_controls(penalty_origin, value):
ocp = prepare_test_ocp(with_muscles=True)
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_ALL_CONTROLS
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type, target=np.ones((8, 1)) * value)
else:
penalty = ConstraintOption(penalty_type,
target=np.ones((8, 1)) * value)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [6], [], u, [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value, value, value, value, value]]).T,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].min,
np.array([[0.0, 0, 0, 0, 0, 0, 0, 0]]).T)
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].max,
np.array([[0.0, 0, 0, 0, 0, 0, 0, 0]]).T)
def test_penalty_proportional_state(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.PROPORTIONAL_STATE
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type)
else:
penalty = ConstraintOption(penalty_type)
penalty_type.value[0](penalty,
ocp,
ocp.nlp[0], [],
x, [], [],
which_var="states",
first_dof=0,
second_dof=1,
coef=2)
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array([[-value]]),
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]]))
def test_penalty_minimize_contact_forces(penalty_origin, value):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_CONTACT_FORCES
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, u, [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([[-9.6680105, 127.2360329, 5.0905995]]).T,
)
else:
np.testing.assert_almost_equal(
res,
np.array([[25.6627161, 462.7973306, -94.0182191]]).T,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0.0]]).T)
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0.0]]).T)
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_align_markers,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuessOption([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessOption([tau_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def test_penalty_minimize_time(penalty_origin, value):
ocp = prepare_test_ocp()
penalty_type = penalty_origin.MINIMIZE_TIME
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], [], [], [])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"],
np.array(1),
)
def test_penalty_minimize_markers_displacement(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MARKERS_DISPLACEMENT
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0],
np.array([]),
)
def test_penalty_minimize_state(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_STATE
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"],
np.array([[value]] * 8),
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points,
time_min, time_max):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintOption(Constraint.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min,
max_bound=time_max)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuessOption([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_penalty_minimize_predicted_com_height(value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
res = np.array(0.0501274)
if value == -10:
res = np.array([[-3.72579]])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"],
res,
)
def test_penalty_align_markers(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.ALIGN_MARKERS
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type)
else:
penalty = ConstraintOption(penalty_type)
penalty_type.value[0](penalty,
ocp,
ocp.nlp[0], [],
x, [], [],
first_marker_idx=0,
second_marker_idx=1)
expected = np.array([
[-0.8951707],
[0],
[1.0948376],
])
if value == -10:
expected = np.array([
[1.3830926],
[0],
[-0.2950504],
])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]] * 3),
)
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]] * 3),
)
def test_penalty_track_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_MARKERS_VELOCITY
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type,
target=np.ones((3, 7, 1)) * value)
else:
penalty = ConstraintOption(penalty_type,
target=np.ones((3, 7, 1)) * value)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [3], x, [], [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][6]["val"]
else:
res = ocp.nlp[0].g[0][6]
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([
[-0.00499167],
[0],
[-0.0497502],
]),
)
else:
np.testing.assert_almost_equal(
res,
np.array([
[2.7201056],
[0],
[-4.1953576],
]),
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]] * 3),
)
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]] * 3),
)
def test_penalty_track_markers(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_MARKERS
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type,
target=np.ones((3, 7, 1)) * value)
else:
penalty = ConstraintOption(penalty_type,
target=np.ones((3, 7, 1)) * value)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [2], x, [], [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
expected = np.array([
[0.1, 0.99517075, 1.9901749, 1.0950042, 1, 2, 0.49750208],
[0, 0, 0, 0, 0, 0, 0],
[0.1, -0.9948376, -1.094671, 0.000166583, 0, 0, -0.0499167],
])
if value == -10:
expected = np.array([
[-10, -11.3830926, -12.2221642, -10.8390715, 1.0, 2.0, -0.4195358],
[0, 0, 0, 0, 0, 0, 0],
[-10, -9.7049496, -10.2489707, -10.5440211, 0, 0, -0.2720106],
])
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].min,
np.array([[0]] * 3),
)
np.testing.assert_almost_equal(
ocp.nlp[0].g_bounds[0][0].max,
np.array([[0]] * 3),
)
def test_penalty_minimize_muscles_control(penalty_origin, value):
ocp = prepare_test_ocp(with_muscles=True)
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MUSCLES_CONTROL
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], [], u, [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value, value, value]]).T,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0.0, 0, 0, 0, 0, 0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0.0, 0, 0, 0, 0, 0]]))
def test_penalty_minimize_torque_derivative(value):
ocp = prepare_test_ocp()
u = [DM.ones((12, 1)) * value, DM.ones((12, 1)) * value * 3]
penalty_type = Objective.Lagrange.MINIMIZE_TORQUE_DERIVATIVE
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], [], u, [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array([[value * 2, value * 2, value * 2, value * 2]]).T,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0.0, 0, 0, 0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0.0, 0, 0, 0]]))
def test_penalty_track_contact_forces(penalty_origin, value):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.TRACK_CONTACT_FORCES
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type,
target=np.ones((1, 1)) * value,
index=0)
else:
penalty = ConstraintOption(penalty_type,
target=np.ones((1, 1)) * value,
index=0)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [7], x, u, [])
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([[-9.6680105]]),
)
else:
np.testing.assert_almost_equal(
res,
np.array([[25.6627161]]),
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0.0]]).T)
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0.0]]).T)
def test_penalty_minimize_markers(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MARKERS
penalty = ObjectiveOption(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
res = np.array([
[0.1, 0.99517075, 1.9901749, 1.0950042, 1, 2, 0.49750208],
[0, 0, 0, 0, 0, 0, 0],
[0.1, -0.9948376, -1.094671, 0.000166583, 0, 0, -0.0499167],
])
if value == -10:
res = np.array([
[-10, -11.3830926, -12.2221642, -10.8390715, 1.0, 2.0, -0.4195358],
[0, 0, 0, 0, 0, 0, 0],
[-10, -9.7049496, -10.2489707, -10.5440211, 0, 0, -0.2720106],
])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"],
res,
)
def test_penalty_proportional_control(penalty_origin, value):
ocp = prepare_test_ocp()
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.PROPORTIONAL_CONTROL
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type)
else:
penalty = ConstraintOption(penalty_type)
first = 0
second = 1
coef = 2
penalty_type.value[0](penalty,
ocp,
ocp.nlp[0], [], [],
u, [],
which_var="controls",
first_dof=first,
second_dof=second,
coef=coef)
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(
res,
np.array(u[0][first] - coef * u[0][second]),
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0.0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0.0]]))
def test_penalty_align_segment_with_custom_rt(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.ALIGN_SEGMENT_WITH_CUSTOM_RT
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
penalty = ObjectiveOption(penalty_type)
else:
penalty = ConstraintOption(penalty_type)
penalty_type.value[0](penalty,
ocp,
ocp.nlp[0], [],
x, [], [],
segment_idx=1,
rt_idx=0)
if isinstance(penalty_type, (Objective.Lagrange, Objective.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]
expected = np.array([[0], [0.1], [0]])
if value == -10:
expected = np.array([[3.1415927], [0.575222], [3.1415927]])
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[0], [0], [0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[0], [0], [0]]))
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
initial_guess=InterpolationType.CONSTANT,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint and control path constraints
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
u_bounds = BoundsOption([[tau_min] * ntau, [tau_max] * ntau])
# Initial guesses
t = None
extra_params_x = {}
extra_params_u = {}
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [tau_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, number_shooting_points + 1))
u = np.random.random((ntau, number_shooting_points))
elif initial_guess == InterpolationType.SPLINE:
# Bound spline assume the first and last point are 0 and final respectively
t = np.hstack((0, np.sort(np.random.random(
(3, )) * final_time), final_time))
x = np.random.random((nq + nqdot, 5))
u = np.random.random((ntau, 5))
elif initial_guess == InterpolationType.CUSTOM:
# The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation
x = custom_init_func
u = custom_init_func
extra_params_x = {
"my_values": np.random.random((nq + nqdot, 2)),
"nb_shooting": number_shooting_points
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"nb_shooting": number_shooting_points
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialGuessOption(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialGuessOption(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path,
number_shooting_points,
final_time,
loop_from_constraint,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.MID,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(Constraint.TRACK_STATE, node=Node.MID, index=2)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialGuessOption([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessOption([tau_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
# A state transition loop constraint is treated as
# hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or
# as a soft penalty (objective) otherwise
state_transitions = StateTransitionList()
if loop_from_constraint:
state_transitions.add(StateTransition.CYCLIC, weight=0)
else:
state_transitions.add(StateTransition.CYCLIC, weight=10000)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
state_transitions=state_transitions,
)
def test_add_wrong_param():
g_min, g_max, g_init = -10, -6, -8
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
parameters = ParameterList()
initial_gravity = InitialGuess(g_init)
bounds_gravity = Bounds(min_bound=g_min,
max_bound=g_max,
interpolation=InterpolationType.CONSTANT)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=-8)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
[],
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
InitialGuess(),
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
Bounds(),
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
penalty_list=parameter_objective_functions,
extra_value=1,
)
def test_update_bounds_and_init_with_param():
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
g_min, g_max, g_init = -10, -6, -8
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(min_bound=g_min,
max_bound=g_max,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=-8)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
ocp = OptimalControlProgram(biorbd_model,
dynamics,
ns,
1.0,
parameters=parameters)
x_bounds = BoundsOption([-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))])
u_bounds = BoundsOption([-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))])
ocp.update_bounds(x_bounds, u_bounds)
expected = np.append(
np.tile(np.append(-np.ones((nq * 2, 1)), -2.0 * np.ones((nq, 1))), ns),
-np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_bounds.min,
np.append([g_min], expected).reshape(129, 1))
expected = np.append(
np.tile(np.append(np.ones((nq * 2, 1)), 2.0 * np.ones((nq, 1))), ns),
np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_bounds.max,
np.append([[g_max]], expected).reshape(129, 1))
x_init = InitialGuessOption(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuessOption(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.append(
np.tile(np.append(0.5 * np.ones((nq * 2, 1)), -0.5 * np.ones((nq, 1))),
ns), 0.5 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_init.init,
np.append([g_init], expected).reshape(129, 1))
def prepare_ocp(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model_path = str(
PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK
# --- Options --- #
nb_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(
Constraint.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, 2))
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_bound=min_g,
max_bound=max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:nb_phases],
dynamics,
ns,
final_time[:nb_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
