diff --git a/control/frdata.py b/control/frdata.py index a80208963..c43a241e4 100644 --- a/control/frdata.py +++ b/control/frdata.py @@ -204,7 +204,7 @@ def __init__(self, *args, **kwargs): w=1.0/(absolute(self.fresp[i, j, :]) + 0.001), s=0.0) else: self.ifunc = None - LTI.__init__(self, self.fresp.shape[1], self.fresp.shape[0]) + super().__init__(self.fresp.shape[1], self.fresp.shape[0]) def __str__(self): """String representation of the transfer function.""" diff --git a/control/iosys.py b/control/iosys.py index 916fe9d6a..142fdf0cc 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -31,7 +31,9 @@ import copy from warnings import warn +from .namedio import _NamedIOStateSystem, _process_signal_list from .statesp import StateSpace, tf2ss, _convert_to_statespace +from .statesp import _ss, _rss_generate from .xferfcn import TransferFunction from .timeresp import _check_convert_array, _process_time_response, \ TimeResponseData @@ -40,8 +42,8 @@ __all__ = ['InputOutputSystem', 'LinearIOSystem', 'NonlinearIOSystem', 'InterconnectedSystem', 'LinearICSystem', 'input_output_response', - 'find_eqpt', 'linearize', 'ss2io', 'tf2io', 'interconnect', - 'summing_junction'] + 'find_eqpt', 'linearize', 'ss', 'rss', 'drss', 'ss2io', 'tf2io', + 'interconnect', 'summing_junction'] # Define module default parameter values _iosys_defaults = { @@ -53,7 +55,7 @@ } -class InputOutputSystem(object): +class InputOutputSystem(_NamedIOStateSystem): """A class for representing input/output systems. The InputOutputSystem class allows (possibly nonlinear) input/output @@ -124,14 +126,6 @@ class for a set of subclasses that are used to implement specific # Allow ndarray * InputOutputSystem to give IOSystem._rmul_() priority __array_priority__ = 12 # override ndarray, matrix, SS types - _idCounter = 0 - - def _name_or_default(self, name=None): - if name is None: - name = "sys[{}]".format(InputOutputSystem._idCounter) - InputOutputSystem._idCounter += 1 - return name - def __init__(self, inputs=None, outputs=None, states=None, params={}, name=None, **kwargs): """Create an input/output system. @@ -144,58 +138,19 @@ def __init__(self, inputs=None, outputs=None, states=None, params={}, :class:`~control.InterconnectedSystem`. """ - # Store the input arguments + # Store the system name, inputs, outputs, and states + _NamedIOStateSystem.__init__( + self, inputs=inputs, outputs=outputs, states=states, name=name) # default parameters self.params = params.copy() - # timebase - self.dt = kwargs.get('dt', config.defaults['control.default_dt']) - # system name - self.name = self._name_or_default(name) - # Parse and store the number of inputs, outputs, and states - self.set_inputs(inputs) - self.set_outputs(outputs) - self.set_states(states) - - # - # Class attributes - # - # These attributes are defined as class attributes so that they are - # documented properly. They are "overwritten" in __init__. - # - - #: Number of system inputs. - #: - #: :meta hide-value: - ninputs = 0 - - #: Number of system outputs. - #: - #: :meta hide-value: - noutputs = 0 - - #: Number of system states. - #: - #: :meta hide-value: - nstates = 0 - - def __repr__(self): - return self.name if self.name is not None else str(type(self)) + # timebase + self.dt = kwargs.pop('dt', config.defaults['control.default_dt']) - def __str__(self): - """String representation of an input/output system""" - str = "System: " + (self.name if self.name else "(None)") + "\n" - str += "Inputs (%s): " % self.ninputs - for key in self.input_index: - str += key + ", " - str += "\nOutputs (%s): " % self.noutputs - for key in self.output_index: - str += key + ", " - str += "\nStates (%s): " % self.nstates - for key in self.state_index: - str += key + ", " - return str + # Make sure there were no extraneous keyworks + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) def __mul__(sys2, sys1): """Multiply two input/output systems (series interconnection)""" @@ -393,34 +348,6 @@ def __neg__(sys): # Return the newly created system return newsys - def _isstatic(self): - """Check to see if a system is a static system (no states)""" - return self.nstates == 0 - - # Utility function to parse a list of signals - def _process_signal_list(self, signals, prefix='s'): - if signals is None: - # No information provided; try and make it up later - return None, {} - - elif isinstance(signals, int): - # Number of signals given; make up the names - return signals, {'%s[%d]' % (prefix, i): i for i in range(signals)} - - elif isinstance(signals, str): - # Single string given => single signal with given name - return 1, {signals: 0} - - elif all(isinstance(s, str) for s in signals): - # Use the list of strings as the signal names - return len(signals), {signals[i]: i for i in range(len(signals))} - - else: - raise TypeError("Can't parse signal list %s" % str(signals)) - - # Find a signal by name - def _find_signal(self, name, sigdict): return sigdict.get(name, None) - # Update parameters used for _rhs, _out (used by subclasses) def _update_params(self, params, warning=False): if warning: @@ -508,82 +435,6 @@ def output(self, t, x, u): """ return self._out(t, x, u) - def set_inputs(self, inputs, prefix='u'): - """Set the number/names of the system inputs. - - Parameters - ---------- - inputs : int, list of str, or None - Description of the system inputs. This can be given as an integer - count or as a list of strings that name the individual signals. - If an integer count is specified, the names of the signal will be - of the form `u[i]` (where the prefix `u` can be changed using the - optional prefix parameter). - prefix : string, optional - If `inputs` is an integer, create the names of the states using - the given prefix (default = 'u'). The names of the input will be - of the form `prefix[i]`. - - """ - self.ninputs, self.input_index = \ - self._process_signal_list(inputs, prefix=prefix) - - def set_outputs(self, outputs, prefix='y'): - """Set the number/names of the system outputs. - - Parameters - ---------- - outputs : int, list of str, or None - Description of the system outputs. This can be given as an integer - count or as a list of strings that name the individual signals. - If an integer count is specified, the names of the signal will be - of the form `u[i]` (where the prefix `u` can be changed using the - optional prefix parameter). - prefix : string, optional - If `outputs` is an integer, create the names of the states using - the given prefix (default = 'y'). The names of the input will be - of the form `prefix[i]`. - - """ - self.noutputs, self.output_index = \ - self._process_signal_list(outputs, prefix=prefix) - - def set_states(self, states, prefix='x'): - """Set the number/names of the system states. - - Parameters - ---------- - states : int, list of str, or None - Description of the system states. This can be given as an integer - count or as a list of strings that name the individual signals. - If an integer count is specified, the names of the signal will be - of the form `u[i]` (where the prefix `u` can be changed using the - optional prefix parameter). - prefix : string, optional - If `states` is an integer, create the names of the states using - the given prefix (default = 'x'). The names of the input will be - of the form `prefix[i]`. - - """ - self.nstates, self.state_index = \ - self._process_signal_list(states, prefix=prefix) - - def find_input(self, name): - """Find the index for an input given its name (`None` if not found)""" - return self.input_index.get(name, None) - - def find_output(self, name): - """Find the index for an output given its name (`None` if not found)""" - return self.output_index.get(name, None) - - def find_state(self, name): - """Find the index for a state given its name (`None` if not found)""" - return self.state_index.get(name, None) - - def issiso(self): - """Check to see if a system is single input, single output""" - return self.ninputs == 1 and self.noutputs == 1 - def feedback(self, other=1, sign=-1, params={}): """Feedback interconnection between two input/output systems @@ -799,6 +650,7 @@ def __init__(self, linsys, inputs=None, outputs=None, states=None, "or transfer function object") # Look for 'input' and 'output' parameter name variants + states = _parse_signal_parameter(states, 'state', kwargs) inputs = _parse_signal_parameter(inputs, 'input', kwargs) outputs = _parse_signal_parameter(outputs, 'output', kwargs, end=True) @@ -812,15 +664,15 @@ def __init__(self, linsys, inputs=None, outputs=None, states=None, # Process input, output, state lists, if given # Make sure they match the size of the linear system - ninputs, self.input_index = self._process_signal_list( + ninputs, self.input_index = _process_signal_list( inputs if inputs is not None else linsys.ninputs, prefix='u') if ninputs is not None and linsys.ninputs != ninputs: raise ValueError("Wrong number/type of inputs given.") - noutputs, self.output_index = self._process_signal_list( + noutputs, self.output_index = _process_signal_list( outputs if outputs is not None else linsys.noutputs, prefix='y') if noutputs is not None and linsys.noutputs != noutputs: raise ValueError("Wrong number/type of outputs given.") - nstates, self.state_index = self._process_signal_list( + nstates, self.state_index = _process_signal_list( states if states is not None else linsys.nstates, prefix='x') if nstates is not None and linsys.nstates != nstates: raise ValueError("Wrong number/type of states given.") @@ -853,6 +705,10 @@ def _out(self, t, x, u): + self.D @ np.reshape(u, (-1, 1)) return np.array(y).reshape((-1,)) + def __str__(self): + return InputOutputSystem.__str__(self) + "\n\n" \ + + StateSpace.__str__(self) + class NonlinearIOSystem(InputOutputSystem): """Nonlinear I/O system. @@ -1030,7 +886,7 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], # Look for 'input' and 'output' parameter name variants inputs = _parse_signal_parameter(inputs, 'input', kwargs) - outputs = _parse_signal_parameter(outputs, 'output', kwargs, end=True) + outputs = _parse_signal_parameter(outputs, 'output', kwargs, end=True) # Convert input and output names to lists if they aren't already if not isinstance(inplist, (list, tuple)): @@ -1104,12 +960,12 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[], # If input or output list was specified, update it if inputs is not None: nsignals, self.input_index = \ - self._process_signal_list(inputs, prefix='u') + _process_signal_list(inputs, prefix='u') if nsignals is not None and len(inplist) != nsignals: raise ValueError("Wrong number/type of inputs given.") if outputs is not None: nsignals, self.output_index = \ - self._process_signal_list(outputs, prefix='y') + _process_signal_list(outputs, prefix='y') if nsignals is not None and len(outlist) != nsignals: raise ValueError("Wrong number/type of outputs given.") @@ -2261,6 +2117,184 @@ def _find_size(sysval, vecval): raise ValueError("Can't determine size of system component.") +# Define a state space object that is an I/O system +def ss(*args, **kwargs): + """ss(A, B, C, D[, dt]) + + Create a state space system. + + The function accepts either 1, 4 or 5 parameters: + + ``ss(sys)`` + Convert a linear system into space system form. Always creates a + new system, even if sys is already a state space system. + + ``ss(A, B, C, D)`` + Create a state space system from the matrices of its state and + output equations: + + .. math:: + \\dot x = A \\cdot x + B \\cdot u + + y = C \\cdot x + D \\cdot u + + ``ss(A, B, C, D, dt)`` + Create a discrete-time state space system from the matrices of + its state and output equations: + + .. math:: + x[k+1] = A \\cdot x[k] + B \\cdot u[k] + + y[k] = C \\cdot x[k] + D \\cdot u[ki] + + The matrices can be given as *array like* data types or strings. + Everything that the constructor of :class:`numpy.matrix` accepts is + permissible here too. + + Parameters + ---------- + sys : StateSpace or TransferFunction + A linear system. + A, B, C, D : array_like or string + System, control, output, and feed forward matrices. + dt : None, True or float, optional + System timebase. 0 (default) indicates continuous + time, True indicates discrete time with unspecified sampling + time, positive number is discrete time with specified + sampling time, None indicates unspecified timebase (either + continuous or discrete time). + inputs, outputs, states : str, or list of str, optional + List of strings that name the individual signals. If this parameter + is not given or given as `None`, the signal names will be of the + form `s[i]` (where `s` is one of `u`, `y`, or `x`). + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. + + Returns + ------- + out: :class:`LinearIOSystem` + Linear input/output system. + + Raises + ------ + ValueError + If matrix sizes are not self-consistent. + + See Also + -------- + tf + ss2tf + tf2ss + + Examples + -------- + >>> # Create a Linear I/O system object from from for matrices + >>> sys1 = ss([[1, -2], [3 -4]], [[5], [7]], [[6, 8]], [[9]]) + + >>> # Convert a TransferFunction to a StateSpace object. + >>> sys_tf = tf([2.], [1., 3]) + >>> sys2 = ss(sys_tf) + + """ + sys = _ss(*args, keywords=kwargs) + return LinearIOSystem(sys, **kwargs) + + +def rss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs): + """ + Create a stable *continuous* random state space object. + + Parameters + ---------- + states : int + Number of state variables + outputs : int + Number of system outputs + inputs : int + Number of system inputs + strictly_proper : bool, optional + If set to 'True', returns a proper system (no direct term). + + Returns + ------- + sys : StateSpace + The randomly created linear system + + Raises + ------ + ValueError + if any input is not a positive integer + + See Also + -------- + drss + + Notes + ----- + If the number of states, inputs, or outputs is not specified, then the + missing numbers are assumed to be 1. The poles of the returned system + will always have a negative real part. + + """ + # Process states, inputs, outputs (ignoring names) + nstates, _ = _process_signal_list(states) + ninputs, _ = _process_signal_list(inputs) + noutputs, _ = _process_signal_list(outputs) + + sys = _rss_generate( + nstates, ninputs, noutputs, 'c', strictly_proper=strictly_proper) + return LinearIOSystem( + sys, states=states, inputs=inputs, outputs=outputs, **kwargs) + + +def drss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs): + """ + Create a stable *discrete* random state space object. + + Parameters + ---------- + states : int + Number of state variables + inputs : integer + Number of system inputs + outputs : int + Number of system outputs + strictly_proper: bool, optional + If set to 'True', returns a proper system (no direct term). + + Returns + ------- + sys : StateSpace + The randomly created linear system + + Raises + ------ + ValueError + if any input is not a positive integer + + See Also + -------- + rss + + Notes + ----- + If the number of states, inputs, or outputs is not specified, then the + missing numbers are assumed to be 1. The poles of the returned system + will always have a magnitude less than 1. + + """ + # Process states, inputs, outputs (ignoring names) + nstates, _ = _process_signal_list(states) + ninputs, _ = _process_signal_list(inputs) + noutputs, _ = _process_signal_list(outputs) + + sys = _rss_generate( + nstates, ninputs, noutputs, 'd', strictly_proper=strictly_proper) + return LinearIOSystem( + sys, states=states, inputs=inputs, outputs=outputs, **kwargs) + + # Convert a state space system into an input/output system (wrapper) def ss2io(*args, **kwargs): return LinearIOSystem(*args, **kwargs) @@ -2492,9 +2526,8 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[], raise ValueError('check_unused is False, but either ' + 'ignore_inputs or ignore_outputs non-empty') - if (connections is False - and not inplist and not outlist - and not inputs and not outputs): + if connections is False and not inplist and not outlist \ + and not inputs and not outputs: # user has disabled auto-connect, and supplied neither input # nor output mappings; assume they know what they're doing check_unused = False diff --git a/control/matlab/__init__.py b/control/matlab/__init__.py index 196a4a6c8..f10a76c54 100644 --- a/control/matlab/__init__.py +++ b/control/matlab/__init__.py @@ -62,6 +62,7 @@ # Control system library from ..statesp import * +from ..iosys import ss, rss, drss # moved from .statesp from ..xferfcn import * from ..lti import * from ..frdata import * diff --git a/control/matlab/wrappers.py b/control/matlab/wrappers.py index f7cbaea41..8eafdaad2 100644 --- a/control/matlab/wrappers.py +++ b/control/matlab/wrappers.py @@ -3,7 +3,7 @@ """ import numpy as np -from ..statesp import ss +from ..iosys import ss from ..xferfcn import tf from ..ctrlutil import issys from ..exception import ControlArgument diff --git a/control/namedio.py b/control/namedio.py new file mode 100644 index 000000000..4ea82d819 --- /dev/null +++ b/control/namedio.py @@ -0,0 +1,212 @@ +# namedio.py - internal named I/O object class +# RMM, 13 Mar 2022 +# +# This file implements the _NamedIOSystem and _NamedIOStateSystem classes, +# which are used as a parent classes for FrequencyResponseData, +# InputOutputSystem, LTI, TimeResponseData, and other similar classes to +# allow naming of signals. + +import numpy as np + + +class _NamedIOSystem(object): + _idCounter = 0 + + def _name_or_default(self, name=None): + if name is None: + name = "sys[{}]".format(_NamedIOSystem._idCounter) + _NamedIOSystem._idCounter += 1 + return name + + def __init__( + self, inputs=None, outputs=None, name=None): + + # system name + self.name = self._name_or_default(name) + + # Parse and store the number of inputs and outputs + self.set_inputs(inputs) + self.set_outputs(outputs) + + # + # Class attributes + # + # These attributes are defined as class attributes so that they are + # documented properly. They are "overwritten" in __init__. + # + + #: Number of system inputs. + #: + #: :meta hide-value: + ninputs = 0 + + #: Number of system outputs. + #: + #: :meta hide-value: + noutputs = 0 + + def __repr__(self): + return str(type(self)) + ": " + self.name if self.name is not None \ + else str(type(self)) + + def __str__(self): + """String representation of an input/output object""" + str = "Object: " + (self.name if self.name else "(None)") + "\n" + str += "Inputs (%s): " % self.ninputs + for key in self.input_index: + str += key + ", " + str += "\nOutputs (%s): " % self.noutputs + for key in self.output_index: + str += key + ", " + return str + + # Find a signal by name + def _find_signal(self, name, sigdict): + return sigdict.get(name, None) + + def set_inputs(self, inputs, prefix='u'): + """Set the number/names of the system inputs. + + Parameters + ---------- + inputs : int, list of str, or None + Description of the system inputs. This can be given as an integer + count or as a list of strings that name the individual signals. + If an integer count is specified, the names of the signal will be + of the form `u[i]` (where the prefix `u` can be changed using the + optional prefix parameter). + prefix : string, optional + If `inputs` is an integer, create the names of the states using + the given prefix (default = 'u'). The names of the input will be + of the form `prefix[i]`. + + """ + self.ninputs, self.input_index = \ + _process_signal_list(inputs, prefix=prefix) + + def find_input(self, name): + """Find the index for an input given its name (`None` if not found)""" + return self.input_index.get(name, None) + + # Property for getting and setting list of input signals + input_labels = property( + lambda self: list(self.input_index.keys()), # getter + set_inputs) # setter + + def set_outputs(self, outputs, prefix='y'): + """Set the number/names of the system outputs. + + Parameters + ---------- + outputs : int, list of str, or None + Description of the system outputs. This can be given as an integer + count or as a list of strings that name the individual signals. + If an integer count is specified, the names of the signal will be + of the form `u[i]` (where the prefix `u` can be changed using the + optional prefix parameter). + prefix : string, optional + If `outputs` is an integer, create the names of the states using + the given prefix (default = 'y'). The names of the input will be + of the form `prefix[i]`. + + """ + self.noutputs, self.output_index = \ + _process_signal_list(outputs, prefix=prefix) + + def find_output(self, name): + """Find the index for an output given its name (`None` if not found)""" + return self.output_index.get(name, None) + + # Property for getting and setting list of output signals + output_labels = property( + lambda self: list(self.output_index.keys()), # getter + set_outputs) # setter + + def issiso(self): + """Check to see if a system is single input, single output""" + return self.ninputs == 1 and self.noutputs == 1 + + +class _NamedIOStateSystem(_NamedIOSystem): + def __init__( + self, inputs=None, outputs=None, states=None, name=None): + # Parse and store the system name, inputs, and outputs + super().__init__(inputs=inputs, outputs=outputs, name=name) + + # Parse and store the number of states + self.set_states(states) + + # + # Class attributes + # + # These attributes are defined as class attributes so that they are + # documented properly. They are "overwritten" in __init__. + # + + #: Number of system states. + #: + #: :meta hide-value: + nstates = 0 + + def __str__(self): + """String representation of an input/output system""" + str = _NamedIOSystem.__str__(self) + str += "\nStates (%s): " % self.nstates + for key in self.state_index: + str += key + ", " + return str + + def _isstatic(self): + """Check to see if a system is a static system (no states)""" + return self.nstates == 0 + + def set_states(self, states, prefix='x'): + """Set the number/names of the system states. + + Parameters + ---------- + states : int, list of str, or None + Description of the system states. This can be given as an integer + count or as a list of strings that name the individual signals. + If an integer count is specified, the names of the signal will be + of the form `u[i]` (where the prefix `u` can be changed using the + optional prefix parameter). + prefix : string, optional + If `states` is an integer, create the names of the states using + the given prefix (default = 'x'). The names of the input will be + of the form `prefix[i]`. + + """ + self.nstates, self.state_index = \ + _process_signal_list(states, prefix=prefix) + + def find_state(self, name): + """Find the index for a state given its name (`None` if not found)""" + return self.state_index.get(name, None) + + # Property for getting and setting list of state signals + state_labels = property( + lambda self: list(self.state_index.keys()), # getter + set_states) # setter + + +# Utility function to parse a list of signals +def _process_signal_list(signals, prefix='s'): + if signals is None: + # No information provided; try and make it up later + return None, {} + + elif isinstance(signals, (int, np.integer)): + # Number of signals given; make up the names + return signals, {'%s[%d]' % (prefix, i): i for i in range(signals)} + + elif isinstance(signals, str): + # Single string given => single signal with given name + return 1, {signals: 0} + + elif all(isinstance(s, str) for s in signals): + # Use the list of strings as the signal names + return len(signals), {signals[i]: i for i in range(len(signals))} + + else: + raise TypeError("Can't parse signal list %s" % str(signals)) diff --git a/control/sisotool.py b/control/sisotool.py index e6343c91e..b47eb7e40 100644 --- a/control/sisotool.py +++ b/control/sisotool.py @@ -5,7 +5,7 @@ from .timeresp import step_response from .lti import issiso, isdtime from .xferfcn import tf -from .statesp import ss +from .iosys import ss from .bdalg import append, connect from .iosys import tf2io, ss2io, summing_junction, interconnect from control.statesp import _convert_to_statespace, StateSpace diff --git a/control/statefbk.py b/control/statefbk.py index ef16cbfff..a866af725 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -46,6 +46,8 @@ from .mateqn import care, dare, _check_shape from .statesp import StateSpace, _ssmatrix, _convert_to_statespace from .lti import LTI, isdtime, isctime +from .iosys import InputOutputSystem, NonlinearIOSystem, LinearIOSystem, \ + interconnect, ss from .exception import ControlSlycot, ControlArgument, ControlDimension, \ ControlNotImplemented @@ -69,7 +71,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): __all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe', - 'dlqr', 'dlqe', 'acker'] + 'dlqr', 'dlqe', 'acker', 'create_statefbk_iosystem'] # Pole placement @@ -576,7 +578,7 @@ def acker(A, B, poles): return _ssmatrix(K) -def lqr(*args, **keywords): +def lqr(*args, **kwargs): """lqr(A, B, Q, R[, N]) Linear quadratic regulator design @@ -606,6 +608,13 @@ def lqr(*args, **keywords): State and input weight matrices 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. 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 @@ -644,18 +653,15 @@ def lqr(*args, **keywords): # Process the arguments and figure out what inputs we received # - # Get the method to use (if specified as a keyword) - method = keywords.get('method', None) + # If we were passed a discrete time system as the first arg, use dlqr() + if isinstance(args[0], LTI) and isdtime(args[0], strict=True): + # Call dlqr + return dlqr(*args, **kwargs) # Get the system description if (len(args) < 3): raise ControlArgument("not enough input arguments") - # If we were passed a discrete time system as the first arg, use dlqr() - if isinstance(args[0], LTI) and isdtime(args[0], strict=True): - # Call dlqr - return dlqr(*args, **keywords) - # If we were passed a state space system, use that to get system matrices if isinstance(args[0], StateSpace): A = np.array(args[0].A, ndmin=2, dtype=float) @@ -680,12 +686,47 @@ def lqr(*args, **keywords): else: N = None + # + # Process keywords + # + + # Get the method to use (if specified as a keyword) + method = kwargs.pop('method', None) + + # See if we should augment the controller with integral feedback + integral_action = kwargs.pop('integral_action', None) + if integral_action is not None: + # Figure out the size of the system + nstates = A.shape[0] + ninputs = B.shape[1] + + # Make sure that the integral action argument is the right type + if not isinstance(integral_action, np.ndarray): + raise ControlArgument("Integral action must pass an array") + elif integral_action.shape[1] != nstates: + raise ControlArgument( + "Integral gain size must match system state size") + + # Process the states to be integrated + nintegrators = integral_action.shape[0] + C = integral_action + + # Augment the system with integrators + A = np.block([ + [A, np.zeros((nstates, nintegrators))], + [C, np.zeros((nintegrators, nintegrators))] + ]) + B = np.vstack([B, np.zeros((nintegrators, ninputs))]) + + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) + # Compute the result (dimension and symmetry checking done in care()) X, L, G = care(A, B, Q, R, N, None, method=method, S_s="N") return G, X, L -def dlqr(*args, **keywords): +def dlqr(*args, **kwargs): """dlqr(A, B, Q, R[, N]) Discrete-time linear quadratic regulator design @@ -716,6 +757,17 @@ def dlqr(*args, **keywords): State and input weight matrices 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. + 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 + and then 'scipy'. Returns ------- @@ -745,9 +797,6 @@ def dlqr(*args, **keywords): # Process the arguments and figure out what inputs we received # - # Get the method to use (if specified as a keyword) - method = keywords.get('method', None) - # Get the system description if (len(args) < 3): raise ControlArgument("not enough input arguments") @@ -780,11 +829,236 @@ def dlqr(*args, **keywords): else: N = np.zeros((Q.shape[0], R.shape[1])) + # + # Process keywords + # + + # Get the method to use (if specified as a keyword) + method = kwargs.pop('method', None) + + # See if we should augment the controller with integral feedback + integral_action = kwargs.pop('integral_action', None) + if integral_action is not None: + # Figure out the size of the system + nstates = A.shape[0] + ninputs = B.shape[1] + + if not isinstance(integral_action, np.ndarray): + raise ControlArgument("Integral action must pass an array") + elif integral_action.shape[1] != nstates: + raise ControlArgument( + "Integral gain size must match system state size") + else: + nintegrators = integral_action.shape[0] + C = integral_action + + # Augment the system with integrators + A = np.block([ + [A, np.zeros((nstates, nintegrators))], + [C, np.eye(nintegrators)] + ]) + B = np.vstack([B, np.zeros((nintegrators, ninputs))]) + + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) + # Compute the result (dimension and symmetry checking done in dare()) S, E, K = dare(A, B, Q, R, N, method=method, S_s="N") return _ssmatrix(K), _ssmatrix(S), E +# Function to create an I/O sytems representing a state feedback controller +def create_statefbk_iosystem( + sys, K, integral_action=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]', + estimator=None, type='linear'): + """Create an I/O system using a (full) state feedback controller + + This function creates an input/output system that implements a + state feedback controller of the form + + u = ud - K_p (x - xd) - K_i integral(C x - C x_d) + + It can be called in the form + + ctrl, clsys = ct.create_statefbk_iosystem(sys, K) + + where ``sys`` is the process dynamics and ``K`` is the state (+ integral) + feedback gain (eg, from LQR). The function returns the controller + ``ctrl`` and the closed loop systems ``clsys``, both as I/O systems. + + Parameters + ---------- + sys : InputOutputSystem + The I/O system that represents the process dynamics. If no estimator + is given, the output of this system should represent the full state. + + K : ndarray + The state feedback gain. This matrix defines the gains to be + applied to the system. If ``integral_action`` is None, then the + dimensions of this array should be (sys.ninputs, sys.nstates). If + `integral action` is set to a matrix or a function, then additional + columns represent the gains of the integral states of the + controller. + + xd_labels, ud_labels : str or list of str, optional + Set the name of the signals to use for the desired state and inputs. + If a single string is specified, it should be a format string using + the variable ``i`` as an index. Otherwise, a list of strings matching + the size of xd and ud, respectively, should be used. Default is + ``'xd[{i}]'`` for xd_labels and ``'xd[{i}]'`` for ud_labels. + + integral_action : None, ndarray, or func, optional + If this keyword is specified, the controller can include integral + action in addition to state feedback. If ``integral_action`` is an + ndarray, 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. + + estimator : InputOutputSystem, optional + If an estimator is provided, using the states of the estimator as + the system inputs for the controller. + + type : 'nonlinear' or 'linear', optional + Set the type of controller to create. The default is a linear + controller implementing the LQR regulator. If the type is 'nonlinear', + a :class:NonlinearIOSystem is created instead, with the gain ``K`` as + a parameter (allowing modifications of the gain at runtime). + + Returns + ------- + ctrl : InputOutputSystem + Input/output system representing the controller. This system takes + as inputs the desired state xd, the desired input ud, and the system + state x. It outputs the controller action u according to the + formula u = ud - K(x - xd). If the keyword `integral_action` is + specified, then an additional set of integrators is included in the + control system (with the gain matrix K having the integral gains + appended after the state gains). + + clsys : InputOutputSystem + Input/output system representing the closed loop system. This + systems takes as inputs the desired trajectory (xd, ud) and outputs + the system state x and the applied input u (vertically stacked). + + """ + # Make sure that we were passed an I/O system as an input + if not isinstance(sys, InputOutputSystem): + raise ControlArgument("Input system must be I/O system") + + # 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: + # 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: + # 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: + raise ControlArgument( + "Integral gain size must match system state size") + else: + nintegrators = integral_action.shape[0] + C = integral_action + else: + # Create a C matrix with no outputs, just in case update gets called + C = np.zeros((0, sys.nstates)) + + # Check to make sure that state feedback has the right shape + if not isinstance(K, np.ndarray) or \ + 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'+{nintegrators}' if nintegrators > 0 else '')) + + # Figure out the labels to use + if isinstance(xd_labels, str): + # Gnerate the list of labels using the argument as a format string + xd_labels = [xd_labels.format(i=i) for i in range(sys.nstates)] + + if isinstance(ud_labels, str): + # Gnerate the list of labels using the argument as a format string + ud_labels = [ud_labels.format(i=i) for i in range(sys.ninputs)] + + # Define the controller system + if type == 'nonlinear': + # Create an I/O system for the state feedback gains + def _control_update(t, x, inputs, params): + # Split input into desired state, nominal input, and current state + xd_vec = inputs[0:sys.nstates] + x_vec = inputs[-estimator.nstates:] + + # Compute the integral error in the xy coordinates + return C @ x_vec - C @ xd_vec + + def _control_output(t, e, z, params): + K = params.get('K') + + # Split input into desired state, nominal input, and current state + xd_vec = z[0:sys.nstates] + ud_vec = z[sys.nstates:sys.nstates + sys.ninputs] + x_vec = z[-sys.nstates:] + + # Compute the control law + u = ud_vec - K[:, 0:sys.nstates] @ (x_vec - xd_vec) + if nintegrators > 0: + u -= K[:, sys.nstates:] @ e + + return u + + ctrl = NonlinearIOSystem( + _control_update, _control_output, name='control', + inputs=xd_labels + ud_labels + estimator.output_labels, + outputs=list(sys.input_index.keys()), params={'K': K}, + states=nintegrators) + + elif type == 'linear' or type is None: + # Create the matrices implementing the controller + if isctime(sys): + # Continuous time: integrator + A_lqr = np.zeros((C.shape[0], C.shape[0])) + 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:] + D_lqr = np.hstack([ + K[:, 0:sys.nstates], np.eye(sys.ninputs), -K[:, 0:sys.nstates] + ]) + + ctrl = ss( + A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name='control', + inputs=xd_labels + ud_labels + estimator.output_labels, + outputs=list(sys.input_index.keys()), states=nintegrators) + + else: + raise ControlArgument(f"unknown type '{type}'") + + # Define the closed loop system + closed = interconnect( + [sys, ctrl] if estimator == sys else [sys, ctrl, estimator], + name=sys.name + "_" + ctrl.name, + inplist=xd_labels + ud_labels, inputs=xd_labels + ud_labels, + outlist=sys.output_labels + sys.input_labels, + outputs=sys.output_labels + sys.input_labels + ) + return ctrl, closed + + def ctrb(A, B): """Controllabilty matrix diff --git a/control/statesp.py b/control/statesp.py index 0f1c560e2..36682532c 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -59,11 +59,11 @@ from scipy.signal import StateSpace as signalStateSpace from warnings import warn from .lti import LTI, common_timebase, isdtime, _process_frequency_response +from .namedio import _NamedIOStateSystem, _process_signal_list from . import config from copy import deepcopy -__all__ = ['StateSpace', 'ss', 'rss', 'drss', 'tf2ss', 'ssdata'] - +__all__ = ['StateSpace', 'tf2ss', 'ssdata'] # Define module default parameter values _statesp_defaults = { @@ -153,7 +153,7 @@ def _f2s(f): return s -class StateSpace(LTI): +class StateSpace(LTI, _NamedIOStateSystem): """StateSpace(A, B, C, D[, dt]) A class for representing state-space models. @@ -244,7 +244,7 @@ class StateSpace(LTI): # Allow ndarray * StateSpace to give StateSpace._rmul_() priority __array_priority__ = 11 # override ndarray and matrix types - def __init__(self, *args, **kwargs): + def __init__(self, *args, keywords=None, **kwargs): """StateSpace(A, B, C, D[, dt]) Construct a state space object. @@ -263,6 +263,10 @@ def __init__(self, *args, **kwargs): (default = False). """ + # Use keywords object if we received one (and pop keywords we use) + if keywords is None: + keywords = kwargs + # first get A, B, C, D matrices if len(args) == 4: # The user provided A, B, C, and D matrices. @@ -285,7 +289,7 @@ def __init__(self, *args, **kwargs): "Expected 1, 4, or 5 arguments; received %i." % len(args)) # Process keyword arguments - remove_useless_states = kwargs.get( + remove_useless_states = keywords.pop( 'remove_useless_states', config.defaults['statesp.remove_useless_states']) @@ -305,8 +309,7 @@ def __init__(self, *args, **kwargs): D = np.zeros((C.shape[0], B.shape[1])) D = _ssmatrix(D) - # TODO: use super here? - LTI.__init__(self, inputs=D.shape[1], outputs=D.shape[0]) + super().__init__(inputs=D.shape[1], outputs=D.shape[0]) self.A = A self.B = B self.C = C @@ -314,17 +317,18 @@ def __init__(self, *args, **kwargs): # now set dt if len(args) == 4: - if 'dt' in kwargs: - dt = kwargs['dt'] + if 'dt' in keywords: + dt = keywords.pop('dt') elif self._isstatic(): dt = None else: dt = config.defaults['control.default_dt'] elif len(args) == 5: dt = args[4] - if 'dt' in kwargs: + if 'dt' in keywords: warn("received multiple dt arguments, " "using positional arg dt = %s" % dt) + keywords.pop('dt') elif len(args) == 1: try: dt = args[0].dt @@ -1768,83 +1772,10 @@ def _mimo2simo(sys, input, warn_conversion=False): return sys -def ss(*args, **kwargs): - """ss(A, B, C, D[, dt]) - - Create a state space system. - - The function accepts either 1, 4 or 5 parameters: - - ``ss(sys)`` - Convert a linear system into space system form. Always creates a - new system, even if sys is already a StateSpace object. - - ``ss(A, B, C, D)`` - Create a state space system from the matrices of its state and - output equations: - - .. math:: - \\dot x = A \\cdot x + B \\cdot u - - y = C \\cdot x + D \\cdot u - - ``ss(A, B, C, D, dt)`` - Create a discrete-time state space system from the matrices of - its state and output equations: - - .. math:: - x[k+1] = A \\cdot x[k] + B \\cdot u[k] - - y[k] = C \\cdot x[k] + D \\cdot u[ki] - - The matrices can be given as *array like* data types or strings. - Everything that the constructor of :class:`numpy.matrix` accepts is - permissible here too. - - Parameters - ---------- - sys: StateSpace or TransferFunction - A linear system - A: array_like or string - System matrix - B: array_like or string - Control matrix - C: array_like or string - Output matrix - D: array_like or string - Feed forward matrix - dt: If present, specifies the timebase of the system - - Returns - ------- - out: :class:`StateSpace` - The new linear system - - Raises - ------ - ValueError - if matrix sizes are not self-consistent - - See Also - -------- - StateSpace - tf - ss2tf - tf2ss - - Examples - -------- - >>> # Create a StateSpace object from four "matrices". - >>> sys1 = ss("1. -2; 3. -4", "5.; 7", "6. 8", "9.") - - >>> # Convert a TransferFunction to a StateSpace object. - >>> sys_tf = tf([2.], [1., 3]) - >>> sys2 = ss(sys_tf) - - """ - +def _ss(*args, keywords=None, **kwargs): + """Internal function to create StateSpace system""" if len(args) == 4 or len(args) == 5: - return StateSpace(*args, **kwargs) + return StateSpace(*args, keywords=keywords, **kwargs) elif len(args) == 1: from .xferfcn import TransferFunction sys = args[0] @@ -1932,89 +1863,6 @@ def tf2ss(*args): raise ValueError("Needs 1 or 2 arguments; received %i." % len(args)) -def rss(states=1, outputs=1, inputs=1, strictly_proper=False): - """ - Create a stable *continuous* random state space object. - - Parameters - ---------- - states : int - Number of state variables - outputs : int - Number of system outputs - inputs : int - Number of system inputs - strictly_proper : bool, optional - If set to 'True', returns a proper system (no direct term). - - Returns - ------- - sys : StateSpace - The randomly created linear system - - Raises - ------ - ValueError - if any input is not a positive integer - - See Also - -------- - drss - - Notes - ----- - If the number of states, inputs, or outputs is not specified, then the - missing numbers are assumed to be 1. The poles of the returned system - will always have a negative real part. - - """ - - return _rss_generate(states, inputs, outputs, 'c', - strictly_proper=strictly_proper) - - -def drss(states=1, outputs=1, inputs=1, strictly_proper=False): - """ - Create a stable *discrete* random state space object. - - Parameters - ---------- - states : int - Number of state variables - inputs : integer - Number of system inputs - outputs : int - Number of system outputs - strictly_proper: bool, optional - If set to 'True', returns a proper system (no direct term). - - - Returns - ------- - sys : StateSpace - The randomly created linear system - - Raises - ------ - ValueError - if any input is not a positive integer - - See Also - -------- - rss - - Notes - ----- - If the number of states, inputs, or outputs is not specified, then the - missing numbers are assumed to be 1. The poles of the returned system - will always have a magnitude less than 1. - - """ - - return _rss_generate(states, inputs, outputs, 'd', - strictly_proper=strictly_proper) - - def ssdata(sys): """ Return state space data objects for a system diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index 5fd83e946..d8fcc7e56 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -1016,7 +1016,7 @@ def test_sys_naming_convention(self, tsys): ct.config.use_legacy_defaults('0.8.4') # changed delims in 0.9.0 ct.config.use_numpy_matrix(False) # np.matrix deprecated - ct.InputOutputSystem._idCounter = 0 + ct.namedio._NamedIOSystem._idCounter = 0 sys = ct.LinearIOSystem(tsys.mimo_linsys1) assert sys.name == "sys[0]" @@ -1080,7 +1080,7 @@ def test_signals_naming_convention_0_8_4(self, tsys): ct.config.use_legacy_defaults('0.8.4') # changed delims in 0.9.0 ct.config.use_numpy_matrix(False) # np.matrix deprecated - ct.InputOutputSystem._idCounter = 0 + ct.namedio._NamedIOSystem._idCounter = 0 sys = ct.LinearIOSystem(tsys.mimo_linsys1) for statename in ["x[0]", "x[1]"]: assert statename in sys.state_index diff --git a/control/tests/namedio_test.py b/control/tests/namedio_test.py new file mode 100644 index 000000000..9278136b5 --- /dev/null +++ b/control/tests/namedio_test.py @@ -0,0 +1,51 @@ +"""namedio_test.py - test named input/output object operations + +RMM, 13 Mar 2022 + +This test suite checks to make sure that named input/output class +operations are working. It doesn't do exhaustive testing of +operations on input/output objects. Separate unit tests should be +created for that purpose. +""" + +import re + +import numpy as np +import control as ct +import pytest + +def test_named_ss(): + # Create a system to play with + sys = ct.rss(2, 2, 2) + assert sys.input_labels == ['u[0]', 'u[1]'] + assert sys.output_labels == ['y[0]', 'y[1]'] + assert sys.state_labels == ['x[0]', 'x[1]'] + + # Get the state matrices for later use + A, B, C, D = sys.A, sys.B, sys.C, sys.D + + # Set up a named state space systems with default names + ct.namedio._NamedIOSystem._idCounter = 0 + sys = ct.ss(A, B, C, D) + assert sys.name == 'sys[0]' + assert sys.input_labels == ['u[0]', 'u[1]'] + assert sys.output_labels == ['y[0]', 'y[1]'] + assert sys.state_labels == ['x[0]', 'x[1]'] + + # Pass the names as arguments + sys = ct.ss( + A, B, C, D, name='system', + inputs=['u1', 'u2'], outputs=['y1', 'y2'], states=['x1', 'x2']) + assert sys.name == 'system' + assert ct.namedio._NamedIOSystem._idCounter == 1 + assert sys.input_labels == ['u1', 'u2'] + assert sys.output_labels == ['y1', 'y2'] + assert sys.state_labels == ['x1', 'x2'] + + # Do the same with rss + sys = ct.rss(['x1', 'x2', 'x3'], ['y1', 'y2'], 'u1', name='random') + assert sys.name == 'random' + assert ct.namedio._NamedIOSystem._idCounter == 1 + assert sys.input_labels == ['u1'] + assert sys.output_labels == ['y1', 'y2'] + assert sys.state_labels == ['x1', 'x2', 'x3'] diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 73410312f..10ae85a78 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -616,3 +616,272 @@ def test_lqe_discrete(self): # Calling dlqe() with a continuous time system should raise an error with pytest.raises(ControlArgument, match="called with a continuous"): K, S, E = ct.dlqe(csys, Q, R) + + @pytest.mark.parametrize( + 'nstates, noutputs, ninputs, nintegrators, type', + [(2, 0, 1, 0, None), + (2, 1, 1, 0, None), + (4, 0, 2, 0, None), + (4, 3, 2, 0, None), + (2, 0, 1, 1, None), + (4, 0, 2, 2, None), + (4, 3, 2, 2, None), + (2, 0, 1, 0, 'nonlinear'), + (4, 0, 2, 2, 'nonlinear'), + (4, 3, 2, 2, 'nonlinear'), + ]) + def test_lqr_iosys(self, nstates, ninputs, noutputs, nintegrators, type): + # Create the system to be controlled (and estimator) + # TODO: make sure it is controllable? + if noutputs == 0: + # Create a system with full state output + sys = ct.rss(nstates, nstates, ninputs, strictly_proper=True) + sys.C = np.eye(nstates) + est = None + + else: + # Create a system with of the desired size + sys = ct.rss(nstates, noutputs, ninputs, strictly_proper=True) + + # Create an estimator with different signal names + L, _, _ = ct.lqe( + sys.A, sys.B, sys.C, np.eye(ninputs), np.eye(noutputs)) + est = ss( + sys.A - L @ sys.C, np.hstack([L, sys.B]), np.eye(nstates), 0, + inputs=sys.output_labels + sys.input_labels, + outputs=[f'xhat[{i}]' for i in range(nstates)]) + + # Decide whether to include integral action + if nintegrators: + # Choose the first 'n' outputs as integral terms + C_int = np.eye(nintegrators, nstates) + + # Set up an augmented system for LQR computation + # TODO: move this computation into LQR + A_aug = np.block([ + [sys.A, np.zeros((sys.nstates, nintegrators))], + [C_int, np.zeros((nintegrators, nintegrators))] + ]) + B_aug = np.vstack([sys.B, np.zeros((nintegrators, ninputs))]) + C_aug = np.hstack([sys.C, np.zeros((sys.C.shape[0], nintegrators))]) + aug = ss(A_aug, B_aug, C_aug, 0) + else: + C_int = np.zeros((0, nstates)) + aug = sys + + # Design an LQR controller + K, _, _ = ct.lqr(aug, np.eye(nstates + nintegrators), np.eye(ninputs)) + Kp, Ki = K[:, :nstates], K[:, nstates:] + + # Create an I/O system for the controller + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, integral_action=C_int, estimator=est, type=type) + + # If we used a nonlinear controller, linearize it for testing + if type == 'nonlinear': + clsys = clsys.linearize(0, 0) + + # Make sure the linear system elements are correct + if noutputs == 0: + # No estimator + Ac = np.block([ + [sys.A - sys.B @ Kp, -sys.B @ Ki], + [C_int, np.zeros((nintegrators, nintegrators))] + ]) + Bc = np.block([ + [sys.B @ Kp, sys.B], + [-C_int, np.zeros((nintegrators, ninputs))] + ]) + Cc = np.block([ + [np.eye(nstates), np.zeros((nstates, nintegrators))], + [-Kp, -Ki] + ]) + Dc = np.block([ + [np.zeros((nstates, nstates + ninputs))], + [Kp, np.eye(ninputs)] + ]) + else: + # Estimator + Be1, Be2 = est.B[:, :noutputs], est.B[:, noutputs:] + Ac = np.block([ + [sys.A, -sys.B @ Ki, -sys.B @ Kp], + [np.zeros((nintegrators, nstates + nintegrators)), C_int], + [Be1 @ sys.C, -Be2 @ Ki, est.A - Be2 @ Kp] + ]) + Bc = np.block([ + [sys.B @ Kp, sys.B], + [-C_int, np.zeros((nintegrators, ninputs))], + [Be2 @ Kp, Be2] + ]) + Cc = np.block([ + [sys.C, np.zeros((noutputs, nintegrators + nstates))], + [np.zeros_like(Kp), -Ki, -Kp] + ]) + Dc = np.block([ + [np.zeros((noutputs, nstates + ninputs))], + [Kp, np.eye(ninputs)] + ]) + + # Check to make sure everything matches + np.testing.assert_array_almost_equal(clsys.A, Ac) + np.testing.assert_array_almost_equal(clsys.B, Bc) + np.testing.assert_array_almost_equal(clsys.C, Cc) + np.testing.assert_array_almost_equal(clsys.D, Dc) + + def test_lqr_integral_continuous(self): + # Generate a continuous time system for testing + sys = ct.rss(4, 4, 2, strictly_proper=True) + sys.C = np.eye(4) # reset output to be full state + C_int = np.eye(2, 4) # integrate outputs for first two states + nintegrators = C_int.shape[0] + + # Generate a controller with integral action + K, _, _ = ct.lqr( + sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs), + integral_action=C_int) + Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:] + + # Create an I/O system for the controller + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, integral_action=C_int) + + # Construct the state space matrices for the controller + # Controller inputs = xd, ud, x + # Controller state = z (integral of x-xd) + # Controller output = ud - Kp(x - xd) - Ki z + A_ctrl = np.zeros((nintegrators, nintegrators)) + B_ctrl = np.block([ + [-C_int, np.zeros((nintegrators, sys.ninputs)), C_int] + ]) + C_ctrl = -K[:, sys.nstates:] + D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]]) + + # Check to make sure everything matches + np.testing.assert_array_almost_equal(ctrl.A, A_ctrl) + np.testing.assert_array_almost_equal(ctrl.B, B_ctrl) + np.testing.assert_array_almost_equal(ctrl.C, C_ctrl) + np.testing.assert_array_almost_equal(ctrl.D, D_ctrl) + + # Construct the state space matrices for the closed loop system + A_clsys = np.block([ + [sys.A - sys.B @ Kp, -sys.B @ Ki], + [C_int, np.zeros((nintegrators, nintegrators))] + ]) + B_clsys = np.block([ + [sys.B @ Kp, sys.B], + [-C_int, np.zeros((nintegrators, sys.ninputs))] + ]) + C_clsys = np.block([ + [np.eye(sys.nstates), np.zeros((sys.nstates, nintegrators))], + [-Kp, -Ki] + ]) + D_clsys = np.block([ + [np.zeros((sys.nstates, sys.nstates + sys.ninputs))], + [Kp, np.eye(sys.ninputs)] + ]) + + # Check to make sure closed loop matches + np.testing.assert_array_almost_equal(clsys.A, A_clsys) + np.testing.assert_array_almost_equal(clsys.B, B_clsys) + np.testing.assert_array_almost_equal(clsys.C, C_clsys) + np.testing.assert_array_almost_equal(clsys.D, D_clsys) + + # Check the poles of the closed loop system + assert all(np.real(clsys.pole()) < 0) + + # Make sure controller infinite zero frequency gain + if slycot_check(): + ctrl_tf = tf(ctrl) + assert abs(ctrl_tf(1e-9)[0][0]) > 1e6 + assert abs(ctrl_tf(1e-9)[1][1]) > 1e6 + + def test_lqr_integral_discrete(self): + # Generate a discrete time system for testing + sys = ct.drss(4, 4, 2, strictly_proper=True) + sys.C = np.eye(4) # reset output to be full state + C_int = np.eye(2, 4) # integrate outputs for first two states + nintegrators = C_int.shape[0] + + # Generate a controller with integral action + K, _, _ = ct.lqr( + sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs), + integral_action=C_int) + Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:] + + # Create an I/O system for the controller + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, integral_action=C_int) + + # Construct the state space matrices by hand + A_ctrl = np.eye(nintegrators) + B_ctrl = np.block([ + [-C_int, np.zeros((nintegrators, sys.ninputs)), C_int] + ]) + C_ctrl = -K[:, sys.nstates:] + D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]]) + + # Check to make sure everything matches + assert ct.isdtime(clsys) + np.testing.assert_array_almost_equal(ctrl.A, A_ctrl) + np.testing.assert_array_almost_equal(ctrl.B, B_ctrl) + np.testing.assert_array_almost_equal(ctrl.C, C_ctrl) + np.testing.assert_array_almost_equal(ctrl.D, D_ctrl) + + @pytest.mark.parametrize( + "rss_fun, lqr_fun", + [(ct.rss, lqr), (ct.drss, dlqr)]) + def test_lqr_errors(self, rss_fun, lqr_fun): + # Generate a discrete time system for testing + sys = rss_fun(4, 4, 2, strictly_proper=True) + + with pytest.raises(ControlArgument, match="must pass an array"): + K, _, _ = lqr_fun( + sys, np.eye(sys.nstates), np.eye(sys.ninputs), + integral_action="invalid argument") + + with pytest.raises(ControlArgument, match="gain size must match"): + C_int = np.eye(2, 3) + K, _, _ = lqr_fun( + sys, np.eye(sys.nstates), np.eye(sys.ninputs), + integral_action=C_int) + + with pytest.raises(TypeError, match="unrecognized keywords"): + K, _, _ = lqr_fun( + sys, np.eye(sys.nstates), np.eye(sys.ninputs), + integrator=None) + + 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.raises(ControlArgument, match="must be I/O system"): + 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"): + 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"): + sys_nf = ct.rss(4, 3, 2, strictly_proper=True) + ctrl, clsys = ct.create_statefbk_iosystem(sys_nf, K) + + 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) + + # Errors involving integral action + C_int = np.eye(2, 4) + K_int, _, _ = ct.lqr( + sys, np.eye(sys.nstates + C_int.shape[0]), np.eye(sys.ninputs), + integral_action=C_int) + + with pytest.raises(ControlArgument, match="must pass an array"): + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K_int, integral_action="bad argument") + + with pytest.raises(ControlArgument, match="must be an array of size"): + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, integral_action=C_int) diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 78eacf857..be6cd9a6b 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -18,8 +18,9 @@ from control.config import defaults from control.dtime import sample_system from control.lti import evalfr -from control.statesp import (StateSpace, _convert_to_statespace, drss, - rss, ss, tf2ss, _statesp_defaults, _rss_generate) +from control.statesp import StateSpace, _convert_to_statespace, tf2ss, \ + _statesp_defaults, _rss_generate +from control.iosys import ss, rss, drss from control.tests.conftest import ismatarrayout, slycotonly from control.xferfcn import TransferFunction, ss2tf diff --git a/control/tests/type_conversion_test.py b/control/tests/type_conversion_test.py index dadcc587e..d8c2d2b71 100644 --- a/control/tests/type_conversion_test.py +++ b/control/tests/type_conversion_test.py @@ -59,7 +59,7 @@ def sys_dict(): rtype_list = ['ss', 'tf', 'frd', 'lio', 'ios', 'arr', 'flt'] conversion_table = [ # op left ss tf frd lio ios arr flt - ('add', 'ss', ['ss', 'ss', 'xrd', 'ss', 'xos', 'ss', 'ss' ]), + ('add', 'ss', ['ss', 'ss', 'xrd', 'ss', 'ios', 'ss', 'ss' ]), ('add', 'tf', ['tf', 'tf', 'xrd', 'tf', 'xos', 'tf', 'tf' ]), ('add', 'frd', ['xrd', 'xrd', 'frd', 'xrd', 'E', 'xrd', 'xrd']), ('add', 'lio', ['lio', 'lio', 'xrd', 'lio', 'ios', 'lio', 'lio']), @@ -68,7 +68,7 @@ def sys_dict(): ('add', 'flt', ['ss', 'tf', 'xrd', 'lio', 'ios', 'arr', 'flt']), # op left ss tf frd lio ios arr flt - ('sub', 'ss', ['ss', 'ss', 'xrd', 'ss', 'xos', 'ss', 'ss' ]), + ('sub', 'ss', ['ss', 'ss', 'xrd', 'ss', 'ios', 'ss', 'ss' ]), ('sub', 'tf', ['tf', 'tf', 'xrd', 'tf', 'xos', 'tf', 'tf' ]), ('sub', 'frd', ['xrd', 'xrd', 'frd', 'xrd', 'E', 'xrd', 'xrd']), ('sub', 'lio', ['lio', 'lio', 'xrd', 'lio', 'ios', 'lio', 'lio']), @@ -77,7 +77,7 @@ def sys_dict(): ('sub', 'flt', ['ss', 'tf', 'xrd', 'lio', 'ios', 'arr', 'flt']), # op left ss tf frd lio ios arr flt - ('mul', 'ss', ['ss', 'ss', 'xrd', 'ss', 'xos', 'ss', 'ss' ]), + ('mul', 'ss', ['ss', 'ss', 'xrd', 'ss', 'ios', 'ss', 'ss' ]), ('mul', 'tf', ['tf', 'tf', 'xrd', 'tf', 'xos', 'tf', 'tf' ]), ('mul', 'frd', ['xrd', 'xrd', 'frd', 'xrd', 'E', 'xrd', 'frd']), ('mul', 'lio', ['lio', 'lio', 'xrd', 'lio', 'ios', 'lio', 'lio']), diff --git a/control/tests/xferfcn_test.py b/control/tests/xferfcn_test.py index bd073e0f3..7821ce54d 100644 --- a/control/tests/xferfcn_test.py +++ b/control/tests/xferfcn_test.py @@ -8,14 +8,11 @@ import operator import control as ct -from control.statesp import StateSpace, _convert_to_statespace, rss -from control.xferfcn import TransferFunction, _convert_to_transfer_function, \ - ss2tf -from control.lti import evalfr +from control import StateSpace, TransferFunction, rss, ss2tf, evalfr +from control import isctime, isdtime, sample_system, defaults +from control.statesp import _convert_to_statespace +from control.xferfcn import _convert_to_transfer_function from control.tests.conftest import slycotonly, nopython2, matrixfilter -from control.lti import isctime, isdtime -from control.dtime import sample_system -from control.config import defaults class TestXferFcn: diff --git a/control/timeresp.py b/control/timeresp.py index 3f3eacc27..e2ce822f6 100644 --- a/control/timeresp.py +++ b/control/timeresp.py @@ -87,7 +87,7 @@ 'initial_response', 'impulse_response', 'TimeResponseData'] -class TimeResponseData(): +class TimeResponseData: """A class for returning time responses. This class maintains and manipulates the data corresponding to the diff --git a/control/xferfcn.py b/control/xferfcn.py index fd859f675..df1b6d404 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -233,7 +233,7 @@ def __init__(self, *args, **kwargs): if zeronum: den[i][j] = ones(1) - LTI.__init__(self, inputs, outputs) + super().__init__(inputs, outputs) self.num = num self.den = den @@ -243,7 +243,7 @@ def __init__(self, *args, **kwargs): if len(args) == 2: # no dt given in positional arguments if 'dt' in kwargs: - dt = kwargs['dt'] + dt = kwargs.pop('dt') elif self._isstatic(): dt = None else: @@ -253,6 +253,7 @@ def __init__(self, *args, **kwargs): if 'dt' in kwargs: warn('received multiple dt arguments, ' 'using positional arg dt=%s' % dt) + kwargs.pop('dt') elif len(args) == 1: # TODO: not sure this can ever happen since dt is always present try: