add error for using solve_ivp for discrete time + formatting tweaks

murrayrm · murrayrm · commit 2860d897ff49 · 2022-11-19T08:04:49.000-08:00
diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py
@@ -89,7 +89,8 @@ def time_optimal_lq_basis(basis_name, basis_size, npoints):
     basis = get_basis(basis_name, basis_size, Tf)
 
     res = opt.solve_ocp(
-        sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, 
+        sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost,
+        basis=basis,
     )
     # Only count this as a benchmark if we converged
     assert res.success
@@ -130,7 +131,7 @@ def time_optimal_lq_methods(integrator_name, minimizer_name):
     basis = get_basis('poly', 12, Tf)
 
     res = opt.solve_ocp(
-        sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, 
+        sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost,
         solve_ivp_method=integrator[0], solve_ivp_kwargs=integrator[1],
         minimize_method=minimizer[0], minimize_options=minimizer[1],
     )
@@ -179,21 +180,21 @@ def time_optimal_lq_size(nstates, ninputs, npoints):
     timepts = np.linspace(0, Tf, npoints)
 
     res = opt.solve_ocp(
-        sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost, 
+        sys, timepts, x0, traj_cost, constraints, terminal_cost=term_cost,
     )
     # Only count this as a benchmark if we converged
     assert res.success
 
 # Parameterize the test against different choices of integrator and minimizer
 time_optimal_lq_size.param_names = ['nstates', 'ninputs', 'npoints']
 time_optimal_lq_size.params = ([1, 2, 4], [1, 2, 4], [5, 10, 20])
-    
+
 
 #
 # Aircraft MPC example (from multi-parametric toolbox)
 #
 
-def time_aircraft_mpc():
+def time_discrete_aircraft_mpc(minimizer_name):
     # model of an aircraft discretized with 0.2s sampling time
     # Source: https://www.mpt3.org/UI/RegulationProblem
     A = [[0.99, 0.01, 0.18, -0.09,   0],
@@ -228,9 +229,18 @@ def time_aircraft_mpc():
     R = np.diag([3, 2])
     cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)
 
+    # Set the time horizon and time points
+    Tf = 3
+    timepts = np.arange(0, 6) * 0.2
+
+    # Get the minimizer parameters to use
+    minimizer = minimizer_table[minimizer_name]
+
     # online MPC controller object is constructed with a horizon 6
     ctrl = opt.create_mpc_iosystem(
-        model, np.arange(0, 6) * 0.2, cost, constraints)
+        model, timepts, cost, constraints,
+        minimize_method=minimizer[0], minimize_options=minimizer[1],
+    )
 
     # Define an I/O system implementing model predictive control
     loop = ct.feedback(sys, ctrl, 1)
@@ -245,3 +255,8 @@ def time_aircraft_mpc():
     # Make sure the system converged to the desired state
     np.testing.assert_allclose(
         xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01)
+
+# Parameterize the test against different choices of minimizer and basis
+time_discrete_aircraft_mpc.param_names = ['minimizer']
+time_discrete_aircraft_mpc.params = (
+    ['trust', 'trust_bigstep', 'SLSQP', 'SLSQP_bigstep', 'COBYLA'])
diff --git a/control/optimal.py b/control/optimal.py
@@ -154,6 +154,14 @@ def __init__(
         self.minimize_kwargs.update(kwargs.pop(
             'minimize_kwargs', config.defaults['optimal.minimize_kwargs']))
 
+        # Check to make sure arguments for discrete-time systems are OK
+        if sys.isdtime(strict=True):
+            if self.solve_ivp_kwargs['method'] is not None or \
+                 len(self.solve_ivp_kwargs) > 1:
+                raise TypeError(
+                    "solve_ivp method, kwargs not allowed for"
+                    " discrete time systems")
+
         # Make sure there were no extraneous keywords
         if kwargs:
             raise TypeError("unrecognized keyword(s): ", str(kwargs))
diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py
@@ -471,6 +471,15 @@ def test_ocp_argument_errors():
         res = opt.solve_ocp(
             sys, time, x0, cost, terminal_constraints=constraints)
 
+    # Discrete time system checks: solve_ivp keywords not allowed
+    sys = ct.rss(2, 1, 1, dt=True)
+    with pytest.raises(TypeError, match="solve_ivp method, kwargs not allowed"):
+        res = opt.solve_ocp(
+            sys, time, x0, cost, solve_ivp_method='LSODA')
+    with pytest.raises(TypeError, match="solve_ivp method, kwargs not allowed"):
+        res = opt.solve_ocp(
+            sys, time, x0, cost, solve_ivp_kwargs={'eps': 0.1})
+
 
 @pytest.mark.parametrize("basis", [
     flat.PolyFamily(4), flat.PolyFamily(6),
