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]], )