From e14d7b37ae9b3958d7adbc970f0f57fbda593d84 Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Sat, 27 Aug 2022 16:15:15 -0700
Subject: [PATCH 1/5] move optimal_bench.py to steering_bench.py + small fixes

---
 benchmarks/README                                  | 2 +-
 benchmarks/{optimal_bench.py => steering_bench.py} | 0
 control/flatsys/bezier.py                          | 2 +-
 control/flatsys/poly.py                            | 2 +-
 4 files changed, 3 insertions(+), 3 deletions(-)
 rename benchmarks/{optimal_bench.py => steering_bench.py} (100%)

diff --git a/benchmarks/README b/benchmarks/README
index a10bbfc21..9c788b250 100644
--- a/benchmarks/README
+++ b/benchmarks/README
@@ -11,7 +11,7 @@ you can use the following command from the root directory of the repository:
 
   PYTHONPATH=`pwd` asv run --python=python
 
-You can also run benchmarks against specific commits usuing
+You can also run benchmarks against specific commits using
 
   asv run <range>
 
diff --git a/benchmarks/optimal_bench.py b/benchmarks/steering_bench.py
similarity index 100%
rename from benchmarks/optimal_bench.py
rename to benchmarks/steering_bench.py
diff --git a/control/flatsys/bezier.py b/control/flatsys/bezier.py
index 02b4209a6..fcf6201e9 100644
--- a/control/flatsys/bezier.py
+++ b/control/flatsys/bezier.py
@@ -73,7 +73,7 @@ def eval_deriv(self, i, k, t, var=None):
             raise ValueError("Basis function index too high")
         elif k >= self.N:
             # Higher order derivatives are zero
-            return np.zeros(t.shape)
+            return 0 * t
 
         # Compute the variables used in Bezier curve formulas
         n = self.N - 1
diff --git a/control/flatsys/poly.py b/control/flatsys/poly.py
index bfd8de633..f315091aa 100644
--- a/control/flatsys/poly.py
+++ b/control/flatsys/poly.py
@@ -66,6 +66,6 @@ def __init__(self, N, T=1):
     # Compute the kth derivative of the ith basis function at time t
     def eval_deriv(self, i, k, t, var=None):
         """Evaluate the kth derivative of the ith basis function at time t."""
-        if (i < k): return 0;           # higher derivative than power
+        if (i < k): return 0 * t        # higher derivative than power
         return factorial(i)/factorial(i-k) * \
             np.power(t/self.T, i-k) / np.power(self.T, k)

From 08f6464d616bda5cd2e33ff85ec80c06779d60e9 Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Sat, 27 Aug 2022 16:16:17 -0700
Subject: [PATCH 2/5] improved flat system benchmarking (+ docstring, unit test
 updates)

---
 benchmarks/flatsys_bench.py   | 84 +++++++++++++++++++++++++++++------
 control/flatsys/flatsys.py    |  5 +++
 control/tests/flatsys_test.py | 20 +++++++++
 3 files changed, 96 insertions(+), 13 deletions(-)

diff --git a/benchmarks/flatsys_bench.py b/benchmarks/flatsys_bench.py
index 0c0a5e53a..05a2e7066 100644
--- a/benchmarks/flatsys_bench.py
+++ b/benchmarks/flatsys_bench.py
@@ -11,6 +11,10 @@
 import control.flatsys as flat
 import control.optimal as opt
 
+#
+# System setup: vehicle steering (bicycle model)
+#
+
 # Vehicle steering dynamics
 def vehicle_update(t, x, u, params):
     # Get the parameters for the model
@@ -67,11 +71,28 @@ def vehicle_reverse(zflag, params={}):
 # Define the time points where the cost/constraints will be evaluated
 timepts = np.linspace(0, Tf, 10, endpoint=True)
 
