From 99ec0229e7f8dc4220e2d278e22cd1257348cb2d Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 12 Jan 2023 21:52:06 -0800 Subject: [PATCH 01/12] add link to documentation for step_info --- doc/control.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/control.rst b/doc/control.rst index 79702dc6a..1ddf7f80a 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -86,6 +86,7 @@ Control system analysis ispassive margin stability_margins + step_info phase_crossover_frequencies poles zeros From c03b77178233ee4aaa0a7747fc58e24788c4c9ef Mon Sep 17 00:00:00 2001 From: Ben Greiner Date: Thu, 2 Feb 2023 13:18:34 +0100 Subject: [PATCH 02/12] rename 'type' keyword --- control/statefbk.py | 38 +++++++++++++++++++++++----------- control/tests/statefbk_test.py | 24 ++++++++++++++++++--- 2 files changed, 47 insertions(+), 15 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 1f61134a6..ab8369991 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -42,6 +42,7 @@ # External packages and modules import numpy as np import scipy as sp +import warnings from . import statesp from .mateqn import care, dare, _check_shape @@ -597,10 +598,10 @@ def dlqr(*args, **kwargs): # Function to create an I/O sytems representing a state feedback controller def create_statefbk_iosystem( - sys, gain, integral_action=None, estimator=None, type=None, + sys, gain, integral_action=None, estimator=None, controller_type=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None, gainsched_method='linear', name=None, inputs=None, outputs=None, - states=None): + states=None, **kwargs): """Create an I/O system using a (full) state feedback controller This function creates an input/output system that implements a @@ -684,7 +685,7 @@ def create_statefbk_iosystem( hull of the scheduling points, the gain at the nearest point is used. - type : 'linear' or 'nonlinear', optional + controller_type : 'linear' or 'nonlinear', optional Set the type of controller to create. The default for a linear gain is a linear controller implementing the LQR regulator. If the type is 'nonlinear', a :class:NonlinearIOSystem is created instead, with @@ -728,6 +729,18 @@ def create_statefbk_iosystem( if not isinstance(sys, InputOutputSystem): raise ControlArgument("Input system must be I/O system") + # Process (legacy) keywords + if kwargs.get('type') is not None: + warnings.warn( + "keyword 'type' is deprecated; use 'controller_type'", + DeprecationWarning) + if controller_type is not None: + raise ControlArgument( + "duplicate keywords 'type` and 'controller_type'") + controller_type = kwargs.pop('type') + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) + # See whether we were give an estimator if estimator is not None: # Check to make sure the estimator is the right size @@ -781,13 +794,14 @@ def create_statefbk_iosystem( raise ControlArgument("gain must be an array or a tuple") # Decide on the type of system to create - if gainsched and type == 'linear': + if gainsched and controller_type == 'linear': raise ControlArgument( - "type 'linear' not allowed for gain scheduled controller") - elif type is None: - type = 'nonlinear' if gainsched else 'linear' - elif type not in {'linear', 'nonlinear'}: - raise ControlArgument(f"unknown type '{type}'") + "controller_type 'linear' not allowed for" + " gain scheduled controller") + elif controller_type is None: + controller_type = 'nonlinear' if gainsched else 'linear' + elif controller_type not in {'linear', 'nonlinear'}: + raise ControlArgument(f"unknown controller_type '{controller_type}'") # Figure out the labels to use if isinstance(xd_labels, str): @@ -845,7 +859,7 @@ def _compute_gain(mu): return K # Define the controller system - if type == 'nonlinear': + if controller_type == 'nonlinear': # Create an I/O system for the state feedback gains def _control_update(t, states, inputs, params): # Split input into desired state, nominal input, and current state @@ -879,7 +893,7 @@ def _control_output(t, states, inputs, params): _control_update, _control_output, name=name, inputs=inputs, outputs=outputs, states=states, params=params) - elif type == 'linear' or type is None: + elif controller_type == 'linear' or controller_type is None: # Create the matrices implementing the controller if isctime(sys): # Continuous time: integrator @@ -898,7 +912,7 @@ def _control_output(t, states, inputs, params): inputs=inputs, outputs=outputs, states=states) else: - raise ControlArgument(f"unknown type '{type}'") + raise ControlArgument(f"unknown controller_type '{controller_type}'") # Define the closed loop system closed = interconnect( diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 9e8feb4c9..acb47a27c 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -6,6 +6,7 @@ import numpy as np import pytest import itertools +import warnings from math import pi, atan import control as ct @@ -569,7 +570,12 @@ def test_statefbk_iosys( # Create an I/O system for the controller ctrl, clsys = ct.create_statefbk_iosystem( - sys, K, integral_action=C_int, estimator=est, type=type) + sys, K, integral_action=C_int, estimator=est, + controller_type=type, name=type) + + # Make sure the name got set correctly + if type is not None: + assert ctrl.name == type # If we used a nonlinear controller, linearize it for testing if type == 'nonlinear': @@ -763,8 +769,20 @@ def test_statefbk_errors(self): with pytest.raises(ControlArgument, match="gain must be an array"): ctrl, clsys = ct.create_statefbk_iosystem(sys, "bad argument") - with pytest.raises(ControlArgument, match="unknown type"): - ctrl, clsys = ct.create_statefbk_iosystem(sys, K, type=1) + with pytest.warns(DeprecationWarning, match="'type' is deprecated"): + ctrl, clsys = ct.create_statefbk_iosystem(sys, K, type='nonlinear') + + with pytest.raises(ControlArgument, match="duplicate keywords"): + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, type='nonlinear', controller_type='nonlinear') + + with pytest.raises(TypeError, match="unrecognized keywords"): + ctrl, clsys = ct.create_statefbk_iosystem(sys, K, typo='nonlinear') + + with pytest.raises(ControlArgument, match="unknown controller_type"): + ctrl, clsys = ct.create_statefbk_iosystem(sys, K, controller_type=1) # Errors involving integral action C_int = np.eye(2, 4) From cd89353ab7c1b36dd0de189d2045c2168011f477 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 14 Jan 2023 10:25:17 -0800 Subject: [PATCH 03/12] add entry for unused kwargs in create_statefbk_iosystem --- control/tests/kwargs_test.py | 1 + 1 file changed, 1 insertion(+) diff --git a/control/tests/kwargs_test.py b/control/tests/kwargs_test.py index 8116f013a..1cb3b596a 100644 --- a/control/tests/kwargs_test.py +++ b/control/tests/kwargs_test.py @@ -158,6 +158,7 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): kwarg_unittest = { 'bode': test_matplotlib_kwargs, 'bode_plot': test_matplotlib_kwargs, + 'create_statefbk_iosystem': statefbk_test.TestStatefbk.test_statefbk_iosys, 'describing_function_plot': test_matplotlib_kwargs, 'dlqe': test_unrecognized_kwargs, 'dlqr': test_unrecognized_kwargs, From c90781dacfebe358aceca02016845cc50bba4e9b Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 20 Jan 2023 22:51:07 -0800 Subject: [PATCH 04/12] require at least 3 points for point_to_point w/ cost, constraints --- control/flatsys/flatsys.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/control/flatsys/flatsys.py b/control/flatsys/flatsys.py index 5741a9bd3..4bd767a99 100644 --- a/control/flatsys/flatsys.py +++ b/control/flatsys/flatsys.py @@ -436,6 +436,12 @@ def point_to_point( warnings.warn("basis too small; solution may not exist") if cost is not None or trajectory_constraints is not None: + # Make sure that we have enough timepoints to evaluate + if timepts.size < 3: + raise ControlArgument( + "There must be at least three time points if trajectory" + " cost or constraints are specified") + # Search over the null space to minimize cost/satisfy constraints N = sp.linalg.null_space(M) From dd1e2d3abd403e37ee0bbfe2b0cf08bd77f79046 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 20 Jan 2023 22:52:05 -0800 Subject: [PATCH 05/12] add support for 1D gain scheduling --- control/statefbk.py | 11 ++++++++++- control/tests/statefbk_test.py | 30 +++++++++++++++++++++++++++++- 2 files changed, 39 insertions(+), 2 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index ab8369991..c4b6f31de 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -826,6 +826,10 @@ def create_statefbk_iosystem( gainsched_indices = range(sys.nstates) if gainsched_indices is None \ else list(gainsched_indices) + # If points is a 1D list, convert to 2D + if points.ndim == 1: + points = points.reshape(-1, 1) + # Make sure the scheduling variable indices are the right length if len(gainsched_indices) != points.shape[1]: raise ControlArgument( @@ -838,7 +842,12 @@ def create_statefbk_iosystem( gainsched_indices[i] = inputs.index(gainsched_indices[i]) # Create interpolating function - if gainsched_method == 'nearest': + if points.shape[1] < 2: + _interp = sp.interpolate.interp1d( + points[:, 0], gains, axis=0, kind=gainsched_method) + _nearest = sp.interpolate.interp1d( + points[:, 0], gains, axis=0, kind='nearest') + elif gainsched_method == 'nearest': _interp = sp.interpolate.NearestNDInterpolator(points, gains) def _nearest(mu): raise SystemError(f"could not find nearest gain at mu = {mu}") diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index acb47a27c..4579ca2a2 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -924,6 +924,34 @@ def test_gainsched_unicycle(unicycle, method): resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2) +@pytest.mark.parametrize("method", ['nearest', 'linear', 'cubic']) +def test_gainsched_1d(method): + # Define a linear system to test + sys = ct.ss([[-1, 0.1], [0, -2]], [[0], [1]], np.eye(2), 0) + + # Define gains for the first state only + points = [-1, 0, 1] + + # Define gain to be constant + K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs)) + gains = [K for p in points] + + # Define the paramters for the simulations + timepts = np.linspace(0, 10, 100) + X0 = np.ones(sys.nstates) * 1.1 # Start outside defined range + + # Create a controller and simulate the initial response + gs_ctrl, gs_clsys = ct.create_statefbk_iosystem( + sys, (gains, points), gainsched_indices=[0]) + gs_resp = ct.input_output_response(gs_clsys, timepts, 0, X0) + + # Verify that we get the same result as a constant gain + ck_clsys = ct.ss(sys.A - sys.B @ K, sys.B, sys.C, 0) + ck_resp = ct.input_output_response(ck_clsys, timepts, 0, X0) + + np.testing.assert_allclose(gs_resp.states, ck_resp.states) + + def test_gainsched_default_indices(): # Define a linear system to test sys = ct.ss([[-1, 0.1], [0, -2]], [[0], [1]], np.eye(2), 0) @@ -937,7 +965,7 @@ def test_gainsched_default_indices(): # Define the paramters for the simulations timepts = np.linspace(0, 10, 100) - X0 = np.ones(sys.nstates) * 0.9 + X0 = np.ones(sys.nstates) * 1.1 # Start outside defined range # Create a controller and simulate the initial response gs_ctrl, gs_clsys = ct.create_statefbk_iosystem(sys, (gains, points)) From 1535738c5c649f161aa4987f8a18f765745808d8 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 2 Feb 2023 08:31:24 -0800 Subject: [PATCH 06/12] update integral_action docstrings (must be ndarray) --- control/statefbk.py | 41 ++++++++++++++++++++--------------------- control/timeresp.py | 2 +- 2 files changed, 21 insertions(+), 22 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index c4b6f31de..4e5c6c7ec 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -338,12 +338,13 @@ def lqr(*args, **kwargs): N : 2D array, optional Cross weight matrix integral_action : ndarray, optional - If this keyword is specified, the controller includes integral action - in addition to state feedback. The value of the `integral_action`` - keyword should be an ndarray that will be multiplied by the current to - generate the error for the internal integrator states of the control - law. The number of outputs that are to be integrated must match the - number of additional rows and columns in the ``Q`` matrix. + If this keyword is specified, the controller includes integral + action in addition to state feedback. The value of the + `integral_action`` keyword should be an ndarray that will be + multiplied by the current state to generate the error for the + internal integrator states of the control law. The number of + outputs that are to be integrated must match the number of + additional rows and columns in the ``Q`` matrix. method : str, optional Set the method used for computing the result. Current methods are 'slycot' and 'scipy'. If set to None (default), try 'slycot' first @@ -487,12 +488,13 @@ def dlqr(*args, **kwargs): N : 2D array, optional Cross weight matrix integral_action : ndarray, optional - If this keyword is specified, the controller includes integral action - in addition to state feedback. The value of the `integral_action`` - keyword should be an ndarray that will be multiplied by the current to - generate the error for the internal integrator states of the control - law. The number of outputs that are to be integrated must match the - number of additional rows and columns in the ``Q`` matrix. + If this keyword is specified, the controller includes integral + action in addition to state feedback. The value of the + `integral_action`` keyword should be an ndarray that will be + multiplied by the current state to generate the error for the + internal integrator states of the control law. The number of + outputs that are to be integrated must match the number of + additional rows and columns in the ``Q`` matrix. method : str, optional Set the method used for computing the result. Current methods are 'slycot' and 'scipy'. If set to None (default), try 'slycot' first @@ -520,6 +522,7 @@ def dlqr(*args, **kwargs): -------- >>> K, S, E = dlqr(dsys, Q, R, [N]) >>> K, S, E = dlqr(A, B, Q, R, [N]) + """ # @@ -654,16 +657,12 @@ def create_statefbk_iosystem( ud_labels. These settings can also be overriden using the `inputs` keyword. - integral_action : None, ndarray, or func, optional + integral_action : ndarray, optional If this keyword is specified, the controller can include integral - action in addition to state feedback. If ``integral_action`` is a - matrix, it will be multiplied by the current and desired state to - generate the error for the internal integrator states of the control - law. If ``integral_action`` is a function ``h``, that function will - be called with the signature h(t, x, u, params) to obtain the - outputs that should be integrated. The number of outputs that are - to be integrated must match the number of additional columns in the - ``K`` matrix. + action in addition to state feedback. The value of the + `integral_action`` keyword should be an ndarray that will be + multiplied by the current and desired state to generate the error + for the internal integrator states of the control law. estimator : InputOutputSystem, optional If an estimator is provided, use the states of the estimator as diff --git a/control/timeresp.py b/control/timeresp.py index 509107cc8..24f553a32 100644 --- a/control/timeresp.py +++ b/control/timeresp.py @@ -1384,7 +1384,7 @@ def step_info(sysdata, T=None, T_num=None, yfinal=None, Parameters ---------- sysdata : StateSpace or TransferFunction or array_like - The system data. Either LTI system to similate (StateSpace, + The system data. Either LTI system to simulate (StateSpace, TransferFunction), or a time series of step response data. T : array_like or float, optional Time vector, or simulation time duration if a number (time vector is From 1a32831ec6c35c757a243d00a123fbe3d22aac79 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 4 Feb 2023 11:15:22 -0800 Subject: [PATCH 07/12] add signal name kwargs for create_mpc_iosysm + kwargs testing for ct.optimal --- control/optimal.py | 28 +++++++++++++++---------- control/tests/kwargs_test.py | 15 +++++++++++--- control/tests/optimal_test.py | 37 ++++++++++++++++++++++++++++++++++ control/tests/statefbk_test.py | 2 +- 4 files changed, 67 insertions(+), 15 deletions(-) diff --git a/control/optimal.py b/control/optimal.py index 377a6972e..856c6d352 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -18,7 +18,6 @@ from . import config from .exception import ControlNotImplemented -from .timeresp import TimeResponseData # Define module default parameter values _optimal_trajectory_methods = {'shooting', 'collocation'} @@ -140,7 +139,8 @@ class OptimalControlProblem(): def __init__( self, sys, timepts, integral_cost, trajectory_constraints=[], terminal_cost=None, terminal_constraints=[], initial_guess=None, - trajectory_method=None, basis=None, log=False, **kwargs): + trajectory_method=None, basis=None, log=False, kwargs_check=True, + **kwargs): """Set up an optimal control problem.""" # Save the basic information for use later self.system = sys @@ -183,7 +183,7 @@ def __init__( " discrete time systems") # Make sure there were no extraneous keywords - if kwargs: + if kwargs_check and kwargs: raise TypeError("unrecognized keyword(s): ", str(kwargs)) self.trajectory_constraints = _process_constraints( @@ -829,7 +829,7 @@ def compute_mpc(self, x, squeeze=None): return res.inputs[:, 0] # Create an input/output system implementing an MPC controller - def create_mpc_iosystem(self): + def create_mpc_iosystem(self, **kwargs): """Create an I/O system implementing an MPC controller""" # Check to make sure we are in discrete time if self.system.dt == 0: @@ -857,11 +857,17 @@ def _output(t, x, u, params={}): res = self.compute_trajectory(u, print_summary=False) return res.inputs[:, 0] + # Define signal names, if they are not already given + if not kwargs.get('inputs'): + kwargs['inputs'] = self.system.state_labels + if not kwargs.get('outputs'): + kwargs['outputs'] = self.system.input_labels + if not kwargs.get('states'): + kwargs['states'] = self.system.ninputs * \ + (self.timepts.size if self.basis is None else self.basis.N) + return ct.NonlinearIOSystem( - _update, _output, dt=self.system.dt, - inputs=self.system.nstates, outputs=self.system.ninputs, - states=self.system.ninputs * \ - (self.timepts.size if self.basis is None else self.basis.N)) + _update, _output, dt=self.system.dt, **kwargs) # Optimal control result @@ -923,7 +929,7 @@ def __init__( print("* Final cost:", self.cost) # Process data as a time response (with "outputs" = inputs) - response = TimeResponseData( + response = ct.TimeResponseData( ocp.timepts, inputs, states, issiso=ocp.system.issiso(), transpose=transpose, return_x=return_states, squeeze=squeeze) @@ -1129,10 +1135,10 @@ def create_mpc_iosystem( ocp = OptimalControlProblem( sys, horizon, cost, trajectory_constraints=constraints, terminal_cost=terminal_cost, terminal_constraints=terminal_constraints, - log=log, **kwargs) + log=log, kwargs_check=False, **kwargs) # Return an I/O system implementing the model predictive controller - return ocp.create_mpc_iosystem() + return ocp.create_mpc_iosystem(**kwargs) # diff --git a/control/tests/kwargs_test.py b/control/tests/kwargs_test.py index 1cb3b596a..577d8a4dc 100644 --- a/control/tests/kwargs_test.py +++ b/control/tests/kwargs_test.py @@ -22,12 +22,13 @@ import control.tests.flatsys_test as flatsys_test import control.tests.frd_test as frd_test import control.tests.interconnect_test as interconnect_test +import control.tests.optimal_test as optimal_test import control.tests.statefbk_test as statefbk_test import control.tests.trdata_test as trdata_test @pytest.mark.parametrize("module, prefix", [ - (control, ""), (control.flatsys, "flatsys.") + (control, ""), (control.flatsys, "flatsys."), (control.optimal, "optimal.") ]) def test_kwarg_search(module, prefix): # Look through every object in the package @@ -158,7 +159,7 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): kwarg_unittest = { 'bode': test_matplotlib_kwargs, 'bode_plot': test_matplotlib_kwargs, - 'create_statefbk_iosystem': statefbk_test.TestStatefbk.test_statefbk_iosys, + 'create_statefbk_iosystem': statefbk_test.TestStatefbk.test_statefbk_errors, 'describing_function_plot': test_matplotlib_kwargs, 'dlqe': test_unrecognized_kwargs, 'dlqr': test_unrecognized_kwargs, @@ -191,6 +192,8 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): flatsys_test.TestFlatSys.test_point_to_point_errors, 'flatsys.solve_flat_ocp': flatsys_test.TestFlatSys.test_solve_flat_ocp_errors, + 'optimal.create_mpc_iosystem': optimal_test.test_mpc_iosystem_rename, + 'optimal.solve_ocp': optimal_test.test_ocp_argument_errors, 'FrequencyResponseData.__init__': frd_test.TestFRD.test_unrecognized_keyword, 'InputOutputSystem.__init__': test_unrecognized_kwargs, @@ -205,7 +208,13 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): 'StateSpace.sample': test_unrecognized_kwargs, 'TimeResponseData.__call__': trdata_test.test_response_copy, 'TransferFunction.__init__': test_unrecognized_kwargs, - 'TransferFunction.sample': test_unrecognized_kwargs, + 'TransferFunction.sample': test_unrecognized_kwargs, + 'optimal.OptimalControlProblem.__init__': + optimal_test.test_ocp_argument_errors, + 'optimal.OptimalControlProblem.compute_trajectory': + optimal_test.test_ocp_argument_errors, + 'optimal.OptimalControlProblem.create_mpc_iosystem': + optimal_test.test_mpc_iosystem_rename, } # diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py index 53f2c29ad..e0ab392bb 100644 --- a/control/tests/optimal_test.py +++ b/control/tests/optimal_test.py @@ -214,6 +214,35 @@ def test_mpc_iosystem_aircraft(): xout[0:sys.nstates, -1], xd, atol=0.1, rtol=0.01) +def test_mpc_iosystem_rename(): + # Create a discrete time system (double integrator) + cost function + sys = ct.ss([[1, 1], [0, 1]], [[0], [1]], np.eye(2), 0, dt=True) + cost = opt.quadratic_cost(sys, np.eye(2), np.eye(1)) + timepts = np.arange(0, 5) + + # Create the default optimal control problem and check labels + mpc = opt.create_mpc_iosystem(sys, timepts, cost) + assert mpc.input_labels == sys.state_labels + assert mpc.output_labels == sys.input_labels + + # Change the signal names + input_relabels = ['x1', 'x2'] + output_relabels = ['u'] + state_relabels = [f'x_[{i}]' for i in timepts] + mpc_relabeled = opt.create_mpc_iosystem( + sys, timepts, cost, inputs=input_relabels, outputs=output_relabels, + states=state_relabels, name='mpc_relabeled') + assert mpc_relabeled.input_labels == input_relabels + assert mpc_relabeled.output_labels == output_relabels + assert mpc_relabeled.state_labels == state_relabels + assert mpc_relabeled.name == 'mpc_relabeled' + + # Make sure that unknown keywords are caught + # Unrecognized arguments + with pytest.raises(TypeError, match="unrecognized keyword"): + mpc = opt.create_mpc_iosystem(sys, timepts, cost, unknown=None) + + def test_mpc_iosystem_continuous(): # Create a random state space system sys = ct.rss(2, 1, 1) @@ -492,6 +521,14 @@ def test_ocp_argument_errors(): res = opt.solve_ocp( sys, time, x0, cost, constraints, terminal_constraint=None) + with pytest.raises(TypeError, match="unrecognized keyword"): + ocp = opt.OptimalControlProblem( + sys, time, x0, cost, constraints, terminal_constraint=None) + + with pytest.raises(TypeError, match="unrecognized keyword"): + ocp = opt.OptimalControlProblem(sys, time, cost, constraints) + ocp.compute_trajectory(x0, unknown=None) + # Unrecognized trajectory constraint type constraints = [(None, np.eye(3), [0, 0, 0], [0, 0, 0])] with pytest.raises(TypeError, match="unknown trajectory constraint type"): diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 4579ca2a2..64ccaea44 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -778,7 +778,7 @@ def test_statefbk_errors(self): ctrl, clsys = ct.create_statefbk_iosystem( sys, K, type='nonlinear', controller_type='nonlinear') - with pytest.raises(TypeError, match="unrecognized keywords"): + with pytest.raises(TypeError, match="unrecognized keyword"): ctrl, clsys = ct.create_statefbk_iosystem(sys, K, typo='nonlinear') with pytest.raises(ControlArgument, match="unknown controller_type"): From 20177f2afbce9d4a5a5fffa6f925218447b0f37a Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 4 Feb 2023 15:55:52 -0800 Subject: [PATCH 08/12] add warning if output might not be state in create_statefbk_iosystem --- control/statefbk.py | 8 +++++++- control/tests/statefbk_test.py | 11 +++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 4e5c6c7ec..c19616a65 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -718,7 +718,7 @@ def create_statefbk_iosystem( List of strings that name the individual signals of the transformed system. If not given, the inputs and outputs are the same as the original system. - + name : string, optional System name. If unspecified, a generic name is generated with a unique integer id. @@ -750,6 +750,12 @@ def create_statefbk_iosystem( # TODO: check to make sure output map is the identity raise ControlArgument("System output must be the full state") else: + # Issue a warning if we can't verify state output + if (isinstance(sys, NonlinearIOSystem) and sys.outfcn is not None) or \ + (isinstance(sys, StateSpace) and + not (np.all(sys.C == np.eye(sys.nstates)) and np.all(sys.D == 0))): + warnings.warn("cannot verify system output is system state") + # Use the system directly instead of an estimator estimator = sys diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 64ccaea44..ec87591f6 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -754,6 +754,12 @@ def test_statefbk_errors(self): sys = ct.rss(4, 4, 2, strictly_proper=True) K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs)) + with pytest.warns(UserWarning, match="cannot verify system output"): + ctrl, clsys = ct.create_statefbk_iosystem(sys, K) + + # reset the system output + sys.C = np.eye(sys.nstates) + with pytest.raises(ControlArgument, match="must be I/O system"): sys_tf = ct.tf([1], [1, 1]) ctrl, clsys = ct.create_statefbk_iosystem(sys_tf, K) @@ -806,11 +812,8 @@ def unicycle(): def unicycle_update(t, x, u, params): return np.array([np.cos(x[2]) * u[0], np.sin(x[2]) * u[0], u[1]]) - def unicycle_output(t, x, u, params): - return x - return ct.NonlinearIOSystem( - unicycle_update, unicycle_output, + unicycle_update, None, inputs = ['v', 'phi'], outputs = ['x', 'y', 'theta'], states = ['x_', 'y_', 'theta_']) From e1e5bc9e41fdae4a2bb341811abdc1c3d4445c53 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Wed, 8 Feb 2023 08:23:08 -0800 Subject: [PATCH 09/12] charge 'horizon' to 'timepts' in solve_ocp --- control/optimal.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/control/optimal.py b/control/optimal.py index 856c6d352..0cc5cfc1a 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -940,7 +940,7 @@ def __init__( # Compute the input for a nonlinear, (constrained) optimal control problem def solve_ocp( - sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None, + sys, timepts, X0, cost, trajectory_constraints=None, terminal_cost=None, terminal_constraints=[], initial_guess=None, basis=None, squeeze=None, transpose=None, return_states=True, print_summary=True, log=False, **kwargs): @@ -952,7 +952,7 @@ def solve_ocp( sys : InputOutputSystem I/O system for which the optimal input will be computed. - horizon : 1D array_like + timepts : 1D array_like List of times at which the optimal input should be computed. X0: array-like or number, optional @@ -990,9 +990,9 @@ def solve_ocp( initial_guess : 1D or 2D array_like Initial inputs to use as a guess for the optimal input. The inputs - should either be a 2D vector of shape (ninputs, horizon) or a 1D - input of shape (ninputs,) that will be broadcast by extension of the - time axis. + should either be a 2D vector of shape (ninputs, len(timepts)) or a + 1D input of shape (ninputs,) that will be broadcast by extension of + the time axis. log : bool, optional If `True`, turn on logging messages (using Python logging module). @@ -1069,7 +1069,7 @@ def solve_ocp( # Set up the optimal control problem ocp = OptimalControlProblem( - sys, horizon, cost, trajectory_constraints=trajectory_constraints, + sys, timepts, cost, trajectory_constraints=trajectory_constraints, terminal_cost=terminal_cost, terminal_constraints=terminal_constraints, initial_guess=initial_guess, basis=basis, log=log, **kwargs) @@ -1081,12 +1081,12 @@ def solve_ocp( # Create a model predictive controller for an optimal control problem def create_mpc_iosystem( - sys, horizon, cost, constraints=[], terminal_cost=None, + sys, timepts, cost, constraints=[], terminal_cost=None, terminal_constraints=[], log=False, **kwargs): """Create a model predictive I/O control system This function creates an input/output system that implements a model - predictive control for a system given the time horizon, cost function and + predictive control for a system given the time points, cost function and constraints that define the finite-horizon optimization that should be carried out at each state. @@ -1095,7 +1095,7 @@ def create_mpc_iosystem( sys : InputOutputSystem I/O system for which the optimal input will be computed. - horizon : 1D array_like + timepts : 1D array_like List of times at which the optimal input should be computed. cost : callable @@ -1133,7 +1133,7 @@ def create_mpc_iosystem( """ # Set up the optimal control problem ocp = OptimalControlProblem( - sys, horizon, cost, trajectory_constraints=constraints, + sys, timepts, cost, trajectory_constraints=constraints, terminal_cost=terminal_cost, terminal_constraints=terminal_constraints, log=log, kwargs_check=False, **kwargs) From 867b353cf2498d17750c24354f89c9ad02e97637 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 18 Feb 2023 13:09:02 -0800 Subject: [PATCH 10/12] allow passthru system inputs in create_statefbk_iosystem --- control/iosys.py | 68 +++++++++++++++----- control/statefbk.py | 110 +++++++++++++++++++++------------ control/stochsys.py | 1 + control/tests/iosys_test.py | 37 ++++++++++- control/tests/statefbk_test.py | 54 +++++++++++++++- 5 files changed, 212 insertions(+), 58 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index f2212d33b..69da2f35a 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -1373,13 +1373,11 @@ def unused_signals(self): ------- unused_inputs : dict - A mapping from tuple of indices (isys, isig) to string '{sys}.{sig}', for all unused subsystem inputs. unused_outputs : dict - - A mapping from tuple of indices (isys, isig) to string + A mapping from tuple of indices (osys, osig) to string '{sys}.{sig}', for all unused subsystem outputs. """ @@ -1433,10 +1431,13 @@ def _find_outputs_by_basename(self, basename): for sig, isig in sys.output_index.items() if sig == (basename)} - def check_unused_signals(self, ignore_inputs=None, ignore_outputs=None): + def check_unused_signals( + self, ignore_inputs=None, ignore_outputs=None, warning=True): """Check for unused subsystem inputs and outputs - If any unused inputs or outputs are found, emit a warning. + Check to see if there are any unused signals and return a list of + unused input and output signal descriptions. If `warning` is True + and any unused inputs or outputs are found, emit a warning. Parameters ---------- @@ -1454,6 +1455,16 @@ def check_unused_signals(self, ignore_inputs=None, ignore_outputs=None): If the 'sig' form is used, all subsystem outputs with that name are considered ignored. + Returns + ------- + dropped_inputs: list of tuples + A list of the dropped input signals, with each element of the + list in the form of (isys, isig). + + dropped_outputs: list of tuples + A list of the dropped output signals, with each element of the + list in the form of (osys, osig). + """ if ignore_inputs is None: @@ -1477,7 +1488,7 @@ def check_unused_signals(self, ignore_inputs=None, ignore_outputs=None): ignore_input_map[self._parse_signal( ignore_input, 'input')[:2]] = ignore_input - # (isys, isig) -> signal-spec + # (osys, osig) -> signal-spec ignore_output_map = {} for ignore_output in ignore_outputs: if isinstance(ignore_output, str) and '.' not in ignore_output: @@ -1496,30 +1507,32 @@ def check_unused_signals(self, ignore_inputs=None, ignore_outputs=None): used_ignored_inputs = set(ignore_input_map) - set(unused_inputs) used_ignored_outputs = set(ignore_output_map) - set(unused_outputs) - if dropped_inputs: + if warning and dropped_inputs: msg = ('Unused input(s) in InterconnectedSystem: ' + '; '.join(f'{inp}={unused_inputs[inp]}' for inp in dropped_inputs)) warn(msg) - if dropped_outputs: + if warning and dropped_outputs: msg = ('Unused output(s) in InterconnectedSystem: ' + '; '.join(f'{out} : {unused_outputs[out]}' for out in dropped_outputs)) warn(msg) - if used_ignored_inputs: + if warning and used_ignored_inputs: msg = ('Input(s) specified as ignored is (are) used: ' + '; '.join(f'{inp} : {ignore_input_map[inp]}' for inp in used_ignored_inputs)) warn(msg) - if used_ignored_outputs: + if warning and used_ignored_outputs: msg = ('Output(s) specified as ignored is (are) used: ' + '; '.join(f'{out}={ignore_output_map[out]}' for out in used_ignored_outputs)) warn(msg) + return dropped_inputs, dropped_outputs + class LinearICSystem(InterconnectedSystem, LinearIOSystem): @@ -2580,9 +2593,10 @@ def tf2io(*args, **kwargs): # Function to create an interconnected system -def interconnect(syslist, connections=None, inplist=None, outlist=None, - params=None, check_unused=True, ignore_inputs=None, - ignore_outputs=None, warn_duplicate=None, **kwargs): +def interconnect( + syslist, connections=None, inplist=None, outlist=None, params=None, + check_unused=True, add_unused=False, ignore_inputs=None, + ignore_outputs=None, warn_duplicate=None, **kwargs): """Interconnect a set of input/output systems. This function creates a new system that is an interconnection of a set of @@ -2653,8 +2667,8 @@ def interconnect(syslist, connections=None, inplist=None, outlist=None, the system input connects to only one subsystem input, a single input specification can be given (without the inner list). - If omitted, the input map can be specified using the - :func:`~control.InterconnectedSystem.set_input_map` method. + If omitted the `input` parameter will be used to identify the list + of input signals to the overall system. outlist : list of output connections, optional List of connections for how the outputs from the subsystems are mapped @@ -2886,10 +2900,30 @@ def interconnect(syslist, connections=None, inplist=None, outlist=None, outlist = new_outlist newsys = InterconnectedSystem( - syslist, connections=connections, inplist=inplist, outlist=outlist, - inputs=inputs, outputs=outputs, states=states, + syslist, connections=connections, inplist=inplist, + outlist=outlist, inputs=inputs, outputs=outputs, states=states, params=params, dt=dt, name=name, warn_duplicate=warn_duplicate) + # See if we should add any signals + if add_unused: + # Get all unused signals + dropped_inputs, dropped_outputs = newsys.check_unused_signals( + ignore_inputs, ignore_outputs, warning=False) + + # Add on any unused signals that we aren't ignoring + for isys, isig in dropped_inputs: + inplist.append((isys, isig)) + inputs.append(newsys.syslist[isys].input_labels[isig]) + for osys, osig in dropped_outputs: + outlist.append((osys, osig)) + outputs.append(newsys.syslist[osys].output_labels[osig]) + + # Rebuild the system with new inputs/outputs + newsys = InterconnectedSystem( + syslist, connections=connections, inplist=inplist, + outlist=outlist, inputs=inputs, outputs=outputs, states=states, + params=params, dt=dt, name=name, warn_duplicate=warn_duplicate) + # check for implicitly dropped signals if check_unused: newsys.check_unused_signals(ignore_inputs, ignore_outputs) diff --git a/control/statefbk.py b/control/statefbk.py index c19616a65..c76a4e31a 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -603,8 +603,8 @@ def dlqr(*args, **kwargs): def create_statefbk_iosystem( sys, gain, integral_action=None, estimator=None, controller_type=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None, - gainsched_method='linear', name=None, inputs=None, outputs=None, - states=None, **kwargs): + gainsched_method='linear', control_indices=None, state_indices=None, + name=None, inputs=None, outputs=None, states=None, **kwargs): """Create an I/O system using a (full) state feedback controller This function creates an input/output system that implements a @@ -714,6 +714,20 @@ def create_statefbk_iosystem( Other Parameters ---------------- + control_indices : list of int or str, optional + Specify the indices of the system inputs that should be determined + by the state feedback controller. If not specified, defaults to + the first `m` system inputs, where `m` is determined by the shape + of the gain matrix, with the remaining inputs remaining as inputs + to the overall closed loop system. + + state_indices : list of int or str, optional + Specify the indices of the system (or estimator) outputs that + should be used by the state feedback controller. If not specified, + defaults to the first `n` system/estimator outputs, where `n` is + determined by the shape of the gain matrix, with the remaining + outputs remaining as outputs to the overall closed loop system. + inputs, outputs : str, or list of str, optional List of strings that name the individual signals of the transformed system. If not given, the inputs and outputs are the same as the @@ -740,31 +754,47 @@ def create_statefbk_iosystem( if kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) - # See whether we were give an estimator - if estimator is not None: - # Check to make sure the estimator is the right size - if estimator.noutputs != sys.nstates: - raise ControlArgument("Estimator output size must match state") - elif sys.noutputs != sys.nstates: + # Figure out what inputs to the system to use + control_indices = range(sys.ninputs) if control_indices is None \ + else list(control_indices) + for i, idx in enumerate(control_indices): + if isinstance(idx, str): + control_indices[i] = sys.input_labels.index(control_indices[i]) + sys_ninputs = len(control_indices) + + # Decide what system is going to pass the states to the controller + if estimator is None: + estimator = sys + + # Figure out what outputs (states) from the system/estimator to use + state_indices = range(sys.nstates) if state_indices is None \ + else list(state_indices) + for i, idx in enumerate(state_indices): + if isinstance(idx, str): + state_indices[i] = estimator.state_labels.index(state_indices[i]) + sys_nstates = len(state_indices) + + # Make sure the system/estimator states are proper dimension + if estimator.noutputs < sys_nstates: # If no estimator, make sure that the system has all states as outputs - # TODO: check to make sure output map is the identity - raise ControlArgument("System output must be the full state") - else: + raise ControlArgument( + ("system" if estimator == sys else "estimator") + + " output must include the full state") + elif estimator == sys: # Issue a warning if we can't verify state output if (isinstance(sys, NonlinearIOSystem) and sys.outfcn is not None) or \ (isinstance(sys, StateSpace) and - not (np.all(sys.C == np.eye(sys.nstates)) and np.all(sys.D == 0))): + not (np.all(sys.C[np.ix_(state_indices, state_indices)] == + np.eye(sys_nstates)) and + np.all(sys.D[state_indices, :] == 0))): warnings.warn("cannot verify system output is system state") - # Use the system directly instead of an estimator - estimator = sys - # See whether we should implement integral action nintegrators = 0 if integral_action is not None: if not isinstance(integral_action, np.ndarray): raise ControlArgument("Integral action must pass an array") - elif integral_action.shape[1] != sys.nstates: + elif integral_action.shape[1] != sys_nstates: raise ControlArgument( "Integral gain size must match system state size") else: @@ -772,15 +802,15 @@ def create_statefbk_iosystem( C = integral_action else: # Create a C matrix with no outputs, just in case update gets called - C = np.zeros((0, sys.nstates)) + C = np.zeros((0, sys_nstates)) # Check to make sure that state feedback has the right shape if isinstance(gain, np.ndarray): K = gain - if K.shape != (sys.ninputs, estimator.noutputs + nintegrators): + if K.shape != (sys_ninputs, estimator.noutputs + nintegrators): raise ControlArgument( - f'Control gain must be an array of size {sys.ninputs}' - f'x {sys.nstates}' + + f'control gain must be an array of size {sys_ninputs}' + f' x {sys_nstates}' + (f'+{nintegrators}' if nintegrators > 0 else '')) gainsched = False @@ -811,24 +841,24 @@ def create_statefbk_iosystem( # Figure out the labels to use if isinstance(xd_labels, str): # Generate the list of labels using the argument as a format string - xd_labels = [xd_labels.format(i=i) for i in range(sys.nstates)] + xd_labels = [xd_labels.format(i=i) for i in range(sys_nstates)] if isinstance(ud_labels, str): # Generate the list of labels using the argument as a format string - ud_labels = [ud_labels.format(i=i) for i in range(sys.ninputs)] + ud_labels = [ud_labels.format(i=i) for i in range(sys_ninputs)] # Create the signal and system names if inputs is None: inputs = xd_labels + ud_labels + estimator.output_labels if outputs is None: - outputs = list(sys.input_index.keys()) + outputs = [sys.input_labels[i] for i in control_indices] if states is None: states = nintegrators # Process gainscheduling variables, if present if gainsched: # Create a copy of the scheduling variable indices (default = xd) - gainsched_indices = range(sys.nstates) if gainsched_indices is None \ + gainsched_indices = range(sys_nstates) if gainsched_indices is None \ else list(gainsched_indices) # If points is a 1D list, convert to 2D @@ -877,8 +907,8 @@ def _compute_gain(mu): # Create an I/O system for the state feedback gains def _control_update(t, states, inputs, params): # Split input into desired state, nominal input, and current state - xd_vec = inputs[0:sys.nstates] - x_vec = inputs[-estimator.nstates:] + xd_vec = inputs[0:sys_nstates] + x_vec = inputs[-sys_nstates:] # Compute the integral error in the xy coordinates return C @ (x_vec - xd_vec) @@ -891,14 +921,14 @@ def _control_output(t, states, inputs, params): K_ = params.get('K') # Split input into desired state, nominal input, and current state - xd_vec = inputs[0:sys.nstates] - ud_vec = inputs[sys.nstates:sys.nstates + sys.ninputs] - x_vec = inputs[-sys.nstates:] + xd_vec = inputs[0:sys_nstates] + ud_vec = inputs[sys_nstates:sys_nstates + sys_ninputs] + x_vec = inputs[-sys_nstates:] # Compute the control law - u = ud_vec - K_[:, 0:sys.nstates] @ (x_vec - xd_vec) + u = ud_vec - K_[:, 0:sys_nstates] @ (x_vec - xd_vec) if nintegrators > 0: - u -= K_[:, sys.nstates:] @ states + u -= K_[:, sys_nstates:] @ states return u @@ -915,10 +945,10 @@ def _control_output(t, states, inputs, params): else: # Discrete time: summer A_lqr = np.eye(C.shape[0]) - B_lqr = np.hstack([-C, np.zeros((C.shape[0], sys.ninputs)), C]) - C_lqr = -K[:, sys.nstates:] + B_lqr = np.hstack([-C, np.zeros((C.shape[0], sys_ninputs)), C]) + C_lqr = -K[:, sys_nstates:] D_lqr = np.hstack([ - K[:, 0:sys.nstates], np.eye(sys.ninputs), -K[:, 0:sys.nstates] + K[:, 0:sys_nstates], np.eye(sys_ninputs), -K[:, 0:sys_nstates] ]) ctrl = ss( @@ -929,12 +959,16 @@ def _control_output(t, states, inputs, params): raise ControlArgument(f"unknown controller_type '{controller_type}'") # Define the closed loop system + inplist=inputs[:-sys.nstates] + input_labels=inputs[:-sys.nstates] + outlist=sys.output_labels + [sys.input_labels[i] for i in control_indices] + output_labels=sys.output_labels + \ + [sys.input_labels[i] for i in control_indices] closed = interconnect( [sys, ctrl] if estimator == sys else [sys, ctrl, estimator], - name=sys.name + "_" + ctrl.name, - inplist=inputs[:-sys.nstates], inputs=inputs[:-sys.nstates], - outlist=sys.output_labels + sys.input_labels, - outputs=sys.output_labels + sys.input_labels + name=sys.name + "_" + ctrl.name, add_unused=True, + inplist=inplist, inputs=input_labels, + outlist=outlist, outputs=output_labels ) return ctrl, closed diff --git a/control/stochsys.py b/control/stochsys.py index 90768a222..000c1b6ab 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -564,6 +564,7 @@ def white_noise(T, Q, dt=0): # Return a linear combination of the noise sources return sp.linalg.sqrtm(Q) @ W + def correlation(T, X, Y=None, squeeze=True): """Compute the correlation of time signals. diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index aaa97ec39..6d8d62135 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -1683,7 +1683,6 @@ def test_interconnect_unused_input(): h = ct.interconnect([g,s,k], connections=False) - # warn if explicity ignored input in fact used with pytest.warns( UserWarning, @@ -1781,6 +1780,42 @@ def test_interconnect_unused_output(): ignore_outputs=['v']) +def test_interconnect_add_unused(): + P = ct.ss( + [[-1]], [[1, -1]], [[-1], [1]], 0, + inputs=['u1', 'u2'], outputs=['y1','y2'], name='g') + S = ct.summing_junction(inputs=['r','-y1'], outputs=['e'], name='s') + C = ct.ss(0, 10, 2, 0, inputs=['e'], outputs=['u1'], name='k') + + # Try a normal interconnection + G1 = ct.interconnect( + [P, S, C], inputs=['r', 'u2'], outputs=['y1', 'y2']) + + # Same system, but using add_unused + G2 = ct.interconnect( + [P, S, C], inputs=['r'], outputs=['y1'], add_unused=True) + assert G2.input_labels == G1.input_labels + assert G2.input_offset == G1.input_offset + assert G2.output_labels == G1.output_labels + assert G2.output_offset == G1.output_offset + + # Ignore one of the inputs + G3 = ct.interconnect( + [P, S, C], inputs=['r'], outputs=['y1'], add_unused=True, + ignore_inputs=['u2']) + assert G3.input_labels == G1.input_labels[0:1] + assert G3.output_labels == G1.output_labels + assert G3.output_offset == G1.output_offset + + # Ignore one of the outputs + G4 = ct.interconnect( + [P, S, C], inputs=['r'], outputs=['y1'], add_unused=True, + ignore_outputs=['y2']) + assert G4.input_labels == G1.input_labels + assert G4.input_offset == G1.input_offset + assert G4.output_labels == G1.output_labels[0:1] + + def test_input_output_broadcasting(): # Create a system, time vector, and noisy input sys = ct.rss(6, 2, 3) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index ec87591f6..21d8036f8 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -628,6 +628,54 @@ def test_statefbk_iosys( np.testing.assert_array_almost_equal(clsys.C, Cc) np.testing.assert_array_almost_equal(clsys.D, Dc) + def test_statefbk_iosys_unused(self): + # Create a base system to work with + sys = ct.rss(2, 1, 1, strictly_proper=True) + + # Create a system with extra input + aug = ct.rss(2, inputs=[sys.input_labels[0], 'd'], + outputs=sys.output_labels, strictly_proper=True,) + aug.A = sys.A + aug.B[:, 0:1] = sys.B + + # Create an estimator + est = ct.create_estimator_iosystem( + sys, np.eye(sys.ninputs), np.eye(sys.noutputs)) + + # Design an LQR controller + K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs)) + + # Create a baseline I/O control system + ctrl0, clsys0 = ct.create_statefbk_iosystem(sys, K, estimator=est) + clsys0_lin = clsys0.linearize(0, 0) + + # Create an I/O system with additional inputs + ctrl1, clsys1 = ct.create_statefbk_iosystem( + aug, K, estimator=est, control_indices=[0]) + clsys1_lin = clsys1.linearize(0, 0) + + # Make sure the extra inputs are there + assert aug.input_labels[1] not in clsys0.input_labels + assert aug.input_labels[1] in clsys1.input_labels + np.testing.assert_allclose(clsys0_lin.A, clsys1_lin.A) + + # Switch around which input we use + aug = ct.rss(2, inputs=['d', sys.input_labels[0]], + outputs=sys.output_labels, strictly_proper=True,) + aug.A = sys.A + aug.B[:, 1:2] = sys.B + + # Create an I/O system with additional inputs + ctrl2, clsys2 = ct.create_statefbk_iosystem( + aug, K, estimator=est, control_indices=[1]) + clsys2_lin = clsys2.linearize(0, 0) + + # Make sure the extra inputs are there + assert aug.input_labels[0] not in clsys0.input_labels + assert aug.input_labels[0] in clsys1.input_labels + np.testing.assert_allclose(clsys0_lin.A, clsys2_lin.A) + + def test_lqr_integral_continuous(self): # Generate a continuous time system for testing sys = ct.rss(4, 4, 2, strictly_proper=True) @@ -764,11 +812,13 @@ def test_statefbk_errors(self): sys_tf = ct.tf([1], [1, 1]) ctrl, clsys = ct.create_statefbk_iosystem(sys_tf, K) - with pytest.raises(ControlArgument, match="output size must match"): + with pytest.raises(ControlArgument, + match="estimator output must include the full"): est = ct.rss(3, 3, 2) ctrl, clsys = ct.create_statefbk_iosystem(sys, K, estimator=est) - with pytest.raises(ControlArgument, match="must be the full state"): + with pytest.raises(ControlArgument, + match="system output must include the full state"): sys_nf = ct.rss(4, 3, 2, strictly_proper=True) ctrl, clsys = ct.create_statefbk_iosystem(sys_nf, K) From 2fcc287b34c698dfb35e5ee303b996f43638169b Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Feb 2023 10:06:09 -0800 Subject: [PATCH 11/12] sampling a LinearIOSystem returns a LinearIOSystem --- control/iosys.py | 6 ++++++ control/tests/iosys_test.py | 11 +++++++++++ control/tests/kwargs_test.py | 9 ++++++--- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 69da2f35a..75392432a 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -676,6 +676,12 @@ def __init__(self, linsys, **kwargs): StateSpace.__init__( self, linsys, remove_useless_states=False, init_namedio=False) + # When sampling a LinearIO system, return a LinearIOSystem + def sample(self, *args, **kwargs): + return LinearIOSystem(StateSpace.sample(self, *args, **kwargs)) + + sample.__doc__ = StateSpace.sample.__doc__ + # The following text needs to be replicated from StateSpace in order for # this entry to show up properly in sphinx doccumentation (not sure why, # but it was the only way to get it to work). diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index 6d8d62135..59338fc62 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -2036,3 +2036,14 @@ def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect): # Check that we got the expected result as well np.testing.assert_allclose(np.array(xeq), x_expect, atol=1e-6) np.testing.assert_allclose(np.array(ueq), u_expect, atol=1e-6) + +def test_iosys_sample(): + csys = ct.rss(2, 1, 1) + dsys = csys.sample(0.1) + assert isinstance(dsys, ct.LinearIOSystem) + assert dsys.dt == 0.1 + + csys = ct.rss(2, 1, 1) + dsys = ct.sample_system(csys, 0.1) + assert isinstance(dsys, ct.LinearIOSystem) + assert dsys.dt == 0.1 diff --git a/control/tests/kwargs_test.py b/control/tests/kwargs_test.py index 577d8a4dc..fe4e67aaa 100644 --- a/control/tests/kwargs_test.py +++ b/control/tests/kwargs_test.py @@ -86,8 +86,8 @@ def test_kwarg_search(module, prefix): (control.lqr, 1, 0, ([[1, 0], [0, 1]], [[1]]), {}), (control.linearize, 1, 0, (0, 0), {}), (control.pzmap, 1, 0, (), {}), - (control.rlocus, 0, 1, ( ), {}), - (control.root_locus, 0, 1, ( ), {}), + (control.rlocus, 0, 1, (), {}), + (control.root_locus, 0, 1, (), {}), (control.rss, 0, 0, (2, 1, 1), {}), (control.set_defaults, 0, 0, ('control',), {'default_dt': True}), (control.ss, 0, 0, (0, 0, 0, 0), {'dt': 1}), @@ -101,7 +101,9 @@ def test_kwarg_search(module, prefix): (control.InputOutputSystem, 0, 0, (), {'inputs': 1, 'outputs': 1, 'states': 1}), (control.InputOutputSystem.linearize, 1, 0, (0, 0), {}), - (control.StateSpace, 0, 0, ([[-1, 0], [0, -1]], [[1], [1]], [[1, 1]], 0), {}), + (control.LinearIOSystem.sample, 1, 0, (0.1,), {}), + (control.StateSpace, 0, 0, + ([[-1, 0], [0, -1]], [[1], [1]], [[1, 1]], 0), {}), (control.TransferFunction, 0, 0, ([1], [1, 1]), {})] ) def test_unrecognized_kwargs(function, nsssys, ntfsys, moreargs, kwargs, @@ -202,6 +204,7 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): interconnect_test.test_interconnect_exceptions, 'LinearIOSystem.__init__': interconnect_test.test_interconnect_exceptions, + 'LinearIOSystem.sample': test_unrecognized_kwargs, 'NonlinearIOSystem.__init__': interconnect_test.test_interconnect_exceptions, 'StateSpace.__init__': test_unrecognized_kwargs, From 574cf69822412b90ba65806651c995d7516786a1 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 17 Mar 2023 22:11:17 -0700 Subject: [PATCH 12/12] small tweaks to comments, docstrings, unit tests --- control/iosys.py | 12 ++++++++---- control/optimal.py | 16 ++++++++-------- control/tests/statefbk_test.py | 12 ++++++------ 3 files changed, 22 insertions(+), 18 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 75392432a..6b0f6cfaa 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -2727,12 +2727,16 @@ def interconnect( System name (used for specifying signals). If unspecified, a generic name is generated with a unique integer id. - check_unused : bool + check_unused : bool, optional If True, check for unused sub-system signals. This check is not done if connections is False, and neither input nor output mappings are specified. - ignore_inputs : list of input-spec + add_unused : bool, optional + If True, subsystem signals that are not connected to other components + are added as inputs and outputs of the interconnected system. + + ignore_inputs : list of input-spec, optional A list of sub-system inputs known not to be connected. This is *only* used in checking for unused signals, and does not disable use of the input. @@ -2742,7 +2746,7 @@ def interconnect( signals from all sub-systems with that base name are considered ignored. - ignore_outputs : list of output-spec + ignore_outputs : list of output-spec, optional A list of sub-system outputs known not to be connected. This is *only* used in checking for unused signals, and does not disable use of the output. @@ -2752,7 +2756,7 @@ def interconnect( outputs from all sub-systems with that base name are considered ignored. - warn_duplicate : None, True, or False + warn_duplicate : None, True, or False, optional Control how warnings are generated if duplicate objects or names are detected. In `None` (default), then warnings are generated for systems that have non-generic names. If `False`, warnings are not diff --git a/control/optimal.py b/control/optimal.py index 0cc5cfc1a..9a8218a91 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -104,14 +104,14 @@ class OptimalControlProblem(): Notes ----- - To describe an optimal control problem we need an input/output system, a - time horizon, a cost function, and (optionally) a set of constraints on - the state and/or input, either along the trajectory and at the terminal - time. This class sets up an optimization over the inputs at each point in - time, using the integral and terminal costs as well as the trajectory and - terminal constraints. The `compute_trajectory` method sets up an - optimization problem that can be solved using - :func:`scipy.optimize.minimize`. + To describe an optimal control problem we need an input/output system, + a set of time points over a a fixed horizon, a cost function, and + (optionally) a set of constraints on the state and/or input, either + along the trajectory and at the terminal time. This class sets up an + optimization over the inputs at each point in time, using the integral + and terminal costs as well as the trajectory and terminal constraints. + The `compute_trajectory` method sets up an optimization problem that + can be solved using :func:`scipy.optimize.minimize`. The `_cost_function` method takes the information computes the cost of the trajectory generated by the proposed input. It does this by calling a diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 21d8036f8..951c817f1 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -512,7 +512,7 @@ def test_lqr_discrete(self): K, S, E = ct.dlqr(csys, Q, R) @pytest.mark.parametrize( - 'nstates, noutputs, ninputs, nintegrators, type', + 'nstates, noutputs, ninputs, nintegrators, type_', [(2, 0, 1, 0, None), (2, 1, 1, 0, None), (4, 0, 2, 0, None), @@ -525,7 +525,7 @@ def test_lqr_discrete(self): (4, 3, 2, 2, 'nonlinear'), ]) def test_statefbk_iosys( - self, nstates, ninputs, noutputs, nintegrators, type): + self, nstates, ninputs, noutputs, nintegrators, type_): # Create the system to be controlled (and estimator) # TODO: make sure it is controllable? if noutputs == 0: @@ -571,14 +571,14 @@ def test_statefbk_iosys( # Create an I/O system for the controller ctrl, clsys = ct.create_statefbk_iosystem( sys, K, integral_action=C_int, estimator=est, - controller_type=type, name=type) + controller_type=type_, name=type_) # Make sure the name got set correctly - if type is not None: - assert ctrl.name == type + if type_ is not None: + assert ctrl.name == type_ # If we used a nonlinear controller, linearize it for testing - if type == 'nonlinear': + if type_ == 'nonlinear': clsys = clsys.linearize(0, 0) # Make sure the linear system elements are correct