def test_penalty_minimize_comddot(value, penalty_origin, implicit):
ocp = prepare_test_ocp(with_contact=True, implicit=implicit)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_COM_ACCELERATION
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
if not implicit:
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([[0.0], [-0.7168803], [-0.0740871]])
if value == -10:
expected = np.array([[0.0], [1.455063], [16.3741091]])
np.testing.assert_almost_equal(res, expected)
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([[0], [-0.0008324], [0.002668]])
if value == -10:
expected = np.array([[0], [-17.5050533], [-18.2891901]])
np.testing.assert_almost_equal(res, expected)
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((8, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((8, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [1], x, [],
[]))
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
expected = np.array([[value]] * 8)
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]] * 8))
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]] * 8),
)
def test_penalty_track_markers(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.TRACK_MARKERS
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((3, 7, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((3, 7, 1)) * value)
res = get_penalty_value(ocp, penalty, t, x, u, [])
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)
def test_penalty_minimize_com_position(value, penalty_origin):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
if "TRACK_COM_POSITION" in penalty_origin._member_names_:
penalty_type = penalty_origin.TRACK_COM_POSITION
else:
penalty_type = penalty_origin.MINIMIZE_COM_POSITION
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, [],
[]))
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
expected = np.array([[0.05], [0.05], [0.05]])
if value == -10:
expected = np.array([[-5], [0.05], [-5]])
np.testing.assert_almost_equal(res, expected)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array(0))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array(0))
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((4, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((4, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [4], [], u,
[]))
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value]]).T,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.zeros((4, 1)))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.zeros((4, 1)))
def test_penalty_minimize_comddot(value, penalty_origin, implicit):
ocp = prepare_test_ocp(with_contact=True, implicit=implicit)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_COM_ACCELERATION
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
if not implicit:
with pytest.raises(
NotImplementedError,
match=re.escape(
"MINIMIZE_COM_ACCELERATION is only working if qddot is defined as a state or a control."
),
):
res = get_penalty_value(ocp, penalty, t, x, u, [])
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([[0], [-0.0008324], [0.002668]])
if value == -10:
expected = np.array([[0], [-17.5050533], [-18.2891901]])
np.testing.assert_almost_equal(res, expected)
def test_tau_max_from_actuators(value, threshold):
ocp = prepare_test_ocp(with_actuator=True)
t = [0]
x = [DM.zeros((6, 1)), DM.zeros((6, 1))]
u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value]
penalty_type = ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT
penalty = Constraint(penalty_type, min_torque=threshold)
if threshold and threshold < 0:
with pytest.raises(
ValueError,
match="min_torque cannot be negative in tau_max_from_actuators"
):
get_penalty_value(ocp, penalty, t, x, u, [])
return
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
if threshold:
np.testing.assert_almost_equal(
res,
np.repeat([value + threshold, value - threshold], 3)[:,
np.newaxis])
else:
np.testing.assert_almost_equal(
res,
np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, [], []),
which_var="states",
first_dof=0,
second_dof=1,
coef=2)
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(
res,
np.array([[-value]]),
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]]))
def test_penalty_track_marker_with_segment_axis(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_MARKER_WITH_SEGMENT_AXIS
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, [], []),
marker="m0",
segment="ground",
axis=Axis.X)
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(
res,
np.array([[0]]),
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]]))
def test_penalty_track_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.TRACK_MARKERS_VELOCITY
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((3, 7, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((3, 7, 1)) * value)
res = get_penalty_value(ocp, penalty, t, x, u, [])
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([
[0.1, -0.00948376, -0.0194671, 0.0900167, 00, 00, -0.00499167],
[0, 0, 0, 0, 00, 00, 0],
[0.1, 0.0104829, -0.0890175, 0.000499583, 0, 0, -0.0497502],
]),
)
else:
np.testing.assert_almost_equal(
res,
np.array([
[-10, -12.9505, -7.51029, -4.55979, 00, 00, 2.72011],
[0, 0, 0, 0, 00, 00, 0],
[-10, -23.8309, -32.2216, -18.3907, 0, 0, -4.19536],
]),
decimal=4,
)
def test_penalty_non_slipping(value):
ocp = prepare_test_ocp(with_contact=True)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = ConstraintFcn.NON_SLIPPING
penalty = Constraint(penalty_type)
penalty_type.value[0](
penalty,
PenaltyNodes(ocp, ocp.nlp[0], t, x, u, []),
tangential_component_idx=0,
normal_component_idx=1,
static_friction_coefficient=2,
)
res = []
for i in range(len(ocp.nlp[0].g[0])):
res.append(ocp.nlp[0].g[0][i]["val"])
if value == 0.1:
expected = [[64662.56185612, 64849.5027121], [0, 0], [np.inf, np.inf]]
elif value == -10:
expected = [[856066.90177734, 857384.05177395], [0, 0],
[np.inf, np.inf]]
else:
raise RuntimeError("Test not ready")
np.testing.assert_almost_equal(
np.concatenate(res)[:, 0], np.array(expected[0]))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([expected[1]]).T)
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([expected[2]]).T)
def test_penalty_custom(penalty_origin, value):
def custom(pn, mult):
my_values = DM.zeros((12, 1)) + pn.x[0] * mult
return my_values
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.CUSTOM
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, index=0)
else:
penalty = Constraint(penalty_type, index=0)
penalty.custom_function = custom
mult = 2
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], t, x, [], []),
mult=mult)
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(res, np.array([[value * mult]] * 12))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]] * 12))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]] * 12))
def test_tau_max_from_actuators(value, threshold):
ocp = prepare_test_ocp(with_actuator=True)
x = [DM.zeros((6, 1)), DM.zeros((6, 1))]
u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value]
penalty_type = ConstraintFcn.TORQUE_MAX_FROM_ACTUATORS
penalty = Constraint(penalty_type)
if threshold and threshold < 0:
with pytest.raises(
ValueError,
match="min_torque cannot be negative in tau_max_from_actuators"
):
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
min_torque=threshold),
else:
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
min_torque=threshold)
val = []
for i in range(len(ocp.nlp[0].g[0])):
val.append(ocp.nlp[0].g[0][i]["val"])
for res in val:
if threshold:
np.testing.assert_almost_equal(
res,
np.repeat([value + threshold, value - threshold],
3)[:, np.newaxis])
else:
np.testing.assert_almost_equal(
res,
np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def test_penalty_non_slipping(value):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = ConstraintFcn.NON_SLIPPING
penalty = Constraint(penalty_type)
penalty_type.value[0](
penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
tangential_component_idx=0,
normal_component_idx=1,
static_friction_coefficient=2,
)
res = []
for i in range(len(ocp.nlp[0].g[0])):
res.append(ocp.nlp[0].g[0][i]["val"])
if value == 0.1:
expected = [[264.1400764, 244.8040553], 0, np.inf]
elif value == -10:
expected = [[899.9319451, 951.2573773], 0, np.inf]
np.testing.assert_almost_equal(res, np.array(expected[0]))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[expected[1]]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[expected[2]]]))
def test_penalty_proportional_control(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [0]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.PROPORTIONAL_CONTROL
first = 0
second = 1
coef = 2
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type,
key="tau",
first_dof=first,
second_dof=second,
coef=coef)
else:
penalty = Constraint(penalty_type,
key="tau",
first_dof=first,
second_dof=second,
coef=coef)
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res,
np.array(u[0][first] - coef * u[0][second]))
def test_penalty_track_segment_with_custom_rt(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_SEGMENT_WITH_CUSTOM_RT
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, [], []),
segment="ground",
rt_idx=0)
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
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, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0], [0], [0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0], [0], [0]]))
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((8, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((8, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [6], [], u,
[]))
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value, value, value, value, value]]).T,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0.0, 0, 0, 0, 0, 0, 0, 0]]).T)
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0.0, 0, 0, 0, 0, 0, 0, 0]]).T)
def test_penalty_time_constraint(value):
ocp = prepare_test_ocp()
t = [0]
x = [0]
u = [0]
penalty_type = ConstraintFcn.TIME_CONSTRAINT
penalty = Constraint(penalty_type)
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, np.array([]))
def test_penalty_time_constraint(value):
ocp = prepare_test_ocp()
penalty_type = ConstraintFcn.TIME_CONSTRAINT
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], [], [],
[]))
res = ocp.nlp[0].g[0]
np.testing.assert_almost_equal(
res,
np.array([]),
)
def test_penalty_contact_force_inequality(penalty_origin, value):
ocp = prepare_test_ocp(with_contact=True)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.TRACK_CONTACT_FORCES
penalty = Constraint(penalty_type, contact_index=0)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = [[-9.6680105, 127.2360329, 5.0905995]
] if value == 0.1 else [[25.6627161, 462.7973306, -94.0182191]]
np.testing.assert_almost_equal(res.T, expected)
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(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_contact_force_inequality(penalty_origin, value, direction):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
if direction == "GREATER_THAN":
min_bound = 1
max_bound = np.inf
if value == 0.1:
expected = [-9.6680105, 1.0, np.inf]
elif value == -10:
expected = [25.6627161, 1.0, np.inf]
else:
raise RuntimeError("Wrong test")
elif direction == "LESSER_THAN":
min_bound = -np.inf
max_bound = 1
if value == 0.1:
expected = [-9.6680105, -np.inf, 1.0]
elif value == -10:
expected = [25.6627161, -np.inf, 1.0]
else:
raise RuntimeError("Wrong test")
else:
raise RuntimeError("Wrong test")
penalty_type = penalty_origin.CONTACT_FORCE
penalty = Constraint(penalty_type,
min_bound=min_bound,
max_bound=max_bound)
penalty_type.value[0](
penalty,
ocp,
ocp.nlp[0],
[],
x,
u,
[],
contact_force_idx=0,
)
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(res, np.array([[expected[0]]]))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[expected[1]]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[expected[2]]]))
def test_penalty_custom_with_bounds(value):
def custom_with_bounds(pn):
return -10, pn.nlp.states["q"].cx, 10
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty = Constraint(custom_with_bounds)
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, [[value]] * 4)
np.testing.assert_almost_equal(penalty.min_bound, -10)
np.testing.assert_almost_equal(penalty.max_bound, 10)
def test_penalty_non_slipping(value):
ocp = prepare_test_ocp(with_contact=True)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = ConstraintFcn.NON_SLIPPING
penalty = Constraint(penalty_type,
tangential_component_idx=0,
normal_component_idx=1,
static_friction_coefficient=2)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = [[64662.56185612, 64849.5027121]
] if value == 0.1 else [[856066.90177734, 857384.05177395]]
np.testing.assert_almost_equal(res.T, expected)
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]] * 3),
)
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]] * 3),
)
def test_penalty_custom_with_bounds_failing_max_bound(value):
def custom_with_bounds(ocp, nlp, t, x, u, p):
my_values = DM.zeros((12, 1)) + x[0]
return -10, my_values, 10
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = ConstraintFcn.CUSTOM
penalty = Constraint(penalty_type)
penalty.max_bound = 0
penalty.custom_function = custom_with_bounds
with pytest.raises(RuntimeError):
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
def test_penalty_track_state(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.TRACK_STATE
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type,
key="qdot",
target=np.ones((4, 1)) * value)
else:
penalty = Constraint(penalty_type,
key="qdot",
target=np.ones((4, 1)) * value)
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, [[value]] * 4)
def test_penalty_track_super_impose_marker(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.SUPERIMPOSE_MARKERS
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, first_marker=0, second_marker=1)
else:
penalty = Constraint(penalty_type, first_marker=0, second_marker=1)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = [[0.8951707, 0, -1.0948376]
] if value == 0.1 else [[-1.3830926, 0, 0.2950504]]
np.testing.assert_almost_equal(res.T, expected)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, time_min, time_max, ode_solver=OdeSolver.RK):
# --- 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = Constraint(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min, max_bound=time_max)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
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, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((3, 7, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((3, 7, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [3], x, [],
[]))
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][6]["val"]
else:
res = ocp.nlp[0].g[0][6]["val"]
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, ConstraintFcn):
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]] * 3),
)
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]] * 3),
)