()
| 19 | |
| 20 | |
| 21 | def test_cost_stack(): |
| 22 | nx = 2 |
| 23 | nu = 2 |
| 24 | space = manifolds.VectorSpace(nx) |
| 25 | cost_stack = CostStack(space, nu) |
| 26 | Qr = np.random.randn(4, nx) |
| 27 | Q = Qr.T @ Qr / nx |
| 28 | R = np.eye(nu) |
| 29 | rcost = QuadraticCost(Q, R) |
| 30 | assert isinstance(rcost.space, manifolds.VectorSpace) |
| 31 | |
| 32 | cost_stack.addCost(rcost, 1.0) |
| 33 | data1 = rcost.createData() |
| 34 | data2 = cost_stack.createData() |
| 35 | |
| 36 | for _ in range(10): |
| 37 | x0 = np.random.randn(nx) |
| 38 | u0 = np.random.randn(nu) |
| 39 | |
| 40 | rcost.evaluate(x0, u0, data1) |
| 41 | cost_stack.evaluate(x0, u0, data2) |
| 42 | rcost.computeGradients(x0, u0, data1) |
| 43 | cost_stack.computeGradients(x0, u0, data2) |
| 44 | |
| 45 | rcost.computeHessians(x0, u0, data1) |
| 46 | cost_stack.computeHessians(x0, u0, data2) |
| 47 | |
| 48 | assert data1.value == data2.value |
| 49 | assert_allclose(data1.grad, data2.grad, atol=ATOL) |
| 50 | assert_allclose(data1.hess, data2.hess, atol=ATOL) |
| 51 | |
| 52 | assert_allclose(data1.Lxx, Q, atol=ATOL) |
| 53 | assert_allclose(data1.Luu, R, atol=ATOL) |
| 54 | |
| 55 | with pytest.raises(KeyError, match="Key unk not found."): |
| 56 | cost_stack.getComponent("unk") |
| 57 | |
| 58 | rcost_ref = cost_stack.getComponent(0) |
| 59 | assert isinstance(rcost_ref, QuadraticCost) |
| 60 | |
| 61 | rcost_ref2 = cost_stack.getComponent(0) |
| 62 | assert isinstance(rcost_ref2, QuadraticCost) |
| 63 | |
| 64 | rcost_ref.interp_u[:] = 0.1 |
| 65 | assert_allclose(rcost_ref2.interp_u, 0.1) # works |
| 66 | |
| 67 | # second getter API |
| 68 | rcost_ref3, w = cost_stack.components[0] |
| 69 | assert isinstance(rcost_ref3, QuadraticCost) |
| 70 | assert w == 1.0 |
| 71 | |
| 72 | cost_stack.setWeight(0, 2.0) |
| 73 | assert cost_stack.components[0][1] == 2.0 |
| 74 | |
| 75 | # check that rcost_ref3, using map getter API, was indeed a reference |
| 76 | with pytest.raises(AssertionError): |
| 77 | rcost_ref3.interp_x[:] = 0.42 |
| 78 | assert_allclose(rcost_ref.interp_x, 0.42) # fails |
nothing calls this directly
no test coverage detected