-def time_steering_point_to_point(basis_name, basis_size):
-    if basis_name == 'poly':
-        basis = flat.PolyFamily(basis_size)
-    elif basis_name == 'bezier':
-        basis = flat.BezierFamily(basis_size)
+#
+# Benchmark test parameters
+#
+
+basis_params = (['poly', 'bezier', 'bspline'], [8, 10, 12])
+basis_param_names = ["basis", "size"]
+
+def get_basis(name, size):
+    if name == 'poly':
+        basis = flat.PolyFamily(size, T=Tf)
+    elif name == 'bezier':
+        basis = flat.BezierFamily(size, T=Tf)
+    elif name == 'bspline':
+        basis = flat.BSplineFamily([0, Tf/2, Tf], size)
+    return basis
+
+#
+# Benchmarks
+#
+
+def time_point_to_point(basis_name, basis_size):
+    basis = get_basis(basis_name, basis_size)
 
     # Find trajectory between initial and final conditions
     traj = flat.point_to_point(vehicle, Tf, x0, u0, xf, uf, basis=basis)
@@ -80,13 +101,16 @@ def time_steering_point_to_point(basis_name, basis_size):
     x, u = traj.eval([0, Tf])
     np.testing.assert_array_almost_equal(x0, x[:, 0])
     np.testing.assert_array_almost_equal(u0, u[:, 0])
-    np.testing.assert_array_almost_equal(xf, x[:, 1])
-    np.testing.assert_array_almost_equal(uf, u[:, 1])
+    np.testing.assert_array_almost_equal(xf, x[:, -1])
+    np.testing.assert_array_almost_equal(uf, u[:, -1])
+
+time_point_to_point.params = basis_params
+time_point_to_point.param_names = basis_param_names
 
-time_steering_point_to_point.params = (['poly', 'bezier'], [6, 8])
-time_steering_point_to_point.param_names = ["basis", "size"]        
 
-def time_steering_cost():
+def time_point_to_point_with_cost(basis_name, basis_size):
+    basis = get_basis(basis_name, basis_size)
+
     # Define cost and constraints
     traj_cost = opt.quadratic_cost(
         vehicle, None, np.diag([0.1, 1]), u0=uf)
@@ -95,13 +119,47 @@ def time_steering_cost():
 
     traj = flat.point_to_point(
         vehicle, timepts, x0, u0, xf, uf,
-        cost=traj_cost, constraints=constraints, basis=flat.PolyFamily(8)
+        cost=traj_cost, constraints=constraints, basis=basis,
     )
 
     # Verify that the trajectory computation is correct
     x, u = traj.eval([0, Tf])
     np.testing.assert_array_almost_equal(x0, x[:, 0])
     np.testing.assert_array_almost_equal(u0, u[:, 0])
-    np.testing.assert_array_almost_equal(xf, x[:, 1])
-    np.testing.assert_array_almost_equal(uf, u[:, 1])
+    np.testing.assert_array_almost_equal(xf, x[:, -1])
+    np.testing.assert_array_almost_equal(uf, u[:, -1])
+
+time_point_to_point_with_cost.params = basis_params
+time_point_to_point_with_cost.param_names = basis_param_names
+
+
+def time_solve_flat_ocp_terminal_cost(method, basis_name, basis_size):
+    basis = get_basis(basis_name, basis_size)
+
+    # Define cost and constraints
+    traj_cost = opt.quadratic_cost(
+        vehicle, None, np.diag([0.1, 1]), u0=uf)
+    term_cost = opt.quadratic_cost(
+        vehicle, np.diag([1e3, 1e3, 1e3]), None, x0=xf)
+    constraints = [
+        opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
+
+    # Initial guess = straight line
+    initial_guess = np.array(
+        [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)])
+
+    traj = flat.solve_flat_ocp(
+        vehicle, timepts, x0, u0, basis=basis, initial_guess=initial_guess,
+        trajectory_cost=traj_cost, constraints=constraints,
+        terminal_cost=term_cost, minimize_method=method,
+    )
+
+    # Verify that the trajectory computation is correct
+    x, u = traj.eval([0, Tf])
+    np.testing.assert_array_almost_equal(x0, x[:, 0])
+    np.testing.assert_array_almost_equal(xf, x[:, -1], decimal=2)
 
