add unit tests for additional coverage

murrayrm · murrayrm · commit d735f794701a · 2021-02-28T22:12:07.000-08:00
diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py
@@ -15,6 +15,7 @@
 from control.tests.conftest import slycotonly
 from numpy.lib import NumpyVersion
 
+
 def test_finite_horizon_simple():
     # Define a linear system with constraints
     # Source: https://www.mpt3.org/UI/RegulationProblem
@@ -108,6 +109,7 @@ def test_discrete_lqr():
     # Make sure we got a different solution
     assert np.any(np.abs(res1.inputs - res2.inputs) > 0.1)
 
+
 def test_mpc_iosystem():
     # model of an aircraft discretized with 0.2s sampling time
     # Source: https://www.mpt3.org/UI/RegulationProblem
@@ -212,6 +214,7 @@ def test_constraint_specification(constraint_list):
     np.testing.assert_almost_equal(
         u_openloop, [-1, -1, 0.1393, 0.3361, -5.204e-16], decimal=3)
 
+
 @pytest.mark.parametrize("sys_args", [
     pytest.param(
         ([[1, 0], [0, 1]], np.eye(2), np.eye(2), 0, True),
@@ -319,6 +322,7 @@ def test_terminal_constraints(sys_args):
         res = optctrl.compute_trajectory(x0, squeeze=True, return_x=True)
         assert not res.success
 
+
 def test_optimal_logging(capsys):
     """Test logging functions (mainly for code coverage)"""
     sys = ct.ss2io(ct.ss(np.eye(2), np.eye(2), np.eye(2), 0, 1))
@@ -435,14 +439,31 @@ def test_optimal_basis_simple():
     cost = opt.quadratic_cost(sys, Q, R)
 
     # Set up the optimal control problem
-    time = np.arange(0, 5, 1)
+    Tf = 5
+    time = np.arange(0, Tf, 1)
     x0 = [4, 0]
 
     # Basic optimal control problem
-    res = opt.solve_ocp(sys, time, x0, cost, constraints, return_x=True)
-    assert res.success
+    res1 = opt.solve_ocp(
+        sys, time, x0, cost, constraints,
+        basis=flat.BezierFamily(4, Tf), return_x=True)
+    assert res1.success
 
     # Make sure the constraints were satisfied
-    np.testing.assert_array_less(np.abs(res.states[0]), 5 + 1e-6)
-    np.testing.assert_array_less(np.abs(res.states[1]), 5 + 1e-6)
-    np.testing.assert_array_less(np.abs(res.inputs[0]), 1 + 1e-6)
+    np.testing.assert_array_less(np.abs(res1.states[0]), 5 + 1e-6)
+    np.testing.assert_array_less(np.abs(res1.states[1]), 5 + 1e-6)
+    np.testing.assert_array_less(np.abs(res1.inputs[0]), 1 + 1e-6)
+
+    # Pass an initial guess and rerun
+    res2 = opt.solve_ocp(
+        sys, time, x0, cost, constraints, initial_guess=0.99*res1.inputs,
+        basis=flat.BezierFamily(4, Tf), return_x=True)
+    assert res2.success
+    np.testing.assert_almost_equal(res2.inputs, res1.inputs, decimal=3)
+
+    # Run with logging turned on for code coverage
+    res3 = opt.solve_ocp(
+        sys, time, x0, cost, constraints,
+        basis=flat.BezierFamily(4, Tf), return_x=True, log=True)
+    assert res3.success
+    np.testing.assert_almost_equal(res3.inputs, res1.inputs, decimal=3)
diff --git a/examples/steering-optimal.py b/examples/steering-optimal.py
@@ -107,8 +107,8 @@ def plot_results(t, y, u, figure=None, yf=None):
 print("Approach 1: standard quadratic cost")
 
 # Set up the cost functions
-Q = np.diag([.1, 10, .1])     # keep lateral error low
-R = np.diag([.1, 1])          # minimize applied inputs
+Q = np.diag([.1, 10, .1])       # keep lateral error low
+R = np.diag([.1, 1])            # minimize applied inputs
 quad_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
 
 # Define the time horizon (and spacing) for the optimization
@@ -209,9 +209,9 @@ def plot_results(t, y, u, figure=None, yf=None):
 start_time = time.process_time()
 result3 = opt.solve_ocp(
     vehicle, horizon, x0, cost3, constraints,
-    terminal_constraints=terminal, initial_guess=u2, log=True,
-    # solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},
-    solve_ivp_kwargs={'method': 'RK23', 'atol': 1e-4, 'rtol': 1e-2},
+    terminal_constraints=terminal, initial_guess=u2, log=False,
+    solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
+    # solve_ivp_kwargs={'method': 'RK23', 'atol': 1e-4, 'rtol': 1e-2},
     minimize_options={'eps': 0.01})
 print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
 
@@ -246,6 +246,7 @@ def plot_results(t, y, u, figure=None, yf=None):
     solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},
     minimize_method='trust-constr', minimize_options={'disp': True},
     # method='SLSQP', options={'eps': 0.01}
+    log=True
 )
 print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
 
