def test_penalty_minimize_time(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_TIME
penalty = Objective(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodeList(ocp, ocp.nlp[0], [], [], [], []))
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, np.array(1))
def get_penalty_value(ocp, penalty, t, x, u, p):
val = penalty.type.value[0](penalty,
PenaltyNodeList(ocp, ocp.nlp[0], t, x, u, []),
**penalty.params)
if isinstance(val, float):
return val
states = ocp.nlp[0].states.cx if ocp.nlp[0].states.cx.shape != (
0, 0) else ocp.cx(0, 0)
controls = ocp.nlp[0].controls.cx if ocp.nlp[0].controls.cx.shape != (
0, 0) else ocp.cx(0, 0)
parameters = ocp.nlp[0].parameters.cx if ocp.nlp[
0].parameters.cx.shape != (0, 0) else ocp.cx(0, 0)
return biorbd.to_casadi_func("penalty", val, states, controls,
parameters)(x[0], u[0], p)
def test_penalty_custom_with_bounds_failing_min_bound(value):
def custom_with_bounds(pn):
return -10, pn.nlp.states["q"].cx, 10
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((12, 1)) * value]
u = [0]
penalty_type = ConstraintFcn.CUSTOM
penalty = Constraint(penalty_type)
penalty.min_bound = 0
penalty.custom_function = custom_with_bounds
with pytest.raises(RuntimeError):
penalty_type.value[0](penalty,
PenaltyNodeList(ocp, ocp.nlp[0], t, x, [], []))
def test_penalty_custom_with_bounds_failing_max_bound(value):
def custom_with_bounds(pn):
return -10, pn.nlp.states["q"].cx, 10
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((12, 1)) * value]
u = [0]
penalty_type = ConstraintFcn.CUSTOM
penalty = Constraint(penalty_type)
penalty.max_bound = 0
penalty.custom_function = custom_with_bounds
with pytest.raises(
RuntimeError,
match=
"You cannot have non linear bounds for custom constraints and min_bound or max_bound defined",
):
penalty_type.value[0](penalty,
PenaltyNodeList(ocp, ocp.nlp[0], t, x, [], []))