+time_solve_flat_ocp_terminal_cost.params = tuple(
+    [['slsqp', 'trust-constr']] + list(basis_params))
+time_solve_flat_ocp_terminal_cost.param_names = tuple(
+    ['method'] + basis_param_names)
diff --git a/control/flatsys/flatsys.py b/control/flatsys/flatsys.py
index 849c41c72..04e5c323a 100644
--- a/control/flatsys/flatsys.py
+++ b/control/flatsys/flatsys.py
@@ -654,6 +654,11 @@ def solve_flat_ocp(
            * cost : computed cost of the returned trajectory
            * message : message returned by optimization if success if False
 
+    3. A common failure in solving optimal control problem is that the
+       default initial guess violates the constraints and the optimizer
+       can't find a feasible solution.  Using the `initial_guess` parameter
+       can often be used to overcome these errors.
+
     """
     #
     # Make sure the problem is one that we can handle
diff --git a/control/tests/flatsys_test.py b/control/tests/flatsys_test.py
index 665bfd968..3f308c79b 100644
--- a/control/tests/flatsys_test.py
+++ b/control/tests/flatsys_test.py
@@ -464,6 +464,26 @@ def test_bezier_basis(self):
         with pytest.raises(ValueError, match="index too high"):
             bezier.eval_deriv(4, 0, time)
 
+    @pytest.mark.parametrize("basis, degree, T", [
+        (fs.PolyFamily(4), 4, 1),
+        (fs.PolyFamily(4, 100), 4, 100),
+        (fs.BezierFamily(4), 4, 1),
+        (fs.BezierFamily(4, 100), 4, 100),
+        (fs.BSplineFamily([0, 0.5, 1], 4), 3, 1),
+        (fs.BSplineFamily([0, 50, 100], 4), 3, 100),
+    ])
+    def test_basis_derivs(self, basis, degree, T):
+        """Make sure that that basis function derivates are correct"""
+        timepts = np.linspace(0, T, 10000)
+        dt = timepts[1] - timepts[0]
+        for i in range(basis.N):
+            for j in range(degree-1):
+                # Compare numerical and analytical derivative
+                np.testing.assert_allclose(
+                    np.diff(basis.eval_deriv(i, j, timepts)) / dt,
+                    basis.eval_deriv(i, j+1, timepts)[0:-1],
+                    atol=1e-2, rtol=1e-4)
+
     def test_point_to_point_errors(self):
         """Test error and warning conditions in point_to_point()"""
         # Double integrator system

From efca9caffad823bf785d8b51d1d5ad300247674c Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Sat, 27 Aug 2022 22:48:36 -0700
Subject: [PATCH 3/5] new optimal benchmarks (replace steering with rss)

---
 benchmarks/optimal_bench.py  | 247 +++++++++++++++++++++++++++++++++++
 benchmarks/steering_bench.py | 220 -------------------------------
 2 files changed, 247 insertions(+), 220 deletions(-)
 create mode 100644 benchmarks/optimal_bench.py
 delete mode 100644 benchmarks/steering_bench.py

diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py
new file mode 100644
index 000000000..bcc598527
--- /dev/null
+++ b/benchmarks/optimal_bench.py
@@ -0,0 +1,247 @@
+# optimal_bench.py - benchmarks for optimal control package
+# RMM, 27 Feb 2021
+#
+# This benchmark tests the timing for the optimal control module
+# (control.optimal) and is intended to be used for helping tune the
+# performance of the functions used for optimization-base control.
+
+import numpy as np
+import math
+import control as ct
+import control.flatsys as fs
+import control.optimal as opt
+
+#
+# Benchmark test parameters
+#
+
+# Define integrator and minimizer methods and options/keywords
+integrator_table = {
+    'default': (None, {}),
+    'RK23': ('RK23', {}),
+    'RK23_sloppy': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}),
+    'RK45': ('RK45', {}),
+    'RK45': ('RK45', {}),
+    'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}),
+    'LSODA': ('LSODA', {}),
+}
+
+minimizer_table = {
+    'default': (None, {}),
+    'trust': ('trust-constr', {}),
+    'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}),
+    'SLSQP': ('SLSQP', {}),
+    'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}),
+    'COBYLA': ('COBYLA', {}),
+}
+
+# Utility function to create a basis of a given size
+def get_basis(name, size, Tf):
+    if name == 'poly':
+        basis = fs.PolyFamily(size, T=Tf)
+    elif name == 'bezier':
+        basis = fs.BezierFamily(size, T=Tf)
+    elif name == 'bspline':
+        basis = fs.BSplineFamily([0, Tf/2, Tf], size)
+    return basis
+
+
+#
+# Optimal trajectory generation with linear quadratic cost
+#
+
+def time_optimal_lq_basis(basis_name, basis_size, npoints):
+    # Create a sufficiently controllable random system to control
+    ntrys = 20
+    while ntrys > 0:
+        # Create a random system
+        sys = ct.rss(2, 2, 2)
+
+        # Compute the controllability Gramian
+        Wc = ct.gram(sys, 'c')
+
+        # Make sure the condition number is reasonable
+        if np.linalg.cond(Wc) < 1e6:
+            break
+
+        ntrys -= 1
+    assert ntrys > 0            # Something wrong if we needed > 20 tries
+
+    # Define cost functions
+    Q = np.eye(sys.nstates)
+    R = np.eye(sys.ninputs) * 10
+
+    # Figure out the LQR solution (for terminal cost)
+    K, S, E = ct.lqr(sys, Q, R)
+
+    # Define the cost functions
+    traj_cost = opt.quadratic_cost(sys, Q, R)
+    term_cost = opt.quadratic_cost(sys, S, None)
+    constraints = opt.input_range_constraint(
+        sys, -np.ones(sys.ninputs), np.ones(sys.ninputs))
+
+    # Define the initial condition, time horizon, and time points
+    x0 = np.ones(sys.nstates)
+    Tf = 10
+    timepts = np.linspace(0, Tf, npoints)
+
+    # Create the basis function to use
+    basis = get_basis(basis_name, basis_size, Tf)
+
+    res = opt.solve_ocp(
+        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_basis.param_names = ['basis', 'size', 'npoints']
+time_optimal_lq_basis.params = (
+    ['poly', 'bezier', 'bspline'], [8, 10, 12], [5, 10, 20])
+
+
+def time_optimal_lq_methods(integrator_name, minimizer_name):
+    # Get the integrator and minimizer parameters to use
+    integrator = integrator_table[integrator_name]
+    minimizer = minimizer_table[minimizer_name]
+
+    # Create a random system to control
+    sys = ct.rss(2, 1, 1)
+
+    # Define cost functions
+    Q = np.eye(sys.nstates)
+    R = np.eye(sys.ninputs) * 10
+
+    # Figure out the LQR solution (for terminal cost)
+    K, S, E = ct.lqr(sys, Q, R)
+
+    # Define the cost functions
+    traj_cost = opt.quadratic_cost(sys, Q, R)
+    term_cost = opt.quadratic_cost(sys, S, None)
+    constraints = opt.input_range_constraint(
+        sys, -np.ones(sys.ninputs), np.ones(sys.ninputs))
+
+    # Define the initial condition, time horizon, and time points
+    x0 = np.ones(sys.nstates)
+    Tf = 10
+    timepts = np.linspace(0, Tf, 20)
+
+    # Create the basis function to use
+    basis = get_basis('poly', 12, Tf)
+
+    res = opt.solve_ocp(
+        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],
+    )
+    # 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_methods.param_names = ['integrator', 'minimizer']
+time_optimal_lq_methods.params = (
+    ['RK23', 'RK45', 'LSODA'], ['trust', 'SLSQP', 'COBYLA'])
+
+
+def time_optimal_lq_size(nstates, ninputs, npoints):
+    # Create a sufficiently controllable random system to control
+    ntrys = 20
+    while ntrys > 0:
+        # Create a random system
+        sys = ct.rss(nstates, ninputs, ninputs)
+
+        # Compute the controllability Gramian
+        Wc = ct.gram(sys, 'c')
+
+        # Make sure the condition number is reasonable
+        if np.linalg.cond(Wc) < 1e6:
+            break
+
+        ntrys -= 1
+    assert ntrys > 0            # Something wrong if we needed > 20 tries
+
+    # Define cost functions
+    Q = np.eye(sys.nstates)
+    R = np.eye(sys.ninputs) * 10
+
+    # Figure out the LQR solution (for terminal cost)
+    K, S, E = ct.lqr(sys, Q, R)
+
+    # Define the cost functions
+    traj_cost = opt.quadratic_cost(sys, Q, R)
+    term_cost = opt.quadratic_cost(sys, S, None)
+    constraints = opt.input_range_constraint(
+        sys, -np.ones(sys.ninputs), np.ones(sys.ninputs))
+
+    # Define the initial condition, time horizon, and time points
+    x0 = np.ones(sys.nstates)
+    Tf = 10
+    timepts = np.linspace(0, Tf, npoints)
+
+    res = opt.solve_ocp(
+        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():
+    # 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],
+         [   0, 0.94,    0,  0.29,   0],
+         [   0, 0.14, 0.81,  -0.9,   0],
+         [   0, -0.2,    0,  0.95,   0],
+         [   0, 0.09,    0,     0, 0.9]]
+    B = [[ 0.01, -0.02],
+         [-0.14,     0],
+         [ 0.05,  -0.2],
+         [ 0.02,     0],
+         [-0.01, 0]]
+    C = [[0, 1, 0, 0, -1],
+         [0, 0, 1, 0,  0],
+         [0, 0, 0, 1,  0],
+         [1, 0, 0, 0,  0]]
+    model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))
+
+    # For the simulation we need the full state output
+    sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))
+
+    # compute the steady state values for a particular value of the input
+    ud = np.array([0.8, -0.3])
+    xd = np.linalg.inv(np.eye(5) - A) @ B @ ud
+    yd = C @ xd
+
+    # provide constraints on the system signals
+    constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])]
+
+    # provide penalties on the system signals
+    Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C
+    R = np.diag([3, 2])
+    cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)
+
+    # online MPC controller object is constructed with a horizon 6
+    ctrl = opt.create_mpc_iosystem(
+        model, np.arange(0, 6) * 0.2, cost, constraints)
+
+    # Define an I/O system implementing model predictive control
+    loop = ct.feedback(sys, ctrl, 1)
+
+    # Choose a nearby initial condition to speed up computation
+    X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99
+
+    Nsim = 12
+    tout, xout = ct.input_output_response(
+        loop, np.arange(0, Nsim) * 0.2, 0, X0)
+
+    # 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)
diff --git a/benchmarks/steering_bench.py b/benchmarks/steering_bench.py
deleted file mode 100644
index 21cabef7e..000000000
--- a/benchmarks/steering_bench.py
+++ /dev/null
@@ -1,220 +0,0 @@
-# optimal_bench.py - benchmarks for optimal control package
-# RMM, 27 Feb 2021
-#
-# This benchmark tests the timing for the optimal control module
-# (control.optimal) and is intended to be used for helping tune the
-# performance of the functions used for optimization-base control.
-
-import numpy as np
-import math
-import control as ct
-import control.flatsys as flat
-import control.optimal as opt
-
-# Vehicle steering dynamics
-def vehicle_update(t, x, u, params):
-    # Get the parameters for the model
-    l = params.get('wheelbase', 3.)         # vehicle wheelbase
-    phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
-
-    # Saturate the steering input (use min/max instead of clip for speed)
-    phi = max(-phimax, min(u[1], phimax))
-
-    # Return the derivative of the state
-    return np.array([
-        math.cos(x[2]) * u[0],            # xdot = cos(theta) v
-        math.sin(x[2]) * u[0],            # ydot = sin(theta) v
-        (u[0] / l) * math.tan(phi)        # thdot = v/l tan(phi)
-    ])
-
-def vehicle_output(t, x, u, params):
-    return x                            # return x, y, theta (full state)
-
-vehicle = ct.NonlinearIOSystem(
-    vehicle_update, vehicle_output, states=3, name='vehicle',
-    inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))
-
-# Initial and final conditions
-x0 = [0., -2., 0.]; u0 = [10., 0.]
-xf = [100., 2., 0.]; uf = [10., 0.]
-Tf = 10
-
-# Define the time horizon (and spacing) for the optimization
-horizon = np.linspace(0, Tf, 10, endpoint=True)
-
-# Provide an intial guess (will be extended to entire horizon)
-bend_left = [10, 0.01]          # slight left veer
-
-def time_steering_integrated_cost():
-    # Set up the cost functions
-    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)
-
-    res = opt.solve_ocp(
-        vehicle, horizon, x0, quad_cost,
-        initial_guess=bend_left, print_summary=False,
-        # solve_ivp_kwargs={'atol': 1e-2, 'rtol': 1e-2},
-        minimize_method='trust-constr',
-        minimize_options={'finite_diff_rel_step': 0.01},
-    )
-
-    # Only count this as a benchmark if we converged
-    assert res.success
-
-def time_steering_terminal_cost():
-    # Define cost and constraints
-    traj_cost = opt.quadratic_cost(
-        vehicle, None, np.diag([0.1, 1]), u0=uf)
-    term_cost = opt.quadratic_cost(
-        vehicle, np.diag([1, 10, 10]), None, x0=xf)
-    constraints = [
-        opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
-
-    res = opt.solve_ocp(
-        vehicle, horizon, x0, traj_cost, constraints,
-        terminal_cost=term_cost, initial_guess=bend_left, print_summary=False,
-        solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
-        # minimize_method='SLSQP', minimize_options={'eps': 0.01}
-        minimize_method='trust-constr',
-        minimize_options={'finite_diff_rel_step': 0.01},
-    )
-    # Only count this as a benchmark if we converged
-    assert res.success
-
-# Define integrator and minimizer methods and options/keywords
-integrator_table = {
-    'RK23_default': ('RK23', {'atol': 1e-4, 'rtol': 1e-2}),
-    'RK23_sloppy': ('RK23', {}),
-    'RK45_default': ('RK45', {}),
-    'RK45_sloppy': ('RK45', {'atol': 1e-4, 'rtol': 1e-2}),
-}
-
-minimizer_table = {
-    'trust_default': ('trust-constr', {}),
-    'trust_bigstep': ('trust-constr', {'finite_diff_rel_step': 0.01}),
-    'SLSQP_default': ('SLSQP', {}),
-    'SLSQP_bigstep': ('SLSQP', {'eps': 0.01}),
-}
-
-
-def time_steering_terminal_constraint(integrator_name, minimizer_name):
-    # Get the integrator and minimizer parameters to use
-    integrator = integrator_table[integrator_name]
-    minimizer = minimizer_table[minimizer_name]
-
-    # Input cost and terminal constraints
-    R = np.diag([1, 1])                 # minimize applied inputs
-    cost = opt.quadratic_cost(vehicle, np.zeros((3,3)), R, u0=uf)
-    constraints = [
-        opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
-    terminal = [ opt.state_range_constraint(vehicle, xf, xf) ]
-
-    res = opt.solve_ocp(
-        vehicle, horizon, x0, cost, constraints,
-        terminal_constraints=terminal, initial_guess=bend_left, log=False,
-        solve_ivp_method=integrator[0], solve_ivp_kwargs=integrator[1],
-        minimize_method=minimizer[0], minimize_options=minimizer[1],
-    )
-    # Only count this as a benchmark if we converged
-    assert res.success
-
-# Reset the timeout value to allow for longer runs
-time_steering_terminal_constraint.timeout = 120
-
-# Parameterize the test against different choices of integrator and minimizer
-time_steering_terminal_constraint.param_names = ['integrator', 'minimizer']
-time_steering_terminal_constraint.params = (
-    ['RK23_default', 'RK23_sloppy', 'RK45_default', 'RK45_sloppy'],
-    ['trust_default', 'trust_bigstep', 'SLSQP_default', 'SLSQP_bigstep']
-)
-
-def time_steering_bezier_basis(nbasis, ntimes):
-    # Set up costs and constriants
-    Q = np.diag([.1, 10, .1])           # keep lateral error low
-    R = np.diag([1, 1])                 # minimize applied inputs
-    cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
-    constraints = [ opt.input_range_constraint(vehicle, [0, -0.1], [20, 0.1]) ]
-    terminal = [ opt.state_range_constraint(vehicle, xf, xf) ]
-
-    # Set up horizon
-    horizon = np.linspace(0, Tf, ntimes, endpoint=True)
-
-    # Set up the optimal control problem
-    res = opt.solve_ocp(
-        vehicle, horizon, x0, cost,
-        constraints,
-        terminal_constraints=terminal,
-        initial_guess=bend_left,
-        basis=flat.BezierFamily(nbasis, T=Tf),
-        # solve_ivp_kwargs={'atol': 1e-4, 'rtol': 1e-2},
-        minimize_method='trust-constr',
-        minimize_options={'finite_diff_rel_step': 0.01},
-        # minimize_method='SLSQP', minimize_options={'eps': 0.01},
-        return_states=True, print_summary=False
-    )
-    t, u, x = res.time, res.inputs, res.states
-
-    # Make sure we found a valid solution
-    assert res.success
-
-# Reset the timeout value to allow for longer runs
-time_steering_bezier_basis.timeout = 120
-
-# Set the parameter values for the number of times and basis vectors
-time_steering_bezier_basis.param_names = ['nbasis', 'ntimes']
-time_steering_bezier_basis.params = ([2, 4, 6], [5, 10, 20])
-
-def time_aircraft_mpc():
-    # 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],
-         [   0, 0.94,    0,  0.29,   0],
-         [   0, 0.14, 0.81,  -0.9,   0],
-         [   0, -0.2,    0,  0.95,   0],
-         [   0, 0.09,    0,     0, 0.9]]
-    B = [[ 0.01, -0.02],
-         [-0.14,     0],
-         [ 0.05,  -0.2],
-         [ 0.02,     0],
-         [-0.01, 0]]
-    C = [[0, 1, 0, 0, -1],
-         [0, 0, 1, 0,  0],
-         [0, 0, 0, 1,  0],
-         [1, 0, 0, 0,  0]]
-    model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))
-
-    # For the simulation we need the full state output
-    sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))
-
-    # compute the steady state values for a particular value of the input
-    ud = np.array([0.8, -0.3])
-    xd = np.linalg.inv(np.eye(5) - A) @ B @ ud
-    yd = C @ xd
-
-    # provide constraints on the system signals
-    constraints = [opt.input_range_constraint(sys, [-5, -6], [5, 6])]
-
-    # provide penalties on the system signals
-    Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C
-    R = np.diag([3, 2])
-    cost = opt.quadratic_cost(model, Q, R, x0=xd, u0=ud)
-
-    # online MPC controller object is constructed with a horizon 6
-    ctrl = opt.create_mpc_iosystem(
-        model, np.arange(0, 6) * 0.2, cost, constraints)
-
-    # Define an I/O system implementing model predictive control
-    loop = ct.feedback(sys, ctrl, 1)
-
-    # Choose a nearby initial condition to speed up computation
-    X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99
-
-    Nsim = 12
-    tout, xout = ct.input_output_response(
-        loop, np.arange(0, Nsim) * 0.2, 0, X0)
-
-    # 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)

From 2860d897ff49c8bd1c95ee435933f230d80d5b76 Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Mon, 29 Aug 2022 07:46:25 -0700
Subject: [PATCH 4/5] add error for using solve_ivp for discrete time +
 formatting tweaks

---
 benchmarks/optimal_bench.py   | 27 +++++++++++++++++++++------
 control/optimal.py            |  8 ++++++++
 control/tests/optimal_test.py |  9 +++++++++
 3 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/benchmarks/optimal_bench.py b/benchmarks/optimal_bench.py
index bcc598527..a424c7827 100644
--- 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,7 +180,7 @@ 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
@@ -187,13 +188,13 @@ def time_optimal_lq_size(nstates, ninputs, npoints):
 # 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
index 4913cc341..3da236e75 100644
--- 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
index b100e7e14..0e5e35d79 100644
--- 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),

From 54de2a3c33b8819c9d6146166d24af9ceda82a75 Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Mon, 29 Aug 2022 17:50:20 -0700
Subject: [PATCH 5/5] allow print_summary to be set in solve_ocp

---
 control/optimal.py | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/control/optimal.py b/control/optimal.py
index 3da236e75..9650272f7 100644
--- a/control/optimal.py
+++ b/control/optimal.py
@@ -897,7 +897,8 @@ def __init__(
 def solve_ocp(
         sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None,
         terminal_constraints=[], initial_guess=None, basis=None, squeeze=None,
-        transpose=None, return_states=True, log=False, **kwargs):
+        transpose=None, return_states=True, print_summary=True, log=False,
+        **kwargs):
 
     """Compute the solution to an optimal control problem
 
@@ -951,6 +952,9 @@ def solve_ocp(
     log : bool, optional
         If `True`, turn on logging messages (using Python logging module).
 
+    print_summary : bool, optional
+        If `True` (default), print a short summary of the computation.
+
     return_states : bool, optional
         If True, return the values of the state at each time (default = True).
 
@@ -1017,7 +1021,8 @@ def solve_ocp(
 
     # Solve for the optimal input from the current state
     return ocp.compute_trajectory(
-        X0, squeeze=squeeze, transpose=transpose, return_states=return_states)
+        X0, squeeze=squeeze, transpose=transpose, print_summary=print_summary,
+        return_states=return_states)
 
 
 # Create a model predictive controller for an optimal control problem

<!DOCTYPE html PUBLIC '-//W3C//DTD XHTML 1.0 Transitional//EN' 'http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd'>
<html xmlns='http://www.w3.org/1999/xhtml'>
<head>
<title>pFad - Phonifier reborn</title>
<meta http-equiv='Content-Type' content='text/html; charset=utf-8' />
</head>
<body>
<h1>Pfad - The Proxy pFad of &#169; 2024 Garber Painting. All rights reserved.</h1>


<!-- Disclaimer -->
<p>Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.</p>
<br>
<p>Alternative Proxies:</p><p><a href="http://rainy.clevelandohioweatherforecast.com/php-proxy/index.php?q=https://patch-diff.githubusercontent.com/raw/python-control/python-control/pull/800.patch" target="_blank">Alternative Proxy</a></p><p><a href="http://rainy.clevelandohioweatherforecast.com/pFad/index.php?u=https://patch-diff.githubusercontent.com/raw/python-control/python-control/pull/800.patch" target="_blank">pFad Proxy</a></p><p><a href="http://rainy.clevelandohioweatherforecast.com/pFad/v3index.php?u=https://patch-diff.githubusercontent.com/raw/python-control/python-control/pull/800.patch" target="_blank">pFad v3 Proxy</a></p><p><a href="http://rainy.clevelandohioweatherforecast.com/pFad/v4index.php?u=https://patch-diff.githubusercontent.com/raw/python-control/python-control/pull/800.patch" target="_blank">pFad v4 Proxy</a></p></body>
</html>