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 = Objective(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, 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"]
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, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0.0]]).T)
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0.0]]).T)
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_blockdiag(self):
# Test blockdiag with DM
correct_res = DM([[1, 1, 0, 0, 0], [1, 1, 0, 0, 0], [0, 0, 1, 1, 1],
[0, 0, 1, 1, 1], [0, 0, 1, 1, 1]])
a = DM.ones(2, 2)
b = DM.ones(3, 3)
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res))
# MX and DM mix
a = MX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = MX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and DM mix
a = SX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = SX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and MX
a = SX.sym('a', 2, 2)
b = MX.sym('b', 2, 2)
self.assertRaises(ValueError, blockdiag, a, b)
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)
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_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_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_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 = ConstraintOption(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]
np.testing.assert_almost_equal(res, np.array([[expected[0]]]))
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[expected[1]]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[expected[2]]]))
def test_penalty_minimize_qddot(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0, 1]
x = [DM.ones((8, 1)) * value, DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
if penalty_origin == ObjectiveFcn.Mayer or penalty_origin == ConstraintFcn:
with pytest.raises(AttributeError, match="MINIMIZE_QDDOT"):
_ = penalty_origin.MINIMIZE_QDDOT
return
else:
penalty_type = penalty_origin.MINIMIZE_QDDOT
penalty = Objective(penalty_type)
res = get_penalty_value(ocp, penalty, t, x, u, []).T
np.testing.assert_almost_equal(res, [[value, -9.81 + value, value, value]])
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_track_muscles_control(penalty_origin, value):
ocp = prepare_test_ocp(with_muscles=True)
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_MUSCLES_CONTROL
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], [5], [], 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]]),
)
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_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_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_minimize_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_MARKERS_VELOCITY
penalty = Objective(penalty_type)
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_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_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_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_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_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_minimize_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MARKERS_VELOCITY
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(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_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_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_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_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_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_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_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_minimize_state(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty = Objective(penalty_origin.MINIMIZE_STATE, key="qdot")
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, np.array([[value]] * 4))
def test_penalty_minimize_contact_forces(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.MINIMIZE_CONTACT_FORCES
penalty = Objective(penalty_type)
res = get_penalty_value(ocp, penalty, t, x, u, [])
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)
def test_penalty_minimize_qddot(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0, 1]
x = [DM.ones((8, 1)) * value, DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
if penalty_origin == ObjectiveFcn.Mayer or penalty_origin == ConstraintFcn:
with pytest.raises(AttributeError, match="MINIMIZE_QDDOT"):
_ = penalty_origin.MINIMIZE_QDDOT
return
else:
penalty_type = penalty_origin.MINIMIZE_QDDOT
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], t, x, u, []))
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"].T,
[[value, -9.81 + value, value, value]],
)
