diff --git a/.github/workflows/os-blas-test-matrix.yml b/.github/workflows/os-blas-test-matrix.yml index 0e5fd25fc..0a7bb9c8f 100644 --- a/.github/workflows/os-blas-test-matrix.yml +++ b/.github/workflows/os-blas-test-matrix.yml @@ -71,14 +71,14 @@ jobs: unset | Generic | Apple ) ;; # Found in system OpenBLAS ) brew install openblas - echo "BLAS_ROOT=/usr/local/opt/openblas/" >> $GITHUB_ENV - echo "LAPACK_ROOT=/usr/local/opt/openblas/" >> $GITHUB_ENV + echo "LDFLAGS=-L/opt/homebrew/opt/openblas/lib" >> $GITHUB_ENV + echo "CPPFLAGS=-I/opt/homebrew/opt/openblas/include" >> $GITHUB_ENV ;; *) echo "bla_vendor option ${{ matrix.bla_vendor }} not supported" exit 1 ;; esac - echo "FC=gfortran-11" >> $GITHUB_ENV + echo "FC=gfortran-14" >> $GITHUB_ENV - name: Build wheel env: BLA_VENDOR: ${{ matrix.bla_vendor }} @@ -204,8 +204,6 @@ jobs: path: slycot-src - name: Checkout python-control uses: actions/checkout@v3 - with: - repository: 'python-control/python-control' - name: Setup Python uses: actions/setup-python@v4 with: diff --git a/.github/workflows/python-package-conda.yml b/.github/workflows/python-package-conda.yml index aac8ab054..a103985a6 100644 --- a/.github/workflows/python-package-conda.yml +++ b/.github/workflows/python-package-conda.yml @@ -56,6 +56,9 @@ jobs: if [[ '${{matrix.pandas}}' == 'conda' ]]; then mamba install pandas fi + if [[ '${{matrix.mplbackend}}' == 'QtAgg' ]]; then + mamba install pyqt + fi - name: Test with pytest shell: bash -l {0} diff --git a/.gitignore b/.gitignore index 4a6aa3cc0..6a262f045 100644 --- a/.gitignore +++ b/.gitignore @@ -42,3 +42,6 @@ venv/ ENV/ env.bak/ venv.bak/ + +# Files for MacOS +.DS_Store diff --git a/LICENSE b/LICENSE index 5c84d3dcd..fbfc42c67 100644 --- a/LICENSE +++ b/LICENSE @@ -1,5 +1,6 @@ Copyright (c) 2009-2016 by California Institute of Technology -Copyright (c) 2016-2023 by python-control developers +Copyright (c) 2012 by Delft University of Technology +Copyright (c) 2016-2024 by python-control developers All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/Pending b/Pending deleted file mode 100644 index a1b5bda09..000000000 --- a/Pending +++ /dev/null @@ -1,63 +0,0 @@ -List of Pending changes for control-python -RMM, 5 Sep 09 - -This file contains brief notes on features that need to be added to -the python control library. Mainly intended to keep track of "bigger -picture" things that need to be done. - ---> See src/matlab.py for a list of MATLAB functions that eventually need - to be implemented. - -OPEN BUGS - * matlab.step() doesn't handle systems with a pole at the origin (use lsim2) - * TF <-> SS transformations are buggy; see tests/convert_test.py - * hsvd returns different value than MATLAB (2010a); see modelsimp_test.py - * lsim doesn't work for StateSpace systems (signal.lsim2 bug??) - -Transfer code from Roberto Bucher's yottalab to python-control - acker - pole placement using Ackermann method - c2d - contimous to discrete time conversion - full_obs - full order observer - red_obs - reduced order observer - comp_form - state feedback controller+observer in compact form - comp_form_i - state feedback controller+observer+integ in compact form - dsimul - simulate discrete time systems - dstep - step response (plot) of discrete time systems - dimpulse - imoulse response (plot) of discrete time systems - bb_step - step response (plot) of continous time systems - sysctr - system+controller+observer+feedback - care - Solve Riccati equation for contimous time systems - dare - Solve Riccati equation for discrete time systems - dlqr - discrete linear quadratic regulator - minreal - minimal state space representation - -Transfer code from Ryan Krauss's control.py to python-control - * phase margin computations (as part of margin command) - * step reponse - * c2d, c2d_tustin (compare to Bucher version first) - -Examples and test cases - * Put together unit tests for all functions (after deciding on framework) - * Figure out how to import 'figure' command properly (version issue?) - * Figure out source of BadCoefficients warning messages (pvtol-lqr and others) - * tests/test_all.py should report on failed tests - * tests/freqresp.py needs to be converted to unit test - * Convert examples/test-{response,statefbk}.py to unit tests - -Root locus plot improvements - * Make sure that scipy.signal.lti objects still work - * Update calling syntax to be consistent with other plotting commands - -State space class fixes - * Implement pzmap for state space systems - -Basic functions to be added - * margin - compute gain and phase margin (no plot) - * lyap - solve Lyapunov equation (use SLICOT SB03MD.f) - * See http://www.slicot.org/shared/libindex.html for list of functions - ----- -Instructions for building python package - * python setup.py build - * python setup.py install - * python setup.py sdist diff --git a/control/__init__.py b/control/__init__.py index 45f2a56d6..40f3a783b 100644 --- a/control/__init__.py +++ b/control/__init__.py @@ -83,6 +83,7 @@ from .timeplot import * from .bdalg import * +from .ctrlplot import * from .delay import * from .descfcn import * from .dtime import * diff --git a/control/bdalg.py b/control/bdalg.py index 6ab9cd9ca..024d95fba 100644 --- a/control/bdalg.py +++ b/control/bdalg.py @@ -54,24 +54,26 @@ """ from functools import reduce -import numpy as np from warnings import warn -from . import xferfcn as tf -from . import statesp as ss + +import numpy as np + from . import frdata as frd +from . import statesp as ss +from . import xferfcn as tf from .iosys import InputOutputSystem __all__ = ['series', 'parallel', 'negate', 'feedback', 'append', 'connect'] -def series(sys1, *sysn): +def series(sys1, *sysn, **kwargs): r"""series(sys1, sys2, [..., sysn]) Return the series connection (`sysn` \* ...\ \*) `sys2` \* `sys1`. Parameters ---------- - sys1, sys2, ..., sysn: scalar, array, or :class:`InputOutputSystem` + sys1, sys2, ..., sysn : scalar, array, or :class:`InputOutputSystem` I/O systems to combine. Returns @@ -79,6 +81,20 @@ def series(sys1, *sysn): out : scalar, array, or :class:`InputOutputSystem` Series interconnection of the systems. + Other Parameters + ---------------- + inputs, outputs : str, or list of str, optional + List of strings that name the individual signals. If not given, + signal names will be of the form `s[i]` (where `s` is one of `u`, + or `y`). See :class:`InputOutputSystem` for more information. + states : str, or list of str, optional + List of names for system states. If not given, state names will be + of of the form `x[i]` for interconnections of linear systems or + '.' for interconnected nonlinear systems. + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. + Raises ------ ValueError @@ -117,17 +133,19 @@ def series(sys1, *sysn): (2, 1, 5) """ - return reduce(lambda x, y: y * x, sysn, sys1) + sys = reduce(lambda x, y: y * x, sysn, sys1) + sys.update_names(**kwargs) + return sys -def parallel(sys1, *sysn): +def parallel(sys1, *sysn, **kwargs): r"""parallel(sys1, sys2, [..., sysn]) Return the parallel connection `sys1` + `sys2` (+ ...\ + `sysn`). Parameters ---------- - sys1, sys2, ..., sysn: scalar, array, or :class:`InputOutputSystem` + sys1, sys2, ..., sysn : scalar, array, or :class:`InputOutputSystem` I/O systems to combine. Returns @@ -135,6 +153,20 @@ def parallel(sys1, *sysn): out : scalar, array, or :class:`InputOutputSystem` Parallel interconnection of the systems. + Other Parameters + ---------------- + inputs, outputs : str, or list of str, optional + List of strings that name the individual signals. If not given, + signal names will be of the form `s[i]` (where `s` is one of `u`, + or `y`). See :class:`InputOutputSystem` for more information. + states : str, or list of str, optional + List of names for system states. If not given, state names will be + of of the form `x[i]` for interconnections of linear systems or + '.' for interconnected nonlinear systems. + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. + Raises ------ ValueError @@ -171,16 +203,17 @@ def parallel(sys1, *sysn): (3, 4, 7) """ - return reduce(lambda x, y: x + y, sysn, sys1) - + sys = reduce(lambda x, y: x + y, sysn, sys1) + sys.update_names(**kwargs) + return sys -def negate(sys): +def negate(sys, **kwargs): """ Return the negative of a system. Parameters ---------- - sys: scalar, array, or :class:`InputOutputSystem` + sys : scalar, array, or :class:`InputOutputSystem` I/O systems to negate. Returns @@ -188,38 +221,53 @@ def negate(sys): out : scalar, array, or :class:`InputOutputSystem` Negated system. - Notes - ----- - This function is a wrapper for the __neg__ function in the StateSpace and - TransferFunction classes. The output type is the same as the input type. + Other Parameters + ---------------- + inputs, outputs : str, or list of str, optional + List of strings that name the individual signals. If not given, + signal names will be of the form `s[i]` (where `s` is one of `u`, + or `y`). See :class:`InputOutputSystem` for more information. + states : str, or list of str, optional + List of names for system states. If not given, state names will be + of of the form `x[i]` for interconnections of linear systems or + '.' for interconnected nonlinear systems. + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. See Also -------- append, feedback, interconnect, parallel, series + Notes + ----- + This function is a wrapper for the __neg__ function in the StateSpace and + TransferFunction classes. The output type is the same as the input type. + Examples -------- >>> G = ct.tf([2], [1, 1]) >>> G.dcgain() - 2.0 + np.float64(2.0) >>> Gn = ct.negate(G) # Same as sys2 = -sys1. >>> Gn.dcgain() - -2.0 + np.float64(-2.0) """ - return -sys + sys = -sys + sys.update_names(**kwargs) + return sys #! TODO: expand to allow sys2 default to work in MIMO case? -#! TODO: allow renaming of signals (for all bdalg operations) -def feedback(sys1, sys2=1, sign=-1): +def feedback(sys1, sys2=1, sign=-1, **kwargs): """Feedback interconnection between two I/O systems. Parameters ---------- - sys1, sys2: scalar, array, or :class:`InputOutputSystem` + sys1, sys2 : scalar, array, or :class:`InputOutputSystem` I/O systems to combine. - sign: scalar + sign : scalar The sign of feedback. `sign` = -1 indicates negative feedback, and `sign` = 1 indicates positive feedback. `sign` is an optional argument; it assumes a value of -1 if not specified. @@ -229,6 +277,20 @@ def feedback(sys1, sys2=1, sign=-1): out : scalar, array, or :class:`InputOutputSystem` Feedback interconnection of the systems. + Other Parameters + ---------------- + inputs, outputs : str, or list of str, optional + List of strings that name the individual signals. If not given, + signal names will be of the form `s[i]` (where `s` is one of `u`, + or `y`). See :class:`InputOutputSystem` for more information. + states : str, or list of str, optional + List of names for system states. If not given, state names will be + of of the form `x[i]` for interconnections of linear systems or + '.' for interconnected nonlinear systems. + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. + Raises ------ ValueError @@ -261,7 +323,7 @@ def feedback(sys1, sys2=1, sign=-1): # Allow anything with a feedback function to call that function # TODO: rewrite to allow __rfeedback__ try: - return sys1.feedback(sys2, sign) + return sys1.feedback(sys2, sign, **kwargs) except (AttributeError, TypeError): pass @@ -279,14 +341,16 @@ def feedback(sys1, sys2=1, sign=-1): if isinstance(sys2, (int, float, complex, np.number, np.ndarray, tf.TransferFunction)): sys1 = tf._convert_to_transfer_function(sys1) - elif isinstance(sys2, frd.FRD): - sys1 = frd._convert_to_FRD(sys1, sys2.omega) + elif isinstance(sys2, frd.FrequencyResponseData): + sys1 = frd._convert_to_frd(sys1, sys2.omega) else: sys1 = ss._convert_to_statespace(sys1) - return sys1.feedback(sys2, sign) + sys = sys1.feedback(sys2, sign) + sys.update_names(**kwargs) + return sys -def append(*sys): +def append(*sys, **kwargs): """append(sys1, sys2, [..., sysn]) Group LTI state space models by appending their inputs and outputs. @@ -299,6 +363,20 @@ def append(*sys): sys1, sys2, ..., sysn: scalar, array, or :class:`StateSpace` I/O systems to combine. + Other Parameters + ---------------- + inputs, outputs : str, or list of str, optional + List of strings that name the individual signals. If not given, + signal names will be of the form `s[i]` (where `s` is one of `u`, + or `y`). See :class:`InputOutputSystem` for more information. + states : str, or list of str, optional + List of names for system states. If not given, state names will be + of of the form `x[i]` for interconnections of linear systems or + '.' for interconnected nonlinear systems. + name : string, optional + System name (used for specifying signals). If unspecified, a generic + name is generated with a unique integer id. + Returns ------- out: :class:`StateSpace` @@ -327,14 +405,15 @@ def append(*sys): s1 = ss._convert_to_statespace(sys[0]) for s in sys[1:]: s1 = s1.append(s) + s1.update_names(**kwargs) return s1 def connect(sys, Q, inputv, outputv): """Index-based interconnection of an LTI system. .. deprecated:: 0.10.0 - `connect` will be removed in a future version of python-control in - favor of `interconnect`, which works with named signals. + `connect` will be removed in a future version of python-control. + Use :func:`interconnect` instead, which works with named signals. The system `sys` is a system typically constructed with `append`, with multiple inputs and outputs. The inputs and outputs are connected @@ -370,6 +449,12 @@ def connect(sys, Q, inputv, outputv): -------- append, feedback, interconnect, negate, parallel, series + Notes + ----- + The :func:`~control.interconnect` function in the :ref:`input/output + systems ` module allows the use of named signals and + provides an alternative method for interconnecting multiple systems. + Examples -------- >>> G = ct.rss(7, inputs=2, outputs=2) @@ -378,15 +463,9 @@ def connect(sys, Q, inputv, outputv): >>> T.ninputs, T.noutputs, T.nstates (1, 2, 7) - Notes - ----- - The :func:`~control.interconnect` function in the :ref:`input/output - systems ` module allows the use of named signals and - provides an alternative method for interconnecting multiple systems. - """ # TODO: maintain `connect` for use in MATLAB submodule (?) - warn("`connect` is deprecated; use `interconnect`", DeprecationWarning) + warn("connect() is deprecated; use interconnect()", FutureWarning) inputv, outputv, Q = \ np.atleast_1d(inputv), np.atleast_1d(outputv), np.atleast_1d(Q) diff --git a/control/canonical.py b/control/canonical.py index 7d091b22f..a62044322 100644 --- a/control/canonical.py +++ b/control/canonical.py @@ -204,7 +204,7 @@ def similarity_transform(xsys, T, timescale=1, inverse=False): The matrix `T` defines the new set of coordinates z = T x. timescale : float, optional If present, also rescale the time unit to tau = timescale * t - inverse: boolean, optional + inverse : bool, optional If True (default), transform so z = T x. If False, transform so x = T z. @@ -397,21 +397,21 @@ def bdschur(a, condmax=None, sort=None): Parameters ---------- - a : (M, M) array_like - Real matrix to decompose - condmax : None or float, optional - If None (default), use 1/sqrt(eps), which is approximately 1e8 - sort : {None, 'continuous', 'discrete'} - Block sorting; see below. + a : (M, M) array_like + Real matrix to decompose + condmax : None or float, optional + If None (default), use 1/sqrt(eps), which is approximately 1e8 + sort : {None, 'continuous', 'discrete'} + Block sorting; see below. Returns ------- - amodal : (M, M) real ndarray - Block-diagonal Schur decomposition of `a` - tmodal : (M, M) real ndarray - Similarity transform relating `a` and `amodal` - blksizes : (N,) int ndarray - Array of Schur block sizes + amodal : (M, M) real ndarray + Block-diagonal Schur decomposition of `a` + tmodal : (M, M) real ndarray + Similarity transform relating `a` and `amodal` + blksizes : (N,) int ndarray + Array of Schur block sizes Notes ----- diff --git a/control/config.py b/control/config.py index b6d5385d4..c5a59250b 100644 --- a/control/config.py +++ b/control/config.py @@ -10,6 +10,7 @@ import collections import warnings + from .exception import ControlArgument __all__ = ['defaults', 'set_defaults', 'reset_defaults', @@ -82,6 +83,13 @@ def set_defaults(module, **keywords): The set_defaults() function can be used to modify multiple parameter values for a module at the same time, using keyword arguments. + Parameters + ---------- + module : str + Name of the module for which the defaults are being given. + **keywords : keyword arguments + Parameter value assignments. + Examples -------- >>> ct.defaults['freqplot.number_of_samples'] @@ -121,6 +129,10 @@ def reset_defaults(): # System level defaults defaults.update(_control_defaults) + from .ctrlplot import _ctrlplot_defaults, reset_rcParams + reset_rcParams() + defaults.update(_ctrlplot_defaults) + from .freqplot import _freqplot_defaults, _nyquist_defaults defaults.update(_freqplot_defaults) defaults.update(_nyquist_defaults) @@ -350,7 +362,7 @@ def _process_legacy_keyword(kwargs, oldkey, newkey, newval): if kwargs.get(oldkey) is not None: warnings.warn( f"keyword '{oldkey}' is deprecated; use '{newkey}'", - DeprecationWarning) + FutureWarning) if newval is not None: raise ControlArgument( f"duplicate keywords '{oldkey}' and '{newkey}'") diff --git a/control/ctrlplot.py b/control/ctrlplot.py new file mode 100644 index 000000000..bb57f19fd --- /dev/null +++ b/control/ctrlplot.py @@ -0,0 +1,766 @@ +# ctrlplot.py - utility functions for plotting +# Richard M. Murray, 14 Jun 2024 +# +# Collection of functions that are used by various plotting functions. + +# Code pattern for control system plotting functions: +# +# def name_plot(sysdata, *fmt, plot=None, **kwargs): +# # Process keywords and set defaults +# ax = kwargs.pop('ax', None) +# color = kwargs.pop('color', None) +# label = kwargs.pop('label', None) +# rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) +# +# # Make sure all keyword arguments were processed (if not checked later) +# if kwargs: +# raise TypeError("unrecognized keywords: ", str(kwargs)) +# +# # Process the data (including generating responses for systems) +# sysdata = list(sysdata) +# if any([isinstance(sys, InputOutputSystem) for sys in sysdata]): +# data = name_response(sysdata) +# nrows = max([data.noutputs for data in sysdata]) +# ncols = max([data.ninputs for data in sysdata]) +# +# # Legacy processing of plot keyword +# if plot is False: +# return data.x, data.y +# +# # Figure out the shape of the plot and find/create axes +# fig, ax_array = _process_ax_keyword(ax, (nrows, ncols), rcParams) +# legend_loc, legend_map, show_legend = _process_legend_keywords( +# kwargs, (nrows, ncols), 'center right') +# +# # Customize axes (curvilinear grids, shared axes, etc) +# +# # Plot the data +# lines = np.full(ax_array.shape, []) +# line_labels = _process_line_labels(label, ntraces, nrows, ncols) +# color_offset, color_cycle = _get_color_offset(ax) +# for i, j in itertools.product(range(nrows), range(ncols)): +# ax = ax_array[i, j] +# for k in range(ntraces): +# if color is None: +# color = _get_color( +# color, fmt=fmt, offset=k, color_cycle=color_cycle) +# label = line_labels[k, i, j] +# lines[i, j] += ax.plot(data.x, data.y, color=color, label=label) +# +# # Customize and label the axes +# for i, j in itertools.product(range(nrows), range(ncols)): +# ax_array[i, j].set_xlabel("x label") +# ax_array[i, j].set_ylabel("y label") +# +# # Create legends +# if show_legend != False: +# legend_array = np.full(ax_array.shape, None, dtype=object) +# for i, j in itertools.product(range(nrows), range(ncols)): +# if legend_map[i, j] is not None: +# lines = ax_array[i, j].get_lines() +# labels = _make_legend_labels(lines) +# if len(labels) > 1: +# legend_array[i, j] = ax.legend( +# lines, labels, loc=legend_map[i, j]) +# else: +# legend_array = None +# +# # Update the plot title (only if ax was not given) +# sysnames = [response.sysname for response in data] +# if ax is None and title is None: +# title = "Name plot for " + ", ".join(sysnames) +# _update_plot_title(title, fig, rcParams=rcParams) +# elif ax == None: +# _update_plot_title(title, fig, rcParams=rcParams, use_existing=False) +# +# # Legacy processing of plot keyword +# if plot is True: +# return data +# +# return ControlPlot(lines, ax_array, fig, legend=legend_map) + +import itertools +import warnings +from os.path import commonprefix + +import matplotlib as mpl +import matplotlib.pyplot as plt +import numpy as np + +from . import config + +__all__ = [ + 'ControlPlot', 'suptitle', 'get_plot_axes', 'pole_zero_subplots', + 'rcParams', 'reset_rcParams'] + +# +# Style parameters +# + +rcParams_default = { + 'axes.labelsize': 'small', + 'axes.titlesize': 'small', + 'figure.titlesize': 'medium', + 'legend.fontsize': 'x-small', + 'xtick.labelsize': 'small', + 'ytick.labelsize': 'small', +} +_ctrlplot_rcParams = rcParams_default.copy() # provide access inside module +rcParams = _ctrlplot_rcParams # provide access outside module + +_ctrlplot_defaults = {'ctrlplot.rcParams': _ctrlplot_rcParams} + + +# +# Control figure +# + +class ControlPlot(object): + """A class for returning control figures. + + This class is used as the return type for control plotting functions. + It contains the information required to access portions of the plot + that the user might want to adjust, as well as providing methods to + modify some of the properties of the plot. + + A control figure consists of a :class:`matplotlib.figure.Figure` with + an array of :class:`matplotlib.axes.Axes`. Each axes in the figure has + a number of lines that represent the data for the plot. There may also + be a legend present in one or more of the axes. + + Attributes + ---------- + lines : array of list of :class:`matplotlib:Line2D` + Array of Line2D objects for each line in the plot. Generally, the + shape of the array matches the subplots shape and the value of the + array is a list of Line2D objects in that subplot. Some plotting + functions will return variants of this structure, as described in + the individual documentation for the functions. + axes : 2D array of :class:`matplotlib:Axes` + Array of Axes objects for each subplot in the plot. + figure : :class:`matplotlib:Figure` + Figure on which the Axes are drawn. + legend : :class:`matplotlib:.legend.Legend` (instance or ndarray) + Legend object(s) for the plot. If more than one legend is + included, this will be an array with each entry being either None + (for no legend) or a legend object. + + """ + def __init__(self, lines, axes=None, figure=None, legend=None): + self.lines = lines + if axes is None: + _get_axes = np.vectorize(lambda lines: lines[0].axes) + axes = _get_axes(lines) + self.axes = np.atleast_2d(axes) + if figure is None: + figure = self.axes[0, 0].figure + self.figure = figure + self.legend = legend + + # Implement methods and properties to allow legacy interface (np.array) + __iter__ = lambda self: self.lines + __len__ = lambda self: len(self.lines) + def __getitem__(self, item): + warnings.warn( + "return of Line2D objects from plot function is deprecated in " + "favor of ControlPlot; use out.lines to access Line2D objects", + category=FutureWarning) + return self.lines[item] + def __setitem__(self, item, val): + self.lines[item] = val + shape = property(lambda self: self.lines.shape, None) + def reshape(self, *args): + return self.lines.reshape(*args) + + def set_plot_title(self, title, frame='axes'): + """Set the title for a control plot. + + This is a wrapper for the matplotlib `suptitle` function, but by + setting ``frame`` to 'axes' (default) then the title is centered on + the midpoint of the axes in the figure, rather than the center of + the figure. This usually looks better (particularly with + multi-panel plots), though it takes longer to render. + + Parameters + ---------- + title : str + Title text. + fig : Figure, optional + Matplotlib figure. Defaults to current figure. + frame : str, optional + Coordinate frame to use for centering: 'axes' (default) or 'figure'. + **kwargs : :func:`matplotlib.pyplot.suptitle` keywords, optional + Additional keywords (passed to matplotlib). + + """ + _update_plot_title( + title, fig=self.figure, frame=frame, use_existing=False) + +# +# User functions +# +# The functions below can be used by users to modify control plots or get +# information about them. +# + +def suptitle( + title, fig=None, frame='axes', **kwargs): + """Add a centered title to a figure. + + .. deprecated:: 0.10.1 + Use :func:`ControlPlot.set_plot_title`. + + """ + warnings.warn( + "suptitle() is deprecated; use cplt.set_plot_title()", FutureWarning) + _update_plot_title( + title, fig=fig, frame=frame, use_existing=False, **kwargs) + + +# Create vectorized function to find axes from lines +def get_plot_axes(line_array): + """Get a list of axes from an array of lines. + + .. deprecated:: 0.10.1 + This function will be removed in a future version of python-control. + Use `cplt.axes` to obtain axes for an instance of :class:`ControlPlot`. + + This function can be used to return the set of axes corresponding + to the line array that is returned by `time_response_plot`. This + is useful for generating an axes array that can be passed to + subsequent plotting calls. + + Parameters + ---------- + line_array : array of list of Line2D + A 2D array with elements corresponding to a list of lines appearing + in an axes, matching the return type of a time response data plot. + + Returns + ------- + axes_array : array of list of Axes + A 2D array with elements corresponding to the Axes associated with + the lines in `line_array`. + + Notes + ----- + Only the first element of each array entry is used to determine the axes. + + """ + warnings.warn( + "get_plot_axes() is deprecated; use cplt.axes()", FutureWarning) + _get_axes = np.vectorize(lambda lines: lines[0].axes) + if isinstance(line_array, ControlPlot): + return _get_axes(line_array.lines) + else: + return _get_axes(line_array) + + +def pole_zero_subplots( + nrows, ncols, grid=None, dt=None, fig=None, scaling=None, + rcParams=None): + """Create axes for pole/zero plot. + + Parameters + ---------- + nrows, ncols : int + Number of rows and columns. + grid : True, False, or 'empty', optional + Grid style to use. Can also be a list, in which case each subplot + will have a different style (columns then rows). + dt : timebase, option + Timebase for each subplot (or a list of timebases). + scaling : 'auto', 'equal', or None + Scaling to apply to the subplots. + fig : :class:`matplotlib.figure.Figure` + Figure to use for creating subplots. + rcParams : dict + Override the default parameters used for generating plots. + Default is set up config.default['ctrlplot.rcParams']. + + Returns + ------- + ax_array : array + 2D array of axes + + """ + from .grid import nogrid, sgrid, zgrid + from .iosys import isctime + + if fig is None: + fig = plt.gcf() + rcParams = config._get_param('ctrlplot', 'rcParams', rcParams) + + if not isinstance(grid, list): + grid = [grid] * nrows * ncols + if not isinstance(dt, list): + dt = [dt] * nrows * ncols + + ax_array = np.full((nrows, ncols), None) + index = 0 + with plt.rc_context(rcParams): + for row, col in itertools.product(range(nrows), range(ncols)): + match grid[index], isctime(dt=dt[index]): + case 'empty', _: # empty grid + ax_array[row, col] = fig.add_subplot(nrows, ncols, index+1) + + case True, True: # continuous time grid + ax_array[row, col], _ = sgrid( + (nrows, ncols, index+1), scaling=scaling) + + case True, False: # discrete time grid + ax_array[row, col] = fig.add_subplot(nrows, ncols, index+1) + zgrid(ax=ax_array[row, col], scaling=scaling) + + case False | None, _: # no grid (just stability boundaries) + ax_array[row, col] = fig.add_subplot(nrows, ncols, index+1) + nogrid( + ax=ax_array[row, col], dt=dt[index], scaling=scaling) + index += 1 + return ax_array + + +def reset_rcParams(): + """Reset rcParams to default values for control plots.""" + _ctrlplot_rcParams.update(rcParams_default) + + +# +# Utility functions +# +# These functions are used by plotting routines to provide a consistent way +# of processing and displaying information. +# + +def _process_ax_keyword( + axs, shape=(1, 1), rcParams=None, squeeze=False, clear_text=False, + create_axes=True, sharex=False, sharey=False): + """Process ax keyword to plotting commands. + + This function processes the `ax` keyword to plotting commands. If no + ax keyword is passed, the current figure is checked to see if it has + the correct shape. If the shape matches the desired shape, then the + current figure and axes are returned. Otherwise a new figure is + created with axes of the desired shape. + + If `create_axes` is False and a new/empty figure is returned, then axs + is an array of the proper shape but None for each element. This allows + the calling function to do the actual axis creation (needed for + curvilinear grids that use the AxisArtist module). + + Legacy behavior: some of the older plotting commands use a axes label + to identify the proper axes for plotting. This behavior is supported + through the use of the label keyword, but will only work if shape == + (1, 1) and squeeze == True. + + """ + if axs is None: + fig = plt.gcf() # get current figure (or create new one) + axs = fig.get_axes() + + # Check to see if axes are the right shape; if not, create new figure + # Note: can't actually check the shape, just the total number of axes + if len(axs) != np.prod(shape): + with plt.rc_context(rcParams): + if len(axs) != 0 and create_axes: + # Create a new figure + fig, axs = plt.subplots( + *shape, sharex=sharex, sharey=sharey, squeeze=False) + elif create_axes: + # Create new axes on (empty) figure + axs = fig.subplots( + *shape, sharex=sharex, sharey=sharey, squeeze=False) + else: + # Create an empty array and let user create axes + axs = np.full(shape, None) + if create_axes: # if not creating axes, leave these to caller + fig.set_layout_engine('tight') + fig.align_labels() + + else: + # Use the existing axes, properly reshaped + axs = np.asarray(axs).reshape(*shape) + + if clear_text: + # Clear out any old text from the current figure + for text in fig.texts: + text.set_visible(False) # turn off the text + del text # get rid of it completely + else: + axs = np.atleast_1d(axs) + try: + axs = axs.reshape(shape) + except ValueError: + raise ValueError( + "specified axes are not the right shape; " + f"got {axs.shape} but expecting {shape}") + fig = axs[0, 0].figure + + # Process the squeeze keyword + if squeeze and shape == (1, 1): + axs = axs[0, 0] # Just return the single axes object + elif squeeze: + axs = axs.squeeze() + + return fig, axs + + +# Turn label keyword into array indexed by trace, output, input +# TODO: move to ctrlutil.py and update parameter names to reflect general use +def _process_line_labels(label, ntraces=1, ninputs=0, noutputs=0): + if label is None: + return None + + if isinstance(label, str): + label = [label] * ntraces # single label for all traces + + # Convert to an ndarray, if not done already + try: + line_labels = np.asarray(label) + except ValueError: + raise ValueError("label must be a string or array_like") + + # Turn the data into a 3D array of appropriate shape + # TODO: allow more sophisticated broadcasting (and error checking) + try: + if ninputs > 0 and noutputs > 0: + if line_labels.ndim == 1 and line_labels.size == ntraces: + line_labels = line_labels.reshape(ntraces, 1, 1) + line_labels = np.broadcast_to( + line_labels, (ntraces, ninputs, noutputs)) + else: + line_labels = line_labels.reshape(ntraces, ninputs, noutputs) + except ValueError: + if line_labels.shape[0] != ntraces: + raise ValueError("number of labels must match number of traces") + else: + raise ValueError("labels must be given for each input/output pair") + + return line_labels + + +# Get labels for all lines in an axes +def _get_line_labels(ax, use_color=True): + labels_colors, lines = [], [] + last_color, counter = None, 0 # label unknown systems + for i, line in enumerate(ax.get_lines()): + label = line.get_label() + color = line.get_color() + if use_color and label.startswith("Unknown"): + label = f"Unknown-{counter}" + if last_color != color: + counter += 1 + last_color = color + elif label[0] == '_': + continue + + if (label, color) not in labels_colors: + lines.append(line) + labels_colors.append((label, color)) + + return lines, [label for label, color in labels_colors] + + +def _process_legend_keywords( + kwargs, shape=None, default_loc='center right'): + legend_loc = kwargs.pop('legend_loc', None) + if shape is None and 'legend_map' in kwargs: + raise TypeError("unexpected keyword argument 'legend_map'") + else: + legend_map = kwargs.pop('legend_map', None) + show_legend = kwargs.pop('show_legend', None) + + # If legend_loc or legend_map were given, always show the legend + if legend_loc is False or legend_map is False: + if show_legend is True: + warnings.warn( + "show_legend ignored; legend_loc or legend_map was given") + show_legend = False + legend_loc = legend_map = None + elif legend_loc is not None or legend_map is not None: + if show_legend is False: + warnings.warn( + "show_legend ignored; legend_loc or legend_map was given") + show_legend = True + + if legend_loc is None: + legend_loc = default_loc + elif not isinstance(legend_loc, (int, str)): + raise ValueError("legend_loc must be string or int") + + # Make sure the legend map is the right size + if legend_map is not None: + legend_map = np.atleast_2d(legend_map) + if legend_map.shape != shape: + raise ValueError("legend_map shape just match axes shape") + + return legend_loc, legend_map, show_legend + + +# Utility function to make legend labels +def _make_legend_labels(labels, ignore_common=False): + if len(labels) == 1: + return labels + + # Look for a common prefix (up to a space) + common_prefix = commonprefix(labels) + last_space = common_prefix.rfind(', ') + if last_space < 0 or ignore_common: + common_prefix = '' + elif last_space > 0: + common_prefix = common_prefix[:last_space + 2] + prefix_len = len(common_prefix) + + # Look for a common suffix (up to a space) + common_suffix = commonprefix( + [label[::-1] for label in labels])[::-1] + suffix_len = len(common_suffix) + # Only chop things off after a comma or space + while suffix_len > 0 and common_suffix[-suffix_len] != ',': + suffix_len -= 1 + + # Strip the labels of common information + if suffix_len > 0 and not ignore_common: + labels = [label[prefix_len:-suffix_len] for label in labels] + else: + labels = [label[prefix_len:] for label in labels] + + return labels + + +def _update_plot_title( + title, fig=None, frame='axes', use_existing=True, **kwargs): + if title is False or title is None: + return + if fig is None: + fig = plt.gcf() + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + + if use_existing: + # Get the current title, if it exists + old_title = None if fig._suptitle is None else fig._suptitle._text + + if old_title is not None: + # Find the common part of the titles + common_prefix = commonprefix([old_title, title]) + + # Back up to the last space + last_space = common_prefix.rfind(' ') + if last_space > 0: + common_prefix = common_prefix[:last_space] + common_len = len(common_prefix) + + # Add the new part of the title (usually the system name) + if old_title[common_len:] != title[common_len:]: + separator = ',' if len(common_prefix) > 0 else ';' + title = old_title + separator + title[common_len:] + + if frame == 'figure': + with plt.rc_context(rcParams): + fig.suptitle(title, **kwargs) + + elif frame == 'axes': + with plt.rc_context(rcParams): + fig.suptitle(title, **kwargs) # Place title in center + plt.tight_layout() # Put everything into place + xc, _ = _find_axes_center(fig, fig.get_axes()) + fig.suptitle(title, x=xc, **kwargs) # Redraw title, centered + + else: + raise ValueError(f"unknown frame '{frame}'") + + +def _find_axes_center(fig, axs): + """Find the midpoint between axes in display coordinates. + + This function finds the middle of a plot as defined by a set of axes. + + """ + inv_transform = fig.transFigure.inverted() + xlim = ylim = [1, 0] + for ax in axs: + ll = inv_transform.transform(ax.transAxes.transform((0, 0))) + ur = inv_transform.transform(ax.transAxes.transform((1, 1))) + + xlim = [min(ll[0], xlim[0]), max(ur[0], xlim[1])] + ylim = [min(ll[1], ylim[0]), max(ur[1], ylim[1])] + + return (np.sum(xlim)/2, np.sum(ylim)/2) + + +# Internal function to add arrows to a curve +def _add_arrows_to_line2D( + axes, line, arrow_locs=[0.2, 0.4, 0.6, 0.8], + arrowstyle='-|>', arrowsize=1, dir=1): + """ + Add arrows to a matplotlib.lines.Line2D at selected locations. + + Parameters + ---------- + axes: Axes object as returned by axes command (or gca) + line: Line2D object as returned by plot command + arrow_locs: list of locations where to insert arrows, % of total length + arrowstyle: style of the arrow + arrowsize: size of the arrow + + Returns + ------- + arrows: list of arrows + + Based on https://stackoverflow.com/questions/26911898/ + + """ + # Get the coordinates of the line, in plot coordinates + if not isinstance(line, mpl.lines.Line2D): + raise ValueError("expected a matplotlib.lines.Line2D object") + x, y = line.get_xdata(), line.get_ydata() + + # Determine the arrow properties + arrow_kw = {"arrowstyle": arrowstyle} + + color = line.get_color() + use_multicolor_lines = isinstance(color, np.ndarray) + if use_multicolor_lines: + raise NotImplementedError("multicolor lines not supported") + else: + arrow_kw['color'] = color + + linewidth = line.get_linewidth() + if isinstance(linewidth, np.ndarray): + raise NotImplementedError("multiwidth lines not supported") + else: + arrow_kw['linewidth'] = linewidth + + # Figure out the size of the axes (length of diagonal) + xlim, ylim = axes.get_xlim(), axes.get_ylim() + ul, lr = np.array([xlim[0], ylim[0]]), np.array([xlim[1], ylim[1]]) + diag = np.linalg.norm(ul - lr) + + # Compute the arc length along the curve + s = np.cumsum(np.sqrt(np.diff(x) ** 2 + np.diff(y) ** 2)) + + # Truncate the number of arrows if the curve is short + # TODO: figure out a smarter way to do this + frac = min(s[-1] / diag, 1) + if len(arrow_locs) and frac < 0.05: + arrow_locs = [] # too short; no arrows at all + elif len(arrow_locs) and frac < 0.2: + arrow_locs = [0.5] # single arrow in the middle + + # Plot the arrows (and return list if patches) + arrows = [] + for loc in arrow_locs: + n = np.searchsorted(s, s[-1] * loc) + + if dir == 1 and n == 0: + # Move the arrow forward by one if it is at start of a segment + n = 1 + + # Place the head of the arrow at the desired location + arrow_head = [x[n], y[n]] + arrow_tail = [x[n - dir], y[n - dir]] + + p = mpl.patches.FancyArrowPatch( + arrow_tail, arrow_head, transform=axes.transData, lw=0, + **arrow_kw) + axes.add_patch(p) + arrows.append(p) + return arrows + + +def _get_color_offset(ax, color_cycle=None): + """Get color offset based on current lines. + + This function determines that the current offset is for the next color + to use based on current colors in a plot. + + Parameters + ---------- + ax : matplotlib.axes.Axes + Axes containing already plotted lines. + color_cycle : list of matplotlib color specs, optional + Colors to use in plotting lines. Defaults to matplotlib rcParams + color cycle. + + Returns + ------- + color_offset : matplotlib color spec + Starting color for next line to be drawn. + color_cycle : list of matplotlib color specs + Color cycle used to determine colors. + + """ + if color_cycle is None: + color_cycle = plt.rcParams['axes.prop_cycle'].by_key()['color'] + + color_offset = 0 + if len(ax.lines) > 0: + last_color = ax.lines[-1].get_color() + if last_color in color_cycle: + color_offset = color_cycle.index(last_color) + 1 + + return color_offset % len(color_cycle), color_cycle + + +def _get_color( + colorspec, offset=None, fmt=None, ax=None, lines=None, + color_cycle=None): + """Get color to use for plotting line. + + This function returns the color to be used for the line to be drawn (or + None if the detault color cycle for the axes should be used). + + Parameters + ---------- + colorspec : matplotlib color specification + User-specified color (or None). + offset : int, optional + Offset into the color cycle (for multi-trace plots). + fmt : str, optional + Format string passed to plotting command. + ax : matplotlib.axes.Axes, optional + Axes containing already plotted lines. + lines : list of matplotlib.lines.Line2D, optional + List of plotted lines. If not given, use ax.get_lines(). + color_cycle : list of matplotlib color specs, optional + Colors to use in plotting lines. Defaults to matplotlib rcParams + color cycle. + + Returns + ------- + color : matplotlib color spec + Color to use for this line (or None for matplotlib default). + + """ + # See if the color was explicitly specified by the user + if isinstance(colorspec, dict): + if 'color' in colorspec: + return colorspec.pop('color') + elif fmt is not None and \ + [isinstance(arg, str) and + any([c in arg for c in "bgrcmykw#"]) for arg in fmt]: + return None # *fmt will set the color + elif colorspec != None: + return colorspec + + # Figure out what color cycle to use, if not given by caller + if color_cycle == None: + color_cycle = plt.rcParams['axes.prop_cycle'].by_key()['color'] + + # Find the lines that we should pay attention to + if lines is None and ax is not None: + lines = ax.lines + + # If we were passed a set of lines, try to increment color from previous + if offset is not None: + return color_cycle[offset] + elif lines is not None: + color_offset = 0 + if len(ax.lines) > 0: + last_color = ax.lines[-1].get_color() + if last_color in color_cycle: + color_offset = color_cycle.index(last_color) + 1 + color_offset = color_offset % len(color_cycle) + return color_cycle[color_offset] + else: + return None diff --git a/control/ctrlutil.py b/control/ctrlutil.py index 6cd32593b..c16b8c35a 100644 --- a/control/ctrlutil.py +++ b/control/ctrlutil.py @@ -88,7 +88,8 @@ def unwrap(angle, period=2*math.pi): def issys(obj): """Deprecated function to check if an object is an LTI system. - Use isinstance(obj, ct.LTI) + .. deprecated:: 0.10.0 + Use isinstance(obj, ct.LTI) """ warnings.warn("issys() is deprecated; use isinstance(obj, ct.LTI)", diff --git a/control/delay.py b/control/delay.py index d22e44107..9a05675b0 100644 --- a/control/delay.py +++ b/control/delay.py @@ -57,10 +57,10 @@ def pade(T, n=1, numdeg=None): time delay n : positive integer degree of denominator of approximation - numdeg: integer, or None (the default) - If None, numerator degree equals denominator degree - If >= 0, specifies degree of numerator - If < 0, numerator degree is n+numdeg + numdeg : integer, or None (the default) + If numdeg is `None`, numerator degree equals denominator degree. + If numdeg >= 0, specifies degree of numerator. + If numdeg < 0, numerator degree is n+numdeg. Returns ------- diff --git a/control/descfcn.py b/control/descfcn.py index 6586e6f20..e17582c3f 100644 --- a/control/descfcn.py +++ b/control/descfcn.py @@ -13,13 +13,15 @@ """ import math -import numpy as np +from warnings import warn + import matplotlib.pyplot as plt +import numpy as np import scipy -from warnings import warn -from .freqplot import nyquist_response from . import config +from .ctrlplot import ControlPlot +from .freqplot import nyquist_response __all__ = ['describing_function', 'describing_function_plot', 'describing_function_response', 'DescribingFunctionResponse', @@ -100,6 +102,10 @@ def describing_function( A : array_like The amplitude(s) at which the describing function should be calculated. + num_points : int, optional + Number of points to use in computing describing function (default = + 100). + zero_check : bool, optional If `True` (default) then `A` is zero, the function will be evaluated and checked to make sure it is zero. If not, a `TypeError` exception @@ -269,7 +275,7 @@ def __len__(self): # Compute the describing function response + intersections def describing_function_response( H, F, A, omega=None, refine=True, warn_nyquist=None, - plot=False, check_kwargs=True, **kwargs): + _check_kwargs=True, **kwargs): """Compute the describing function response of a system. This function uses describing function analysis to analyze a closed @@ -292,6 +298,10 @@ def describing_function_response( Set to True to turn on warnings generated by `nyquist_plot` or False to turn off warnings. If not set (or set to None), warnings are turned off if omega is specified, otherwise they are turned on. + refine : bool, optional + If `True`, :func:`scipy.optimize.minimize` to refine the estimate + of the intersection of the frequency response and the describing + function. Returns ------- @@ -326,7 +336,7 @@ def describing_function_response( # Start by drawing a Nyquist curve response = nyquist_response( H, omega, warn_encirclements=warn_nyquist, warn_nyquist=warn_nyquist, - check_kwargs=check_kwargs, **kwargs) + _check_kwargs=_check_kwargs, **kwargs) H_omega, H_vals = response.contour.imag, H(response.contour) # Compute the describing function @@ -378,7 +388,7 @@ def _cost(x): def describing_function_plot( - *sysdata, label="%5.2g @ %-5.2g", **kwargs): + *sysdata, point_label="%5.2g @ %-5.2g", label=None, **kwargs): """describing_function_plot(data, *args, **kwargs) Plot a Nyquist plot with a describing function for a nonlinear system. @@ -399,7 +409,7 @@ def describing_function_plot( Parameters ---------- - data : :class:`~control.DescribingFunctionData` + data : :class:`~control.DescribingFunctionResponse` A describing function response data object created by :func:`~control.describing_function_response`. H : LTI system @@ -418,18 +428,42 @@ def describing_function_plot( If True (default), refine the location of the intersection of the Nyquist curve for the linear system and the describing function to determine the intersection point - label : str, optional + label : str or array_like of str, optional + If present, replace automatically generated label with the given label. + point_label : str, optional Formatting string used to label intersection points on the Nyquist plot. Defaults to "%5.2g @ %-5.2g". Set to `None` to omit labels. + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). + warn_nyquist : bool, optional + Set to True to turn on warnings generated by `nyquist_plot` or False + to turn off warnings. If not set (or set to None), warnings are + turned off if omega is specified, otherwise they are turned on. + **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional + Additional keywords passed to `matplotlib` to specify line properties + for Nyquist curve. Returns ------- - lines : 1D array of Line2D - Arrray of Line2D objects for each line in the plot. The first - element of the array is a list of lines (typically only one) for - the Nyquist plot of the linear I/O styem. The second element of - the array is a list of lines (typically only one) for the - describing function curve. + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: Array of :class:`matplotlib.lines.Line2D` objects + for each line in the plot. The first element of the array is a + list of lines (typically only one) for the Nyquist plot of the + linear I/O system. The second element of the array is a list + of lines (typically only one) for the describing function + curve. + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + See :class:`ControlPlot` for more detailed information. Examples -------- @@ -442,7 +476,10 @@ def describing_function_plot( # Process keywords warn_nyquist = config._process_legacy_keyword( kwargs, 'warn', 'warn_nyquist', kwargs.pop('warn_nyquist', None)) + point_label = config._process_legacy_keyword( + kwargs, 'label', 'point_label', point_label) + # TODO: update to be consistent with ctrlplot use of `label` if label not in (False, None) and not isinstance(label, str): raise ValueError("label must be formatting string, False, or None") @@ -454,27 +491,35 @@ def describing_function_plot( *sysdata, refine=kwargs.pop('refine', True), warn_nyquist=warn_nyquist) elif len(sysdata) == 1: - dfresp = sysdata[0] + if not isinstance(sysdata[0], DescribingFunctionResponse): + raise TypeError("data must be DescribingFunctionResponse") + else: + dfresp = sysdata[0] else: raise TypeError("1, 3, or 4 position arguments required") + # Don't allow legend keyword arguments + for kw in ['legend_loc', 'legend_map', 'show_legend']: + if kw in kwargs: + raise TypeError(f"unexpected keyword argument '{kw}'") + # Create a list of lines for the output - out = np.empty(2, dtype=object) + lines = np.empty(2, dtype=object) # Plot the Nyquist response - out[0] = dfresp.response.plot(**kwargs)[0] + cfig = dfresp.response.plot(**kwargs) + lines[0] = cfig.lines[0] # Return Nyquist lines for first system # Add the describing function curve to the plot - lines = plt.plot(dfresp.N_vals.real, dfresp.N_vals.imag) - out[1] = lines + lines[1] = plt.plot(dfresp.N_vals.real, dfresp.N_vals.imag) # Label the intersection points - if label: + if point_label: for pos, (a, omega) in zip(dfresp.positions, dfresp.intersections): # Add labels to the intersection points - plt.text(pos.real, pos.imag, label % (a, omega)) + plt.text(pos.real, pos.imag, point_label % (a, omega)) - return out + return ControlPlot(lines, cfig.axes, cfig.figure) # Utility function to figure out whether two line segments intersection @@ -525,11 +570,11 @@ class saturation_nonlinearity(DescribingFunctionNonlinearity): -------- >>> nl = ct.saturation_nonlinearity(5) >>> nl(1) - 1 + np.int64(1) >>> nl(10) - 5 + np.int64(5) >>> nl(-10) - -5 + np.int64(-5) """ def __init__(self, ub=1, lb=None): diff --git a/control/dtime.py b/control/dtime.py index 9b91eabd3..39b207e02 100644 --- a/control/dtime.py +++ b/control/dtime.py @@ -82,7 +82,7 @@ def sample_system(sysc, Ts, method='zoh', alpha=None, prewarp_frequency=None, Other Parameters ---------------- inputs : int, list of str or None, optional - Description of the system inputs. If not specified, the origional + Description of the system inputs. If not specified, the original system inputs are used. See :class:`InputOutputSystem` for more information. outputs : int, list of str or None, optional diff --git a/control/frdata.py b/control/frdata.py index e0f7fdcc6..c5018babb 100644 --- a/control/frdata.py +++ b/control/frdata.py @@ -1,41 +1,8 @@ -# Copyright (c) 2010 by California Institute of Technology -# Copyright (c) 2012 by Delft University of Technology -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# 3. Neither the names of the California Institute of Technology nor -# the Delft University of Technology nor -# the names of its contributors may be used to endorse or promote -# products derived from this software without specific prior -# written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CALTECH -# OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF -# USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT -# OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF -# SUCH DAMAGE. +# frdata.py - frequency response data representation and functions # # Author: M.M. (Rene) van Paassen (using xferfcn.py as basis) # Date: 02 Oct 12 - """ Frequency response data representation and functions. @@ -43,19 +10,18 @@ FRD data. """ -# External function declarations from copy import copy from warnings import warn import numpy as np -from numpy import angle, array, empty, ones, \ - real, imag, absolute, eye, linalg, where, sort -from scipy.interpolate import splprep, splev +from numpy import absolute, angle, array, empty, eye, imag, linalg, ones, \ + real, sort, where +from scipy.interpolate import splev, splprep -from .lti import LTI, _process_frequency_response +from . import config from .exception import pandas_check from .iosys import InputOutputSystem, _process_iosys_keywords, common_timebase -from . import config +from .lti import LTI, _process_frequency_response __all__ = ['FrequencyResponseData', 'FRD', 'frd'] @@ -100,6 +66,10 @@ class constructor, using the :func:~~control.frd` factory function dt : float, True, or None System timebase. + See Also + -------- + frd + Notes ----- The main data members are 'omega' and 'fresp', where 'omega' is a 1D array @@ -120,7 +90,6 @@ class constructor, using the :func:~~control.frd` factory function for a more detailed description. """ - # # Class attributes # @@ -206,11 +175,12 @@ def __init__(self, *args, **kwargs): "Needs 1 or 2 arguments; received %i." % len(args)) # - # Process key word arguments + # Process keyword arguments # - # If data was generated by a system, keep track of that - self.sysname = kwargs.pop('sysname', None) + # If data was generated by a system, keep track of that (used when + # plotting data). Otherwise, use the system name, if given. + self.sysname = kwargs.pop('sysname', kwargs.get('name', None)) # Keep track of default properties for plotting self.plot_phase = kwargs.pop('plot_phase', None) @@ -280,7 +250,7 @@ def __str__(self): """String representation of the transfer function.""" mimo = self.ninputs > 1 or self.noutputs > 1 - outstr = ['Frequency response data'] + outstr = [f"{InputOutputSystem.__str__(self)}"] for i in range(self.ninputs): for j in range(self.noutputs): @@ -322,7 +292,7 @@ def __add__(self, other): # Convert the second argument to a frequency response function. # or re-base the frd to the current omega (if needed) - other = _convert_to_FRD(other, omega=self.omega) + other = _convert_to_frd(other, omega=self.omega) # Check that the input-output sizes are consistent. if self.ninputs != other.ninputs: @@ -359,7 +329,7 @@ def __mul__(self, other): return FRD(self.fresp * other, self.omega, smooth=(self.ifunc is not None)) else: - other = _convert_to_FRD(other, omega=self.omega) + other = _convert_to_frd(other, omega=self.omega) # Check that the input-output sizes are consistent. if self.ninputs != other.noutputs: @@ -386,7 +356,7 @@ def __rmul__(self, other): return FRD(self.fresp * other, self.omega, smooth=(self.ifunc is not None)) else: - other = _convert_to_FRD(other, omega=self.omega) + other = _convert_to_frd(other, omega=self.omega) # Check that the input-output sizes are consistent. if self.noutputs != other.ninputs: @@ -414,7 +384,7 @@ def __truediv__(self, other): return FRD(self.fresp * (1/other), self.omega, smooth=(self.ifunc is not None)) else: - other = _convert_to_FRD(other, omega=self.omega) + other = _convert_to_frd(other, omega=self.omega) if (self.ninputs > 1 or self.noutputs > 1 or other.ninputs > 1 or other.noutputs > 1): @@ -433,7 +403,7 @@ def __rtruediv__(self, other): return FRD(other / self.fresp, self.omega, smooth=(self.ifunc is not None)) else: - other = _convert_to_FRD(other, omega=self.omega) + other = _convert_to_frd(other, omega=self.omega) if (self.ninputs > 1 or self.noutputs > 1 or other.ninputs > 1 or other.noutputs > 1): @@ -572,8 +542,8 @@ def __call__(self, s=None, squeeze=None, return_magphase=None): ------ ValueError If `s` is not purely imaginary, because - :class:`FrequencyDomainData` systems are only defined at imaginary - frequency values. + :class:`FrequencyResponseData` systems are only defined at + imaginary values (corresponding to real frequencies). """ if s is None: @@ -627,18 +597,19 @@ def freqresp(self, omega): Method has been given the more pythonic name :meth:`FrequencyResponseData.frequency_response`. Or use :func:`freqresp` in the MATLAB compatibility module. + """ warn("FrequencyResponseData.freqresp(omega) will be removed in a " "future release of python-control; use " "FrequencyResponseData.frequency_response(omega), or " "freqresp(sys, omega) in the MATLAB compatibility module " - "instead", DeprecationWarning) + "instead", FutureWarning) return self.frequency_response(omega) def feedback(self, other=1, sign=-1): """Feedback interconnection between two FRD objects.""" - other = _convert_to_FRD(other, omega=self.omega) + other = _convert_to_frd(other, omega=self.omega) if (self.noutputs != other.ninputs or self.ninputs != other.noutputs): raise ValueError( @@ -683,6 +654,14 @@ def plot(self, plot_type=None, *args, **kwargs): # Convert to pandas def to_pandas(self): + """Convert response data to pandas data frame. + + Creates a pandas data frame for the value of the frequency + response at each `omega`. The frequency response values are + labeled in the form "H_{, }" where "" and "" + are replaced with the output and input labels for the system. + + """ if not pandas_check(): ImportError('pandas not installed') import pandas @@ -710,7 +689,7 @@ def to_pandas(self): FRD = FrequencyResponseData -def _convert_to_FRD(sys, omega, inputs=1, outputs=1): +def _convert_to_frd(sys, omega, inputs=1, outputs=1): """Convert a system to frequency response data form (if needed). If sys is already an frd, and its frequency range matches or @@ -721,14 +700,14 @@ def _convert_to_FRD(sys, omega, inputs=1, outputs=1): manually, as in: >>> import numpy as np - >>> from control.frdata import _convert_to_FRD + >>> from control.frdata import _convert_to_frd >>> omega = np.logspace(-1, 1) - >>> frd = _convert_to_FRD(3., omega) # Assumes inputs = outputs = 1 + >>> frd = _convert_to_frd(3., omega) # Assumes inputs = outputs = 1 >>> frd.ninputs, frd.noutputs (1, 1) - >>> frd = _convert_to_FRD(1., omega, inputs=3, outputs=2) + >>> frd = _convert_to_frd(1., omega, inputs=3, outputs=2) >>> frd.ninputs, frd.noutputs (3, 2) @@ -777,51 +756,67 @@ def _convert_to_FRD(sys, omega, inputs=1, outputs=1): sys.__class__) -def frd(*args): - """frd(d, w) +def frd(*args, **kwargs): + """frd(response, omega[, dt]) - Construct a frequency response data model. + Construct a frequency response data (FRD) model. - frd models store the (measured) frequency response of a system. + A frequency response data model stores the (measured) frequency response + of a system. This factory function can be called in different ways: - This function can be called in different ways: - - ``frd(response, freqs)`` + ``frd(response, omega)`` Create an frd model with the given response data, in the form of - complex response vector, at matching frequency freqs [in rad/s] + complex response vector, at matching frequencies ``omega`` [in rad/s]. - ``frd(sys, freqs)`` + ``frd(sys, omega)`` Convert an LTI system into an frd model with data at frequencies - freqs. + ``omega``. Parameters ---------- - response: array_like, or list - complex vector with the system response - freq: array_lik or lis - vector with frequencies - sys: LTI (StateSpace or TransferFunction) - A linear system + response : array_like or LTI system + Complex vector with the system response or an LTI system that can + be used to copmute the frequency response at a list of frequencies. + omega : array_like + Vector of frequencies at which the response is evaluated. + dt : float, True, or None + System timebase. + smooth : bool, optional + If ``True``, create an interpolation function that allows the + frequency response to be computed at any frequency within the range + of frequencies give in ``omega``. If ``False`` (default), + frequency response can only be obtained at the frequencies + specified in ``omega``. Returns ------- - sys: FRD - New frequency response system + sys : :class:`FrequencyResponseData` + New frequency response data system. + + Other Parameters + ---------------- + 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 + original system. + name : string, optional + System name. If unspecified, a generic name is generated + with a unique integer id. See Also -------- - FRD, ss, tf + FrequencyResponseData, frequency_response, ss, tf Examples -------- >>> # Create from measurements >>> response = [1.0, 1.0, 0.5] - >>> freqs = [1, 10, 100] - >>> F = ct.frd(response, freqs) + >>> omega = [1, 10, 100] + >>> F = ct.frd(response, omega) >>> G = ct.tf([1], [1, 1]) - >>> freqs = [1, 10, 100] - >>> F = ct.frd(G, freqs) + >>> omega = [1, 10, 100] + >>> F = ct.frd(G, omega) """ - return FRD(*args) + return FrequencyResponseData(*args, **kwargs) diff --git a/control/freqplot.py b/control/freqplot.py index 961f499b3..544425298 100644 --- a/control/freqplot.py +++ b/control/freqplot.py @@ -8,44 +8,36 @@ # charts is in nichols.py. The code for pole-zero diagrams is in pzmap.py # and rlocus.py. -import numpy as np -import matplotlib as mpl -import matplotlib.pyplot as plt +import itertools import math import warnings -import itertools from os.path import commonprefix -from .ctrlutil import unwrap +import matplotlib as mpl +import matplotlib.pyplot as plt +import numpy as np + +from . import config from .bdalg import feedback -from .margins import stability_margins +from .ctrlplot import ControlPlot, _add_arrows_to_line2D, _find_axes_center, \ + _get_color, _get_color_offset, _get_line_labels, _make_legend_labels, \ + _process_ax_keyword, _process_legend_keywords, _process_line_labels, \ + _update_plot_title +from .ctrlutil import unwrap from .exception import ControlMIMONotImplemented +from .frdata import FrequencyResponseData +from .lti import LTI, _process_frequency_response, frequency_response +from .margins import stability_margins from .statesp import StateSpace -from .lti import LTI, frequency_response, _process_frequency_response from .xferfcn import TransferFunction -from .frdata import FrequencyResponseData -from .timeplot import _make_legend_labels -from . import config __all__ = ['bode_plot', 'NyquistResponseData', 'nyquist_response', 'nyquist_plot', 'singular_values_response', 'singular_values_plot', 'gangof4_plot', 'gangof4_response', - 'bode', 'nyquist', 'gangof4'] - -# Default font dictionary -_freqplot_rcParams = mpl.rcParams.copy() -_freqplot_rcParams.update({ - 'axes.labelsize': 'small', - 'axes.titlesize': 'small', - 'figure.titlesize': 'medium', - 'legend.fontsize': 'x-small', - 'xtick.labelsize': 'small', - 'ytick.labelsize': 'small', -}) + 'bode', 'nyquist', 'gangof4', 'FrequencyResponseList'] # Default values for module parameter variables _freqplot_defaults = { - 'freqplot.rcParams': _freqplot_rcParams, 'freqplot.feature_periphery_decades': 1, 'freqplot.number_of_samples': 1000, 'freqplot.dB': False, # Plot gain in dB @@ -53,10 +45,11 @@ 'freqplot.Hz': False, # Plot frequency in Hertz 'freqplot.grid': True, # Turn on grid for gain and phase 'freqplot.wrap_phase': False, # Wrap the phase plot at a given value - 'freqplot.freq_label': "Frequency [%s]", + 'freqplot.freq_label': "Frequency [{units}]", 'freqplot.share_magnitude': 'row', 'freqplot.share_phase': 'row', 'freqplot.share_frequency': 'col', + 'freqplot.title_frame': 'axes', } # @@ -93,9 +86,8 @@ def bode_plot( data, omega=None, *fmt, ax=None, omega_limits=None, omega_num=None, plot=None, plot_magnitude=True, plot_phase=None, overlay_outputs=None, overlay_inputs=None, phase_label=None, - magnitude_label=None, display_margins=None, - margins_method='best', legend_map=None, legend_loc=None, - sharex=None, sharey=None, title=None, **kwargs): + magnitude_label=None, label=None, display_margins=None, + margins_method='best', title=None, sharex=None, sharey=None, **kwargs): """Bode plot for a system. Plot the magnitude and phase of the frequency response over a @@ -107,9 +99,9 @@ def bode_plot( List of LTI systems or :class:`FrequencyResponseData` objects. A single system or frequency response can also be passed. omega : array_like, optoinal - List of frequencies in rad/sec over to plot over. If not specified, - this will be determined from the proporties of the systems. Ignored - if `data` is not a list of systems. + Set of frequencies in rad/sec to plot over. If not specified, this + will be determined from the proporties of the systems. Ignored if + `data` is not a list of systems. *fmt : :func:`matplotlib.pyplot.plot` format string, optional Passed to `matplotlib` as the format string for all lines in the plot. The `omega` parameter must be present (use omega=None if needed). @@ -126,43 +118,103 @@ def bode_plot( graphs and display the margins at the top of the graph. If set to 'overlay', the values for the gain and phase margin are placed on the graph. Setting display_margins turns off the axes grid. - margins_method : str, optional - Method to use in computing margins (see :func:`stability_margins`). **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional Additional keywords passed to `matplotlib` to specify line properties. Returns ------- - lines : array of Line2D - Array of Line2D objects for each line in the plot. The shape of - the array matches the subplots shape and the value of the array is a - list of Line2D objects in that subplot. + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: Array of :class:`matplotlib.lines.Line2D` objects + for each line in the plot. The shape of the array matches the + subplots shape and the value of the array is a list of Line2D + objects in that subplot. + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + * cplt.legend: legend object(s) contained in the plot + + See :class:`ControlPlot` for more detailed information. Other Parameters ---------------- - grid : bool + ax : array of matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified, the + axes for the current figure are used or, if there is no current + figure with the correct number and shape of axes, a new figure is + created. The shape of the array must match the shape of the + plotted data. + freq_label, magnitude_label, phase_label : str, optional + Labels to use for the frequency, magnitude, and phase axes. + Defaults are set by `config.defaults['freqplot.']`. + grid : bool, optional If True, plot grid lines on gain and phase plots. Default is set by `config.defaults['freqplot.grid']`. - initial_phase : float + initial_phase : float, optional Set the reference phase to use for the lowest frequency. If set, the initial phase of the Bode plot will be set to the value closest to the value specified. Units are in either degrees or radians, depending on the `deg` parameter. Default is -180 if wrap_phase is False, 0 if wrap_phase is True. + label : str or array_like of str, optional + If present, replace automatically generated label(s) with the given + label(s). If sysdata is a list, strings should be specified for each + system. If MIMO, strings required for each system, output, and input. + legend_map : array of str, optional + Location of the legend for multi-axes plots. Specifies an array + of legend location strings matching the shape of the subplots, with + each entry being either None (for no legend) or a legend location + string (see :func:`~matplotlib.pyplot.legend`). + legend_loc : int or str, optional + Include a legend in the given location. Default is 'center right', + with no legend for a single response. Use False to suppress legend. + margins_method : str, optional + Method to use in computing margins (see :func:`stability_margins`). omega_limits : array_like of two values - Set limits for plotted frequency range. If Hz=True the limits - are in Hz otherwise in rad/s. + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. Ignored if + data is not a list of systems. omega_num : int Number of samples to use for the frequeny range. Defaults to - config.defaults['freqplot.number_of_samples']. Ignore if data is + config.defaults['freqplot.number_of_samples']. Ignored if data is not a list of systems. + overlay_inputs, overlay_outputs : bool, optional + If set to True, combine input and/or output signals onto a single + plot and use line colors, labels, and a legend to distinguish them. plot : bool, optional (legacy) If given, `bode_plot` returns the legacy return values of magnitude, phase, and frequency. If False, just return the values with no plot. + plot_magnitude, plot_phase : bool, optional + If set to `False`, don't plot the magnitude or phase, respectively. rcParams : dict Override the default parameters used for generating plots. - Default is set by config.default['freqplot.rcParams']. + Default is set by config.default['ctrlplot.rcParams']. + share_frequency, share_magnitude, share_phase : str or bool, optional + Determine whether and how axis limits are shared between the + indicated variables. Can be set set to 'row' to share across all + subplots in a row, 'col' to set across all subplots in a column, or + `False` to allow independent limits. Note: if `sharex` is given, + it sets the value of `share_frequency`; if `sharey` is given, it + sets the value of both `share_magnitude` and `share_phase`. + Default values are 'row' for `share_magnitude` and `share_phase', + 'col', for `share_frequency`, and can be set using + config.defaults['freqplot.share_']. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on an + axis or ``legend_loc`` or ``legend_map`` has been specified. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). + title_frame : str, optional + Set the frame of reference used to center the plot title. If set to + 'axes' (default), the horizontal position of the title will be + centered relative to the axes. If set to 'figure', it will be + centered with respect to the figure (faster execution). wrap_phase : bool or float If wrap_phase is `False` (default), then the phase will be unwrapped so that it is continuously increasing or decreasing. If wrap_phase is @@ -175,14 +227,18 @@ def bode_plot( The default values for Bode plot configuration parameters can be reset using the `config.defaults` dictionary, with module name 'bode'. + See Also + -------- + frequency_response + Notes ----- - 1. Starting with python-control version 0.10, `bode_plot`returns an - array of lines instead of magnitude, phase, and frequency. To - recover the old behavior, call `bode_plot` with `plot=True`, which - will force the legacy values (mag, phase, omega) to be returned - (with a warning). To obtain just the frequency response of a system - (or list of systems) without plotting, use the + 1. Starting with python-control version 0.10, `bode_plot` returns a + :class:`ControlPlot` object instead of magnitude, phase, and + frequency. To recover the old behavior, call `bode_plot` with + `plot=True`, which will force the legacy values (mag, phase, omega) + to be returned (with a warning). To obtain just the frequency + response of a system (or list of systems) without plotting, use the :func:`~control.frequency_response` command. 2. If a discrete time model is given, the frequency response is plotted @@ -217,8 +273,9 @@ def bode_plot( 'freqplot', 'wrap_phase', kwargs, _freqplot_defaults, pop=True) initial_phase = config._get_param( 'freqplot', 'initial_phase', kwargs, None, pop=True) - freqplot_rcParams = config._get_param( - 'freqplot', 'rcParams', kwargs, _freqplot_defaults, pop=True) + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + title_frame = config._get_param( + 'freqplot', 'title_frame', kwargs, _freqplot_defaults, pop=True) # Set the default labels freq_label = config._get_param( @@ -268,7 +325,7 @@ def bode_plot( # # If we were passed a list of systems, convert to data - if all([isinstance( + if any([isinstance( sys, (StateSpace, TransferFunction)) for sys in data]): data = frequency_response( data, omega=omega, omega_limits=omega_limits, @@ -376,8 +433,8 @@ def bode_plot( if plot is not None: warnings.warn( - "`bode_plot` return values of mag, phase, omega is deprecated; " - "use frequency_response()", DeprecationWarning) + "bode_plot() return value of mag, phase, omega is deprecated; " + "use frequency_response()", FutureWarning) if plot is False: # Process the data to match what we were sent @@ -448,47 +505,16 @@ def bode_plot( (noutputs if plot_phase else 0) ncols = ninputs - # See if we can use the current figure axes - fig = plt.gcf() # get current figure (or create new one) - if ax is None and plt.get_fignums(): - ax = fig.get_axes() - if len(ax) == nrows * ncols: - # Assume that the shape is right (no easy way to infer this) - ax = np.array(ax).reshape(nrows, ncols) - - # Clear out any old text from the current figure - for text in fig.texts: - text.set_visible(False) # turn off the text - del text # get rid of it completely - - elif len(ax) != 0: - # Need to generate a new figure - fig, ax = plt.figure(), None - - else: - # Blank figure, just need to recreate axes - ax = None - - # Create new axes, if needed, and customize them if ax is None: - with plt.rc_context(_freqplot_rcParams): - ax_array = fig.subplots(nrows, ncols, squeeze=False) - fig.set_layout_engine('tight') - fig.align_labels() - # Set up default sharing of axis limits if not specified for kw in ['share_magnitude', 'share_phase', 'share_frequency']: if kw not in kwargs or kwargs[kw] is None: kwargs[kw] = config.defaults['freqplot.' + kw] - else: - # Make sure the axes are the right shape - if ax.shape != (nrows, ncols): - raise ValueError( - "specified axes are not the right shape; " - f"got {ax.shape} but expecting ({nrows}, {ncols})") - ax_array = ax - fig = ax_array[0, 0].figure # just in case this is not gcf() + fig, ax_array = _process_ax_keyword( + ax, (nrows, ncols), squeeze=False, rcParams=rcParams, clear_text=True) + legend_loc, legend_map, show_legend = _process_legend_keywords( + kwargs, (nrows,ncols), 'center right') # Get the values for sharing axes limits share_magnitude = kwargs.pop('share_magnitude', None) @@ -562,7 +588,7 @@ def bode_plot( # axes are available and no updates should be made. # - # Utility function to turn off sharing + # Utility function to turn on sharing def _share_axes(ref, share_map, axis): ref_ax = ax_array[ref] for index in np.nditer(share_map, flags=["refs_ok"]): @@ -624,6 +650,9 @@ def _share_axes(ref, share_map, axis): for j in range(ncols): out[i, j] = [] # unique list in each element + # Process label keyword + line_labels = _process_line_labels(label, len(data), ninputs, noutputs) + # Utility function for creating line label def _make_line_label(response, output_index, input_index): label = "" # start with an empty label @@ -664,7 +693,10 @@ def _make_line_label(response, output_index, input_index): phase_plot = phase[i, j] * 180. / math.pi if deg else phase[i, j] # Generate a label - label = _make_line_label(response, i, j) + if line_labels is None: + label = _make_line_label(response, i, j) + else: + label = line_labels[index, i, j] # Magnitude if plot_magnitude: @@ -805,7 +837,7 @@ def _make_line_label(response, output_index, input_index): axes_title = ax.get_title() if axes_title is not None and axes_title != "": axes_title += "\n" - with plt.rc_context(_freqplot_rcParams): + with plt.rc_context(rcParams): ax.set_title( axes_title + f"{sysname}: " "Gm = %.2f %s(at %.2f %s), " @@ -820,11 +852,11 @@ def _make_line_label(response, output_index, input_index): # # Finishing handling axes limit sharing # - # This code handles labels on phase plots and also removes tick labels + # This code handles labels on Bode plots and also removes tick labels # on shared axes. It needs to come *after* the plots are generated, # in order to handle two things: # - # * manually generated labels and grids need to reflect the limts for + # * manually generated labels and grids need to reflect the limits for # shared axes, which we don't know until we have plotted everything; # # * the loglog and semilog functions regenerate the labels (not quite @@ -884,50 +916,6 @@ def gen_zero_centered_series(val_min, val_max, period): for i, j in itertools.product(range(nrows), range(ncols)): ax_array[i, j].set_xlim(omega_limits) - # - # Update the plot title (= figure suptitle) - # - # If plots are built up by multiple calls to plot() and the title is - # not given, then the title is updated to provide a list of unique text - # items in each successive title. For data generated by the frequency - # response function this will generate a common prefix followed by a - # list of systems (e.g., "Step response for sys[1], sys[2]"). - # - - # Set the initial title for the data (unique system names, preserving order) - seen = set() - sysnames = [response.sysname for response in data \ - if not (response.sysname in seen or seen.add(response.sysname))] - if title is None: - if data[0].title is None: - title = "Bode plot for " + ", ".join(sysnames) - else: - title = data[0].title - - if fig is not None and isinstance(title, str): - # Get the current title, if it exists - old_title = None if fig._suptitle is None else fig._suptitle._text - new_title = title - - if old_title is not None: - # Find the common part of the titles - common_prefix = commonprefix([old_title, new_title]) - - # Back up to the last space - last_space = common_prefix.rfind(' ') - if last_space > 0: - common_prefix = common_prefix[:last_space] - common_len = len(common_prefix) - - # Add the new part of the title (usually the system name) - if old_title[common_len:] != new_title[common_len:]: - separator = ',' if len(common_prefix) > 0 else ';' - new_title = old_title + separator + new_title[common_len:] - - # Add the title - with plt.rc_context(freqplot_rcParams): - fig.suptitle(new_title) - # # Label the axes (including header labels) # @@ -945,11 +933,12 @@ def gen_zero_centered_series(val_min, val_max, period): # If we have more than one column, label the individual responses if (noutputs > 1 and not overlay_outputs or ninputs > 1) \ and not overlay_inputs: - with plt.rc_context(_freqplot_rcParams): + with plt.rc_context(rcParams): ax_array[0, j].set_title(f"From {data[0].input_labels[j]}") # Label the frequency axis - ax_array[-1, j].set_xlabel(freq_label % ("Hz" if Hz else "rad/s",)) + ax_array[-1, j].set_xlabel( + freq_label.format(units="Hz" if Hz else "rad/s")) # Label the rows for i in range(noutputs if not overlay_outputs else 1): @@ -966,38 +955,55 @@ def gen_zero_centered_series(val_min, val_max, period): ax_mag.set_ylabel("\n" + ax_mag.get_ylabel()) ax_phase.set_ylabel("\n" + ax_phase.get_ylabel()) - # TODO: remove? - # Redraw the figure to get the proper locations for everything - # fig.tight_layout() + # Find the midpoint between the row axes (+ tight_layout) + _, ypos = _find_axes_center(fig, [ax_mag, ax_phase]) # Get the bounding box including the labels inv_transform = fig.transFigure.inverted() mag_bbox = inv_transform.transform( ax_mag.get_tightbbox(fig.canvas.get_renderer())) - phase_bbox = inv_transform.transform( - ax_phase.get_tightbbox(fig.canvas.get_renderer())) - - # Get the axes limits without labels for use in the y position - mag_bot = inv_transform.transform( - ax_mag.transAxes.transform((0, 0)))[1] - phase_top = inv_transform.transform( - ax_phase.transAxes.transform((0, 1)))[1] # Figure out location for the text (center left in figure frame) xpos = mag_bbox[0, 0] # left edge - ypos = (mag_bot + phase_top) / 2 # centered between axes # Put a centered label as text outside the box fig.text( 0.8 * xpos, ypos, f"To {data[0].output_labels[i]}\n", rotation=90, ha='left', va='center', - fontsize=_freqplot_rcParams['axes.titlesize']) + fontsize=rcParams['axes.titlesize']) else: # Only a single axes => add label to the left ax_array[i, 0].set_ylabel( f"To {data[0].output_labels[i]}\n" + ax_array[i, 0].get_ylabel()) + # + # Update the plot title (= figure suptitle) + # + # If plots are built up by multiple calls to plot() and the title is + # not given, then the title is updated to provide a list of unique text + # items in each successive title. For data generated by the frequency + # response function this will generate a common prefix followed by a + # list of systems (e.g., "Step response for sys[1], sys[2]"). + # + + # Set the initial title for the data (unique system names, preserving order) + seen = set() + sysnames = [response.sysname for response in data \ + if not (response.sysname in seen or seen.add(response.sysname))] + + if ax is None and title is None: + if data[0].title is None: + title = "Bode plot for " + ", ".join(sysnames) + else: + # Allow data to set the title (used by gangof4) + title = data[0].title + _update_plot_title(title, fig, rcParams=rcParams, frame=title_frame) + elif ax is None: + _update_plot_title( + title, fig=fig, rcParams=rcParams, frame=title_frame, + use_existing=False) + # # Create legends # @@ -1018,30 +1024,33 @@ def gen_zero_centered_series(val_min, val_max, period): # different response (system). # - # Figure out where to put legends - if legend_map is None: - legend_map = np.full(ax_array.shape, None, dtype=object) - if legend_loc == None: - legend_loc = 'center right' - - # TODO: add in additional processing later - - # Put legend in the upper right - legend_map[0, -1] = legend_loc - # Create axis legends - for i in range(nrows): - for j in range(ncols): + if show_legend != False: + # Figure out where to put legends + if legend_map is None: + legend_map = np.full(ax_array.shape, None, dtype=object) + legend_map[0, -1] = legend_loc + + legend_array = np.full(ax_array.shape, None, dtype=object) + for i, j in itertools.product(range(nrows), range(ncols)): + if legend_map[i, j] is None: + continue ax = ax_array[i, j] + # Get the labels to use, removing common strings lines = [line for line in ax.get_lines() if line.get_label()[0] != '_'] - labels = _make_legend_labels([line.get_label() for line in lines]) + labels = _make_legend_labels( + [line.get_label() for line in lines], + ignore_common=line_labels is not None) # Generate the label, if needed - if len(labels) > 1 and legend_map[i, j] != None: - with plt.rc_context(freqplot_rcParams): - ax.legend(lines, labels, loc=legend_map[i, j]) + if show_legend == True or len(labels) > 1: + with plt.rc_context(rcParams): + legend_array[i, j] = ax.legend( + lines, labels, loc=legend_map[i, j]) + else: + legend_array = None # # Legacy return pocessing @@ -1059,7 +1068,7 @@ def gen_zero_centered_series(val_min, val_max, period): else: return mag_data, phase_data, omega_data - return out + return ControlPlot(out, ax_array, fig, legend=legend_array) # @@ -1079,7 +1088,9 @@ def gen_zero_centered_series(val_min, val_max, period): 'nyquist.max_curve_magnitude': 20, # clip large values 'nyquist.max_curve_offset': 0.02, # offset of primary/mirror 'nyquist.start_marker': 'o', # marker at start of curve - 'nyquist.start_marker_size': 4, # size of the maker + 'nyquist.start_marker_size': 4, # size of the marker + 'nyquist.circle_style': # style for unit circles + {'color': 'black', 'linestyle': 'dashed', 'linewidth': 1} } @@ -1149,9 +1160,9 @@ def plot(self, *args, **kwargs): def nyquist_response( - sysdata, omega=None, plot=None, omega_limits=None, omega_num=None, + sysdata, omega=None, omega_limits=None, omega_num=None, return_contour=False, warn_encirclements=True, warn_nyquist=True, - check_kwargs=True, **kwargs): + _check_kwargs=True, **kwargs): """Nyquist response for a system. Computes a Nyquist contour for the system over a (optional) frequency @@ -1170,12 +1181,6 @@ def nyquist_response( curves for each system are plotted on the same graph. omega : array_like, optional Set of frequencies to be evaluated, in rad/sec. - omega_limits : array_like of two values, optional - Limits to the range of frequencies. Ignored if omega is provided, and - auto-generated if omitted. - omega_num : int, optional - Number of frequency samples to plot. Defaults to - config.defaults['freqplot.number_of_samples']. Returns ------- @@ -1196,23 +1201,25 @@ def nyquist_response( Define the threshold for generating a warning if the number of net encirclements is a non-integer value. Default value is 0.05 and can be set using config.defaults['nyquist.encirclement_threshold']. - indent_direction : str, optional For poles on the imaginary axis, set the direction of indentation to be 'right' (default), 'left', or 'none'. - indent_points : int, optional Number of points to insert in the Nyquist contour around poles that are at or near the imaginary axis. - indent_radius : float, optional Amount to indent the Nyquist contour around poles on or near the imaginary axis. Portions of the Nyquist plot corresponding to indented portions of the contour are plotted using a different line style. - + omega_limits : array_like of two values + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. + omega_num : int, optional + Number of samples to use for the frequeny range. Defaults to + config.defaults['freqplot.number_of_samples']. warn_nyquist : bool, optional If set to 'False', turn off warnings about frequencies above Nyquist. - warn_encirclements : bool, optional If set to 'False', turn off warnings about number of encirclements not meeting the Nyquist criterion. @@ -1231,8 +1238,7 @@ def nyquist_response( right of stable poles and the left of unstable poles. If a pole is exactly on the imaginary axis, the `indent_direction` parameter can be used to set the direction of indentation. Setting `indent_direction` - to `none` will turn off indentation. If `return_contour` is True, the - exact contour used for evaluation is returned. + to `none` will turn off indentation. 3. For those portions of the Nyquist plot in which the contour is indented to avoid poles, resuling in a scaling of the Nyquist plot, @@ -1245,6 +1251,10 @@ def nyquist_response( response object can be iterated over to return `count, contour`. This behavior is deprecated and will be removed in a future release. + See Also + -------- + nyquist_plot + Examples -------- >>> G = ct.zpk([], [-1, -2, -3], gain=100) @@ -1266,7 +1276,7 @@ def nyquist_response( indent_points = config._get_param( 'nyquist', 'indent_points', kwargs, _nyquist_defaults, pop=True) - if check_kwargs and kwargs: + if _check_kwargs and kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) # Convert the first argument to a list @@ -1295,7 +1305,11 @@ def nyquist_response( "Nyquist plot currently only supports SISO systems.") # Figure out the frequency range - omega_sys = np.asarray(omega) + if isinstance(sys, FrequencyResponseData) and sys.ifunc is None \ + and not omega_range_given: + omega_sys = sys.omega # use system frequencies + else: + omega_sys = np.asarray(omega) # use common omega vector # Determine the contour used to evaluate the Nyquist curve if sys.isdtime(strict=True): @@ -1491,8 +1505,9 @@ def nyquist_response( def nyquist_plot( - data, omega=None, plot=None, label_freq=0, color=None, - return_contour=None, title=None, legend_loc='upper right', **kwargs): + data, omega=None, plot=None, label_freq=0, color=None, label=None, + return_contour=None, title=None, ax=None, + unit_circle=False, mt_circles=None, ms_circles=None, **kwargs): """Nyquist plot for a system. Generates a Nyquist plot for the system over a (optional) frequency @@ -1509,39 +1524,43 @@ def nyquist_plot( List of linear input/output systems (single system is OK) or Nyquist ersponses (computed using :func:`~control.nyquist_response`). Nyquist curves for each system are plotted on the same graph. - omega : array_like, optional - Set of frequencies to be evaluated, in rad/sec. + Set of frequencies to be evaluated, in rad/sec. Specifying + ``omega`` as a list of two elements is equivalent to providing + ``omega_limits``. + unit_circle : bool, optional + If ``True``, display the unit circle, to read gain crossover frequency. + mt_circles : array_like, optional + Draw circles corresponding to the given magnitudes of sensitivity. + ms_circles : array_like, optional + Draw circles corresponding to the given magnitudes of complementary + sensitivity. + **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional + Additional keywords passed to `matplotlib` to specify line properties. - omega_limits : array_like of two values, optional - Limits to the range of frequencies. Ignored if omega is provided, and - auto-generated if omitted. + Returns + ------- + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: - omega_num : int, optional - Number of frequency samples to plot. Defaults to - config.defaults['freqplot.number_of_samples']. + * cplt.lines: 2D array of :class:`matplotlib.lines.Line2D` + objects for each line in the plot. The shape of the array is + given by (nsys, 4) where nsys is the number of systems or + Nyquist responses passed to the function. The second index + specifies the segment type: - color : string, optional - Used to specify the color of the line and arrowhead. + - lines[idx, 0]: unscaled portion of the primary curve + - lines[idx, 1]: scaled portion of the primary curve + - lines[idx, 2]: unscaled portion of the mirror curve + - lines[idx, 3]: scaled portion of the mirror curve - return_contour : bool, optional - If 'True', return the contour used to evaluate the Nyquist plot. + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. - **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional - Additional keywords (passed to `matplotlib`) + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. - Returns - ------- - lines : array of Line2D - 2D array of Line2D objects for each line in the plot. The shape of - the array is given by (nsys, 4) where nsys is the number of systems - or Nyquist responses passed to the function. The second index - specifies the segment type: + * cplt.legend: legend object(s) contained in the plot - * lines[idx, 0]: unscaled portion of the primary curve - * lines[idx, 1]: scaled portion of the primary curve - * lines[idx, 2]: unscaled portion of the mirror curve - * lines[idx, 3]: scaled portion of the mirror curve + See :class:`ControlPlot` for more detailed information. Other Parameters ---------------- @@ -1554,81 +1573,104 @@ def nyquist_plot( a 2D array is passed, the first row will be used to specify arrow locations for the primary curve and the second row will be used for the mirror image. - arrow_size : float, optional Arrowhead width and length (in display coordinates). Default value is 8 and can be set using config.defaults['nyquist.arrow_size']. - arrow_style : matplotlib.patches.ArrowStyle, optional Define style used for Nyquist curve arrows (overrides `arrow_size`). - + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. encirclement_threshold : float, optional Define the threshold for generating a warning if the number of net encirclements is a non-integer value. Default value is 0.05 and can be set using config.defaults['nyquist.encirclement_threshold']. - indent_direction : str, optional For poles on the imaginary axis, set the direction of indentation to be 'right' (default), 'left', or 'none'. - indent_points : int, optional Number of points to insert in the Nyquist contour around poles that are at or near the imaginary axis. - indent_radius : float, optional Amount to indent the Nyquist contour around poles on or near the imaginary axis. Portions of the Nyquist plot corresponding to indented portions of the contour are plotted using a different line style. - + label : str or array_like of str, optional + If present, replace automatically generated label(s) with the given + label(s). If sysdata is a list, strings should be specified for each + system. label_freq : int, optiona Label every nth frequency on the plot. If not specified, no labels are generated. - + legend_loc : int or str, optional + Include a legend in the given location. Default is 'upper right', + with no legend for a single response. Use False to suppress legend. max_curve_magnitude : float, optional Restrict the maximum magnitude of the Nyquist plot to this value. Portions of the Nyquist plot whose magnitude is restricted are plotted using a different line style. - max_curve_offset : float, optional When plotting scaled portion of the Nyquist plot, increase/decrease the magnitude by this fraction of the max_curve_magnitude to allow any overlaps between the primary and mirror curves to be avoided. - mirror_style : [str, str] or False Linestyles for mirror image of the Nyquist curve. The first element is used for unscaled portions of the Nyquist curve, the second element is used for portions that are scaled (using max_curve_magnitude). If `False` then omit completely. Default linestyle (['--', ':']) is determined by config.defaults['nyquist.mirror_style']. - + omega_limits : array_like of two values + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. + omega_num : int, optional + Number of samples to use for the frequeny range. Defaults to + config.defaults['freqplot.number_of_samples']. Ignored if data is + not a list of systems. plot : bool, optional - (legacy) If given, `bode_plot` returns the legacy return values - of magnitude, phase, and frequency. If False, just return the - values with no plot. - + (legacy) If given, `nyquist_plot` returns the legacy return values + of (counts, contours). If False, return the values with no plot. primary_style : [str, str], optional Linestyles for primary image of the Nyquist curve. The first element is used for unscaled portions of the Nyquist curve, the second element is used for portions that are scaled (using max_curve_magnitude). Default linestyle (['-', '-.']) is determined by config.defaults['nyquist.mirror_style']. - + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + return_contour : bool, optional + (legacy) If 'True', return the encirclement count and Nyquist + contour used to generate the Nyquist plot. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on the + plot or ``legend_loc`` has been specified. start_marker : str, optional Matplotlib marker to use to mark the starting point of the Nyquist plot. Defaults value is 'o' and can be set using config.defaults['nyquist.start_marker']. - start_marker_size : float, optional Start marker size (in display coordinates). Default value is 4 and can be set using config.defaults['nyquist.start_marker_size']. - + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). + title_frame : str, optional + Set the frame of reference used to center the plot title. If set to + 'axes' (default), the horizontal position of the title will + centered relative to the axes. If set to 'figure', it will be + centered with respect to the figure (faster execution). warn_nyquist : bool, optional If set to 'False', turn off warnings about frequencies above Nyquist. - warn_encirclements : bool, optional If set to 'False', turn off warnings about number of encirclements not meeting the Nyquist criterion. + See Also + -------- + nyquist_response + Notes ----- 1. If a discrete time model is given, the frequency response is computed @@ -1680,14 +1722,18 @@ def nyquist_plot( arrow_size = config._get_param( 'nyquist', 'arrow_size', kwargs, _nyquist_defaults, pop=True) arrow_style = config._get_param('nyquist', 'arrow_style', kwargs, None) + ax_user = ax max_curve_magnitude = config._get_param( 'nyquist', 'max_curve_magnitude', kwargs, _nyquist_defaults, pop=True) max_curve_offset = config._get_param( 'nyquist', 'max_curve_offset', kwargs, _nyquist_defaults, pop=True) + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) start_marker = config._get_param( 'nyquist', 'start_marker', kwargs, _nyquist_defaults, pop=True) start_marker_size = config._get_param( 'nyquist', 'start_marker_size', kwargs, _nyquist_defaults, pop=True) + title_frame = config._get_param( + 'freqplot', 'title_frame', kwargs, _freqplot_defaults, pop=True) # Set line styles for the curves def _parse_linestyle(style_name, allow_false=False): @@ -1729,6 +1775,9 @@ def _parse_linestyle(style_name, allow_false=False): if not isinstance(data, (list, tuple)): data = [data] + # Process label keyword + line_labels = _process_line_labels(label, len(data)) + # If we are passed a list of systems, compute response first if all([isinstance( sys, (StateSpace, TransferFunction, FrequencyResponseData)) @@ -1740,15 +1789,16 @@ def _parse_linestyle(style_name, allow_false=False): omega_num=kwargs.pop('omega_num', None), warn_encirclements=kwargs.pop('warn_encirclements', True), warn_nyquist=kwargs.pop('warn_nyquist', True), - check_kwargs=False, **kwargs) + indent_radius=kwargs.pop('indent_radius', None), + _check_kwargs=False, **kwargs) else: nyquist_responses = data # Legacy return value processing if plot is not None or return_contour is not None: warnings.warn( - "`nyquist_plot` return values of count[, contour] is deprecated; " - "use nyquist_response()", DeprecationWarning) + "nyquist_plot() return value of count[, contour] is deprecated; " + "use nyquist_response()", FutureWarning) # Extract out the values that we will eventually return counts = [response.count for response in nyquist_responses] @@ -1765,6 +1815,11 @@ def _parse_linestyle(style_name, allow_false=False): # Return counts and (optionally) the contour we used return (counts, contours) if return_contour else counts + fig, ax = _process_ax_keyword( + ax_user, shape=(1, 1), squeeze=True, rcParams=rcParams) + legend_loc, _, show_legend = _process_legend_keywords( + kwargs, None, 'upper right') + # Create a list of lines for the output out = np.empty(len(nyquist_responses), dtype=object) for i in range(out.shape[0]): @@ -1794,12 +1849,14 @@ def _parse_linestyle(style_name, allow_false=False): reg_mask, abs(resp) > max_curve_magnitude) resp[rescale] *= max_curve_magnitude / abs(resp[rescale]) + # Get the label to use for the line + label = response.sysname if line_labels is None else line_labels[idx] + # Plot the regular portions of the curve (and grab the color) x_reg = np.ma.masked_where(reg_mask, resp.real) y_reg = np.ma.masked_where(reg_mask, resp.imag) p = plt.plot( - x_reg, y_reg, primary_style[0], color=color, - label=response.sysname, **kwargs) + x_reg, y_reg, primary_style[0], color=color, label=label, **kwargs) c = p[0].get_color() out[idx] += p @@ -1862,7 +1919,48 @@ def _parse_linestyle(style_name, allow_false=False): # Mark the -1 point plt.plot([-1], [0], 'r+') - # Label the frequencies of the points + # + # Draw circles for gain crossover and sensitivity functions + # + theta = np.linspace(0, 2*np.pi, 100) + cos = np.cos(theta) + sin = np.sin(theta) + label_pos = 15 + + # Display the unit circle, to read gain crossover frequency + if unit_circle: + plt.plot(cos, sin, **config.defaults['nyquist.circle_style']) + + # Draw circles for given magnitudes of sensitivity + if ms_circles is not None: + for ms in ms_circles: + pos_x = -1 + (1/ms)*cos + pos_y = (1/ms)*sin + plt.plot( + pos_x, pos_y, **config.defaults['nyquist.circle_style']) + plt.text(pos_x[label_pos], pos_y[label_pos], ms) + + # Draw circles for given magnitudes of complementary sensitivity + if mt_circles is not None: + for mt in mt_circles: + if mt != 1: + ct = -mt**2/(mt**2-1) # Mt center + rt = mt/(mt**2-1) # Mt radius + pos_x = ct+rt*cos + pos_y = rt*sin + plt.plot( + pos_x, pos_y, + **config.defaults['nyquist.circle_style']) + plt.text(pos_x[label_pos], pos_y[label_pos], mt) + else: + _, _, ymin, ymax = plt.axis() + pos_y = np.linspace(ymin, ymax, 100) + plt.vlines( + -0.5, ymin=ymin, ymax=ymax, + **config.defaults['nyquist.circle_style']) + plt.text(-0.5, pos_y[label_pos], 1) + + # Label the frequencies of the points on the Nyquist curve if label_freq: ind = slice(None, None, label_freq) omega_sys = np.imag(splane_contour[np.real(splane_contour) == 0]) @@ -1888,7 +1986,6 @@ def _parse_linestyle(style_name, allow_false=False): prefix + 'Hz') # Label the axes - fig, ax = plt.gcf(), plt.gca() ax.set_xlabel("Real axis") ax.set_ylabel("Imaginary axis") ax.grid(color="lightgray") @@ -1897,13 +1994,22 @@ def _parse_linestyle(style_name, allow_false=False): lines, labels = _get_line_labels(ax) # Add legend if there is more than one system plotted - if len(labels) > 1: - ax.legend(lines, labels, loc=legend_loc) + if show_legend == True or (show_legend != False and len(labels) > 1): + with plt.rc_context(rcParams): + legend = ax.legend(lines, labels, loc=legend_loc) + else: + legend = None # Add the title - if title is None: - title = "Nyquist plot for " + ", ".join(labels) - fig.suptitle(title) + sysnames = [response.sysname for response in nyquist_responses] + if ax_user is None and title is None: + title = "Nyquist plot for " + ", ".join(sysnames) + _update_plot_title( + title, fig=fig, rcParams=rcParams, frame=title_frame) + elif ax_user is None: + _update_plot_title( + title, fig=fig, rcParams=rcParams, frame=title_frame, + use_existing=False) # Legacy return pocessing if plot is True or return_contour is not None: @@ -1913,81 +2019,7 @@ def _parse_linestyle(style_name, allow_false=False): # Return counts and (optionally) the contour we used return (counts, contours) if return_contour else counts - return out - - -# Internal function to add arrows to a curve -def _add_arrows_to_line2D( - axes, line, arrow_locs=[0.2, 0.4, 0.6, 0.8], - arrowstyle='-|>', arrowsize=1, dir=1, transform=None): - """ - Add arrows to a matplotlib.lines.Line2D at selected locations. - - Parameters: - ----------- - axes: Axes object as returned by axes command (or gca) - line: Line2D object as returned by plot command - arrow_locs: list of locations where to insert arrows, % of total length - arrowstyle: style of the arrow - arrowsize: size of the arrow - transform: a matplotlib transform instance, default to data coordinates - - Returns: - -------- - arrows: list of arrows - - Based on https://stackoverflow.com/questions/26911898/ - - """ - if not isinstance(line, mpl.lines.Line2D): - raise ValueError("expected a matplotlib.lines.Line2D object") - x, y = line.get_xdata(), line.get_ydata() - - arrow_kw = { - "arrowstyle": arrowstyle, - } - - color = line.get_color() - use_multicolor_lines = isinstance(color, np.ndarray) - if use_multicolor_lines: - raise NotImplementedError("multicolor lines not supported") - else: - arrow_kw['color'] = color - - linewidth = line.get_linewidth() - if isinstance(linewidth, np.ndarray): - raise NotImplementedError("multiwidth lines not supported") - else: - arrow_kw['linewidth'] = linewidth - - if transform is None: - transform = axes.transData - - # Compute the arc length along the curve - s = np.cumsum(np.sqrt(np.diff(x) ** 2 + np.diff(y) ** 2)) - - arrows = [] - for loc in arrow_locs: - n = np.searchsorted(s, s[-1] * loc) - - # Figure out what direction to paint the arrow - if dir == 1: - arrow_tail = (x[n], y[n]) - arrow_head = (np.mean(x[n:n + 2]), np.mean(y[n:n + 2])) - elif dir == -1: - # Orient the arrow in the other direction on the segment - arrow_tail = (x[n + 1], y[n + 1]) - arrow_head = (np.mean(x[n:n + 2]), np.mean(y[n:n + 2])) - else: - raise ValueError("unknown value for keyword 'dir'") - - p = mpl.patches.FancyArrowPatch( - arrow_tail, arrow_head, transform=transform, lw=0, - **arrow_kw) - axes.add_patch(p) - arrows.append(p) - return arrows - + return ControlPlot(out, ax, fig, legend=legend) # @@ -2050,7 +2082,8 @@ def _compute_curve_offset(resp, mask, max_offset): # # Gang of Four plot # -def gangof4_response(P, C, omega=None, Hz=False): +def gangof4_response( + P, C, omega=None, omega_limits=None, omega_num=None, Hz=False): """Compute the response of the "Gang of 4" transfer functions for a system. Generates a 2x2 frequency response for the "Gang of 4" sensitivity @@ -2059,9 +2092,21 @@ def gangof4_response(P, C, omega=None, Hz=False): Parameters ---------- P, C : LTI - Linear input/output systems (process and control) + Linear input/output systems (process and control). omega : array - Range of frequencies (list or bounds) in rad/sec + Range of frequencies (list or bounds) in rad/sec. + omega_limits : array_like of two values + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. Ignored if + data is not a list of systems. + omega_num : int + Number of samples to use for the frequeny range. Defaults to + config.defaults['freqplot.number_of_samples']. Ignored if data is + not a list of systems. + Hz : bool, optional + If True, when computing frequency limits automatically set + limits to full decades in Hz instead of rad/s. Returns ------- @@ -2089,8 +2134,8 @@ def gangof4_response(P, C, omega=None, Hz=False): # Select a default range if none is provided # TODO: This needs to be made more intelligent - if omega is None: - omega = _default_frequency_range((P, C, S), Hz=Hz) + omega, _ = _determine_omega_vector( + [P, C, S], omega, omega_limits, omega_num, Hz=Hz) # # bode_plot based implementation @@ -2111,12 +2156,77 @@ def gangof4_response(P, C, omega=None, Hz=False): return FrequencyResponseData( data, omega, outputs=['y', 'u'], inputs=['r', 'd'], - title=f"Gang of Four for P={P.name}, C={C.name}", plot_phase=False) + title=f"Gang of Four for P={P.name}, C={C.name}", + sysname=f"P={P.name}, C={C.name}", plot_phase=False) + + +def gangof4_plot( + *args, omega=None, omega_limits=None, omega_num=None, + Hz=False, **kwargs): + """Plot the response of the "Gang of 4" transfer functions for a system. + + Plots a 2x2 frequency response for the "Gang of 4" sensitivity + functions [T, PS; CS, S]. Can be called in one of two ways: + + gangof4_plot(response[, ...]) + gangof4_plot(P, C[, ...]) + + Parameters + ---------- + response : FrequencyPlotData + Gang of 4 frequency response from `gangof4_response`. + P, C : LTI + Linear input/output systems (process and control). + omega : array + Range of frequencies (list or bounds) in rad/sec. + omega_limits : array_like of two values + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. Ignored if + data is not a list of systems. + omega_num : int + Number of samples to use for the frequeny range. Defaults to + config.defaults['freqplot.number_of_samples']. Ignored if data is + not a list of systems. + Hz : bool, optional + If True, when computing frequency limits automatically set + limits to full decades in Hz instead of rad/s. + + Returns + ------- + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: 2x2 array of :class:`matplotlib.lines.Line2D` + objects for each line in the plot. The value of each array + entry is a list of Line2D objects in that subplot. + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + * cplt.legend: legend object(s) contained in the plot + + See :class:`ControlPlot` for more detailed information. + + """ + if len(args) == 1 and isinstance(arg, FrequencyResponseData): + if any([kw is not None + for kw in [omega, omega_limits, omega_num, Hz]]): + raise ValueError( + "omega, omega_limits, omega_num, Hz not allowed when " + "given a Gang of 4 response as first argument") + return args[0].plot(kwargs) + else: + if len(args) > 3: + raise TypeError( + f"expecting 2 or 3 positional arguments; received {len(args)}") + omega = omega if len(args) < 3 else args[2] + args = args[0:2] + return gangof4_response( + *args, omega=omega, omega_limits=omega_limits, + omega_num=omega_num, Hz=Hz).plot(**kwargs) -def gangof4_plot(P, C, omega=None, **kwargs): - """Legacy Gang of 4 plot; use gangof4_response().plot() instead.""" - return gangof4_response(P, C).plot(**kwargs) # # Singular values plot @@ -2134,15 +2244,9 @@ def singular_values_response( List of linear input/output systems (single system is OK). omega : array_like List of frequencies in rad/sec to be used for frequency response. - omega_limits : array_like of two values - Limits of the frequency vector to generate, in rad/s. - omega_num : int - Number of samples to plot. Default value (1000) set by - config.defaults['freqplot.number_of_samples']. Hz : bool, optional If True, when computing frequency limits automatically set - limits to full decades in Hz instead of rad/s. Omega is always - returned in rad/sec. + limits to full decades in Hz instead of rad/s. Returns ------- @@ -2150,6 +2254,20 @@ def singular_values_response( Frequency response with the number of outputs equal to the number of singular values in the response, and a single input. + Other Parameters + ---------------- + omega_limits : array_like of two values + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. + omega_num : int, optional + Number of samples to use for the frequeny range. Defaults to + config.defaults['freqplot.number_of_samples']. + + See Also + -------- + singular_values_plot + Examples -------- >>> omegas = np.logspace(-4, 1, 1000) @@ -2195,7 +2313,7 @@ def singular_values_response( def singular_values_plot( data, omega=None, *fmt, plot=None, omega_limits=None, omega_num=None, - title=None, legend_loc='center right', **kwargs): + ax=None, label=None, title=None, **kwargs): """Plot the singular values for a system. Plot the singular values as a function of frequency for a system or @@ -2217,36 +2335,51 @@ def singular_values_plot( Hz : bool If True, plot frequency in Hz (omega must be provided in rad/sec). Default value (False) set by config.defaults['freqplot.Hz']. - legend_loc : str, optional - For plots with multiple lines, a legend will be included in the - given location. Default is 'center right'. Use False to supress. **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional Additional keywords passed to `matplotlib` to specify line properties. Returns ------- - lines : array of Line2D - 1-D array of Line2D objects. The size of the array matches - the number of systems and the value of the array is a list of - Line2D objects for that system. - mag : ndarray (or list of ndarray if len(data) > 1)) - If plot=False, magnitude of the response (deprecated). - phase : ndarray (or list of ndarray if len(data) > 1)) - If plot=False, phase in radians of the response (deprecated). - omega : ndarray (or list of ndarray if len(data) > 1)) - If plot=False, frequency in rad/sec (deprecated). + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: 1-D array of :class:`matplotlib.lines.Line2D` objects. + The size of the array matches the number of systems and the + value of the array is a list of Line2D objects for that system. + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + * cplt.legend: legend object(s) contained in the plot + + See :class:`ControlPlot` for more detailed information. Other Parameters ---------------- + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. + color : matplotlib color spec + Color to use for singular values (or None for matplotlib default). grid : bool If True, plot grid lines on gain and phase plots. Default is set by `config.defaults['freqplot.grid']`. + label : str or array_like of str, optional + If present, replace automatically generated label(s) with the given + label(s). If sysdata is a list, strings should be specified for each + system. + legend_loc : int or str, optional + Include a legend in the given location. Default is 'center right', + with no legend for a single response. Use False to suppress legend. omega_limits : array_like of two values - Set limits for plotted frequency range. If Hz=True the limits - are in Hz otherwise in rad/s. - omega_num : int + Set limits for plotted frequency range. If Hz=True the limits are + in Hz otherwise in rad/s. Specifying ``omega`` as a list of two + elements is equivalent to providing ``omega_limits``. + omega_num : int, optional Number of samples to use for the frequeny range. Defaults to - config.defaults['freqplot.number_of_samples']. Ignore if data is + config.defaults['freqplot.number_of_samples']. Ignored if data is not a list of systems. plot : bool, optional (legacy) If given, `singular_values_plot` returns the legacy return @@ -2254,18 +2387,45 @@ def singular_values_plot( the values with no plot. rcParams : dict Override the default parameters used for generating plots. - Default is set up config.default['freqplot.rcParams']. + Default is set up config.default['ctrlplot.rcParams']. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on an + axis or ``legend_loc`` or ``legend_map`` has been specified. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). + title_frame : str, optional + Set the frame of reference used to center the plot title. If set to + 'axes' (default), the horizontal position of the title will + centered relative to the axes. If set to 'figure', it will be + centered with respect to the figure (faster execution). + + See Also + -------- + singular_values_response + + Notes + ----- + 1. If plot==False, the following legacy values are returned: + * mag : ndarray (or list of ndarray if len(data) > 1)) + Magnitude of the response (deprecated). + * phase : ndarray (or list of ndarray if len(data) > 1)) + Phase in radians of the response (deprecated). + * omega : ndarray (or list of ndarray if len(data) > 1)) + Frequency in rad/sec (deprecated). """ # Keyword processing + color = kwargs.pop('color', None) dB = config._get_param( 'freqplot', 'dB', kwargs, _freqplot_defaults, pop=True) Hz = config._get_param( 'freqplot', 'Hz', kwargs, _freqplot_defaults, pop=True) grid = config._get_param( 'freqplot', 'grid', kwargs, _freqplot_defaults, pop=True) - freqplot_rcParams = config._get_param( - 'freqplot', 'rcParams', kwargs, _freqplot_defaults, pop=True) + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + title_frame = config._get_param( + 'freqplot', 'title_frame', kwargs, _freqplot_defaults, pop=True) # If argument was a singleton, turn it into a tuple data = data if isinstance(data, (list, tuple)) else (data,) @@ -2290,11 +2450,14 @@ def singular_values_plot( responses = data + # Process label keyword + line_labels = _process_line_labels(label, len(data)) + # Process (legacy) plot keyword if plot is not None: warnings.warn( "`singular_values_plot` return values of sigma, omega is " - "deprecated; use singular_values_response()", DeprecationWarning) + "deprecated; use singular_values_response()", FutureWarning) # Warn the user if we got past something that is not real-valued if any([not np.allclose(np.imag(response.fresp[:, 0, :]), 0) @@ -2312,31 +2475,14 @@ def singular_values_plot( else: return sigmas, omegas - fig = plt.gcf() # get current figure (or create new one) - ax_sigma = None # axes for plotting singular values - - # Get the current axes if they already exist - for ax in fig.axes: - if ax.get_label() == 'control-sigma': - ax_sigma = ax - - # If no axes present, create them from scratch - if ax_sigma is None: - if len(fig.axes) > 0: - # Create a new figure to avoid overwriting in the old one - fig = plt.figure() + fig, ax_sigma = _process_ax_keyword( + ax, shape=(1, 1), squeeze=True, rcParams=rcParams) + ax_sigma.set_label('control-sigma') # TODO: deprecate? + legend_loc, _, show_legend = _process_legend_keywords( + kwargs, None, 'center right') - with plt.rc_context(_freqplot_rcParams): - ax_sigma = plt.subplot(111, label='control-sigma') - - # Handle color cycle manually as all singular values - # of the same systems are expected to be of the same color - color_cycle = plt.rcParams['axes.prop_cycle'].by_key()['color'] - color_offset = 0 - if len(ax_sigma.lines) > 0: - last_color = ax_sigma.lines[-1].get_color() - if last_color in color_cycle: - color_offset = color_cycle.index(last_color) + 1 + # Get color offset for first (new) line to be drawn + color_offset, color_cycle = _get_color_offset(ax_sigma) # Create a list of lines for the output out = np.empty(len(data), dtype=object) @@ -2351,29 +2497,29 @@ def singular_values_plot( else: nyq_freq = None - # See if the color was specified, otherwise rotate - if kwargs.get('color', None) or any( - [isinstance(arg, str) and - any([c in arg for c in "bgrcmykw#"]) for arg in fmt]): - color_arg = {} # color set by *fmt, **kwargs - else: - color_arg = {'color': color_cycle[ - (idx_sys + color_offset) % len(color_cycle)]} + # Determine the color to use for this response + color = _get_color( + color, fmt=fmt, offset=color_offset + idx_sys, + color_cycle=color_cycle) + + # To avoid conflict with *fmt, only pass color kw if non-None + color_arg = {} if color is None else {'color': color} # Decide on the system name sysname = response.sysname if response.sysname is not None \ else f"Unknown-{idx_sys}" + # Get the label to use for the line + label = sysname if line_labels is None else line_labels[idx_sys] + # Plot the data if dB: - with plt.rc_context(freqplot_rcParams): - out[idx_sys] = ax_sigma.semilogx( - omega, 20 * np.log10(sigma), *fmt, - label=sysname, **color_arg, **kwargs) + out[idx_sys] = ax_sigma.semilogx( + omega, 20 * np.log10(sigma), *fmt, + label=label, **color_arg, **kwargs) else: - with plt.rc_context(freqplot_rcParams): - out[idx_sys] = ax_sigma.loglog( - omega, sigma, label=sysname, *fmt, **color_arg, **kwargs) + out[idx_sys] = ax_sigma.loglog( + omega, sigma, label=label, *fmt, **color_arg, **kwargs) # Plot the Nyquist frequency if nyq_freq is not None: @@ -2388,24 +2534,28 @@ def singular_values_plot( # Add a grid to the plot + labeling if grid: ax_sigma.grid(grid, which='both') - with plt.rc_context(freqplot_rcParams): - ax_sigma.set_ylabel( - "Singular Values [dB]" if dB else "Singular Values") - ax_sigma.set_xlabel("Frequency [Hz]" if Hz else "Frequency [rad/sec]") + + ax_sigma.set_ylabel( + "Singular Values [dB]" if dB else "Singular Values") + ax_sigma.set_xlabel("Frequency [Hz]" if Hz else "Frequency [rad/sec]") # List of systems that are included in this plot lines, labels = _get_line_labels(ax_sigma) # Add legend if there is more than one system plotted - if len(labels) > 1 and legend_loc is not False: - with plt.rc_context(freqplot_rcParams): - ax_sigma.legend(lines, labels, loc=legend_loc) + if show_legend == True or (show_legend != False and len(labels) > 1): + with plt.rc_context(rcParams): + legend = ax_sigma.legend(lines, labels, loc=legend_loc) + else: + legend = None # Add the title - if title is None: - title = "Singular values for " + ", ".join(labels) - with plt.rc_context(freqplot_rcParams): - fig.suptitle(title) + if ax is None: + if title is None: + title = "Singular values for " + ", ".join(labels) + _update_plot_title( + title, fig=fig, rcParams=rcParams, frame=title_frame, + use_existing=False) # Legacy return processing if plot is not None: @@ -2414,13 +2564,13 @@ def singular_values_plot( else: return sigmas, omegas - return out + return ControlPlot(out, ax_sigma, fig, legend=legend) # # Utility functions # # This section of the code contains some utility functions for -# generating frequency domain plots +# generating frequency domain plots. # @@ -2434,24 +2584,32 @@ def _determine_omega_vector(syslist, omega_in, omega_limits, omega_num, on omega_num points according to a default logic defined by _default_frequency_range and tailored for the list of systems syslist, and omega_range_given is set to False. + If omega_in is None but omega_limits is an array-like of 2 elements, then omega_out is computed with the function np.logspace on omega_num points within the interval [min, max] = [omega_limits[0], omega_limits[1]], and omega_range_given is set to True. - If omega_in is not None, then omega_out is set to omega_in, - and omega_range_given is set to True + + If omega_in is a list or tuple of length 2, it is interpreted as a + range and handled like omega_limits. If omega_in is a list or tuple of + length 3, it is interpreted a range plus number of points and handled + like omega_limits and omega_num. + + If omega_in is an array or a list/tuple of length greater than + two, then omega_out is set to omega_in (as an array), and + omega_range_given is set to True Parameters ---------- syslist : list of LTI - List of linear input/output systems (single system is OK) + List of linear input/output systems (single system is OK). omega_in : 1D array_like or None - Frequency range specified by the user + Frequency range specified by the user. omega_limits : 1D array_like or None - Frequency limits specified by the user + Frequency limits specified by the user. omega_num : int - Number of points to be used for the frequency - range (if the frequency range is not user-specified) + Number of points to be used for the frequency range (if the + frequency range is not user-specified). Hz : bool, optional If True, the limits (first and last value) of the frequencies are set to full decades in Hz so it fits plotting with logarithmic @@ -2460,14 +2618,22 @@ def _determine_omega_vector(syslist, omega_in, omega_limits, omega_num, Returns ------- omega_out : 1D array - Frequency range to be used + Frequency range to be used. omega_range_given : bool True if the frequency range was specified by the user, either through omega_in or through omega_limits. False if both omega_in and omega_limits are None. + """ - omega_range_given = True + # Handle the special case of a range of frequencies + if omega_in is not None and omega_limits is not None: + warnings.warn( + "omega and omega_limits both specified; ignoring limits") + elif isinstance(omega_in, (list, tuple)) and len(omega_in) == 2: + omega_limits = omega_in + omega_in = None + omega_range_given = True if omega_in is None: if omega_limits is None: omega_range_given = False @@ -2543,6 +2709,15 @@ def _default_frequency_range(syslist, Hz=None, number_of_samples=None, syslist = (syslist,) for sys in syslist: + # For FRD systems, just use the response frequencies + if isinstance(sys, FrequencyResponseData): + # Add the min and max frequency, minus periphery decades + # (keeps frequency ranges from artificially expanding) + features = np.concatenate([features, np.array([ + np.min(sys.omega) * 10**feature_periphery_decades, + np.max(sys.omega) / 10**feature_periphery_decades])]) + continue + try: # Add new features to the list if sys.isctime(): @@ -2557,7 +2732,8 @@ def _default_frequency_range(syslist, Hz=None, number_of_samples=None, # TODO: What distance to the Nyquist frequency is appropriate? freq_interesting.append(fn * 0.9) - features_ = np.concatenate((sys.poles(), sys.zeros())) + features_ = np.concatenate( + (np.abs(sys.poles()), np.abs(sys.zeros()))) # Get rid of poles and zeros on the real axis (imag==0) # * origin and real < 0 # * at 1.: would result in omega=0. (logaritmic plot!) @@ -2572,8 +2748,9 @@ def _default_frequency_range(syslist, Hz=None, number_of_samples=None, # TODO raise NotImplementedError( "type of system in not implemented now") - features = np.concatenate((features, features_)) + features = np.concatenate([features, features_]) except NotImplementedError: + # Don't add any features for anything we don't understand pass # Make sure there is at least one point in the range @@ -2605,28 +2782,6 @@ def _default_frequency_range(syslist, Hz=None, number_of_samples=None, return omega -# Get labels for all lines in an axes -def _get_line_labels(ax, use_color=True): - labels, lines = [], [] - last_color, counter = None, 0 # label unknown systems - for i, line in enumerate(ax.get_lines()): - label = line.get_label() - if use_color and label.startswith("Unknown"): - label = f"Unknown-{counter}" - if last_color is None: - last_color = line.get_color() - elif last_color != line.get_color(): - counter += 1 - last_color = line.get_color() - elif label[0] == '_': - continue - - if label not in labels: - lines.append(line) - labels.append(label) - - return lines, labels - # # Utility functions to create nice looking labels (KLD 5/23/11) # diff --git a/control/grid.py b/control/grid.py index ef9995947..54a1940c9 100644 --- a/control/grid.py +++ b/control/grid.py @@ -74,7 +74,7 @@ def __call__(self, transform_xy, x1, y1, x2, y2): return lon_min, lon_max, lat_min, lat_max -def sgrid(scaling=None): +def sgrid(subplot=(1, 1, 1), scaling=None): # From matplotlib demos: # https://matplotlib.org/gallery/axisartist/demo_curvelinear_grid.html # https://matplotlib.org/gallery/axisartist/demo_floating_axis.html @@ -101,11 +101,10 @@ def sgrid(scaling=None): # Set up an axes with a specialized grid helper fig = plt.gcf() - ax = SubplotHost(fig, 1, 1, 1, grid_helper=grid_helper) + ax = SubplotHost(fig, *subplot, grid_helper=grid_helper) # make ticklabels of right invisible, and top axis visible. - visible = True - ax.axis[:].major_ticklabels.set_visible(visible) + ax.axis[:].major_ticklabels.set_visible(True) ax.axis[:].major_ticks.set_visible(False) ax.axis[:].invert_ticklabel_direction() ax.axis[:].major_ticklabels.set_color('gray') @@ -141,18 +140,6 @@ def sgrid(scaling=None): return ax, fig -# Utility function used by all grid code -def _final_setup(ax, scaling=None): - ax.set_xlabel('Real') - ax.set_ylabel('Imaginary') - ax.axhline(y=0, color='black', lw=0.25) - ax.axvline(x=0, color='black', lw=0.25) - - # Set up the scaling for the axes - scaling = 'equal' if scaling is None else scaling - plt.axis(scaling) - - # If not grid is given, at least separate stable/unstable regions def nogrid(dt=None, ax=None, scaling=None): fig = plt.gcf() @@ -226,3 +213,15 @@ def zgrid(zetas=None, wns=None, ax=None, scaling=None): _final_setup(ax, scaling=scaling) return ax, fig + + +# Utility function used by all grid code +def _final_setup(ax, scaling=None): + ax.set_xlabel('Real') + ax.set_ylabel('Imaginary') + ax.axhline(y=0, color='black', lw=0.25) + ax.axvline(x=0, color='black', lw=0.25) + + # Set up the scaling for the axes + scaling = 'equal' if scaling is None else scaling + plt.axis(scaling) diff --git a/control/iosys.py b/control/iosys.py index fbd5c1dba..9092b672b 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -6,10 +6,12 @@ # FrequencyResponseData, InterconnectedSystem and other similar classes # that allow naming of signals. -import numpy as np +import re from copy import deepcopy from warnings import warn -import re + +import numpy as np + from . import config __all__ = ['InputOutputSystem', 'issiso', 'timebase', 'common_timebase', @@ -54,7 +56,7 @@ class InputOutputSystem(object): Description of the system inputs. This can be given as an integer count or 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 `s[i]` (where `s` is given by the `input_prefix` parameter and + form 's[i]' (where 's' is given by the `input_prefix` parameter and has default value 'u'). If this parameter is not given or given as `None`, the relevant quantity will be determined when possible based on other information provided to functions using the system. @@ -366,6 +368,51 @@ def find_states(self, name_list): lambda self: list(self.state_index.keys()), # getter set_states) # setter + # TODO: add dict as a means to selective change names? [GH #1019] + def update_names(self, **kwargs): + """update_names([name, inputs, outputs, states]) + + Update signal and system names for an I/O system. + + Parameters + ---------- + name : str, optional + New system name. + inputs : list of str, int, or None, optional + List of strings that name the individual input signals. If + given as an integer or None, signal names default to the form + `u[i]`. See :class:`InputOutputSystem` for more information. + outputs : list of str, int, or None, optional + Description of output signals; defaults to `y[i]`. + states : int, list of str, int, or None, optional + Description of system states; defaults to `x[i]`. + + """ + self.name = kwargs.pop('name', self.name) + if 'inputs' in kwargs: + ninputs, input_index = _process_signal_list( + kwargs.pop('inputs'), prefix=kwargs.pop('input_prefix', 'u')) + if self.ninputs and self.ninputs != ninputs: + raise ValueError("number of inputs does not match system size") + self.input_index = input_index + if 'outputs' in kwargs: + noutputs, output_index = _process_signal_list( + kwargs.pop('outputs'), prefix=kwargs.pop('output_prefix', 'y')) + if self.noutputs and self.noutputs != noutputs: + raise ValueError("number of outputs does not match system size") + self.output_index = output_index + if 'states' in kwargs: + nstates, state_index = _process_signal_list( + kwargs.pop('states'), prefix=kwargs.pop('state_prefix', 'x')) + if self.nstates != nstates: + raise ValueError("number of states does not match system size") + self.state_index = state_index + + # Make sure we processed all of the arguments + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) + + def isctime(self, strict=False): """ Check to see if a system is a continuous-time system. @@ -374,7 +421,7 @@ def isctime(self, strict=False): ---------- sys : Named I/O system System to be checked - strict: bool, optional + strict : bool, optional If strict is True, make sure that timebase is not None. Default is False. """ @@ -389,7 +436,7 @@ def isdtime(self, strict=False): Parameters ---------- - strict: bool, optional + strict : bool, optional If strict is True, make sure that timebase is not None. Default is False. """ @@ -419,7 +466,7 @@ def issiso(sys, strict=False): ---------- sys : I/O or LTI system System to be checked - strict: bool (default = False) + strict : bool (default = False) If strict is True, do not treat scalars as SISO """ if isinstance(sys, (int, float, complex, np.number)) and not strict: @@ -437,7 +484,21 @@ def timebase(sys, strict=True): dt = timebase(sys) returns the timebase for a system 'sys'. If the strict option is - set to False, dt = True will be returned as 1. + set to `True`, dt = True will be returned as 1. + + Parameters + ---------- + sys : InputOutputSystem or float + System whose timebase is to be determined. + strict : bool, optional + Whether to implement strict checking. If set to `True` (default), + a float will always be returned (dt = `True` will be returned as 1). + + Returns + ------- + dt : timebase + Timebase for the system (0 = continuous time, `None` = unspecified). + """ # System needs to be either a constant or an I/O or LTI system if isinstance(sys, (int, float, complex, np.number)): @@ -446,9 +507,9 @@ def timebase(sys, strict=True): raise ValueError("Timebase not defined") # Return the sample time, with converstion to float if strict is false - if (sys.dt == None): + if sys.dt == None: return None - elif (strict): + elif strict: return float(sys.dt) return sys.dt @@ -459,12 +520,12 @@ def common_timebase(dt1, dt2): Parameters ---------- - dt1, dt2: number or system with a 'dt' attribute (e.g. TransferFunction + dt1, dt2 : number or system with a 'dt' attribute (e.g. TransferFunction or StateSpace system) Returns ------- - dt: number + dt : number The common timebase of dt1 and dt2, as specified in :ref:`conventions-ref`. @@ -513,7 +574,7 @@ def isdtime(sys=None, strict=False, dt=None): System to be checked. dt : None or number, optional Timebase to be checked. - strict: bool, default=False + strict : bool, default=False If strict is True, make sure that timebase is not None. """ @@ -545,7 +606,7 @@ def isctime(sys=None, dt=None, strict=False): System to be checked. dt : None or number, optional Timebase to be checked. - strict: bool (default = False) + strict : bool (default = False) If strict is True, make sure that timebase is not None. """ @@ -823,7 +884,6 @@ def _process_labels(labels, name, default): # This function returns the subsystem index, a list of indices for the # system signals, and the gain to use for that set of signals. # -import re def _parse_spec(syslist, spec, signame, dictname=None): """Parse a signal specification, returning system and signal index.""" diff --git a/control/lti.py b/control/lti.py index cccb44a63..e9455aed5 100644 --- a/control/lti.py +++ b/control/lti.py @@ -120,7 +120,8 @@ def frequency_response(self, omega=None, squeeze=None): response = self(s) return FrequencyResponseData( response, omega, return_magphase=True, squeeze=squeeze, - dt=self.dt, sysname=self.name, plot_type='bode') + dt=self.dt, sysname=self.name, inputs=self.input_labels, + outputs=self.output_labels, plot_type='bode') def dcgain(self): """Return the zero-frequency gain""" @@ -209,12 +210,12 @@ def poles(sys): Parameters ---------- - sys: StateSpace or TransferFunction + sys : StateSpace or TransferFunction Linear system Returns ------- - poles: ndarray + poles : ndarray Array that contains the system's poles. See Also @@ -234,7 +235,7 @@ def zeros(sys): Parameters ---------- - sys: StateSpace or TransferFunction + sys : StateSpace or TransferFunction Linear system Returns @@ -329,7 +330,7 @@ def evalfr(sys, x, squeeze=None): Parameters ---------- - sys: StateSpace or TransferFunction + sys : StateSpace or TransferFunction Linear system x : complex scalar or 1D array_like Complex frequency(s) @@ -385,16 +386,18 @@ def frequency_response( sysdata : LTI system or list of LTI systems Linear system(s) for which frequency response is computed. omega : float or 1D array_like, optional - A list of frequencies in radians/sec at which the system should be - evaluated. The list can be either a Python list or a numpy array - and will be sorted before evaluation. If None (default), a common - set of frequencies that works across all given systems is computed. + Frequencies in radians/sec at which the system should be + evaluated. Can be a single frequency or array of frequencies, which + will be sorted before evaluation. If None (default), a common set + of frequencies that works across all given systems is computed. omega_limits : array_like of two values, optional - Limits to the range of frequencies, in rad/sec. Ignored if - omega is provided, and auto-generated if omitted. + Limits to the range of frequencies, in rad/sec. Specifying + ``omega`` as a list of two elements is equivalent to providing + ``omega_limits``. Ignored if omega is provided. omega_num : int, optional - Number of frequency samples to plot. Defaults to - config.defaults['freqplot.number_of_samples']. + Number of frequency samples at which to compute the response. + Defaults to config.defaults['freqplot.number_of_samples']. Ignored + if omega is provided. Returns ------- @@ -472,6 +475,7 @@ def frequency_response( #>>> # s = 0.1i, i, 10i. """ + from .frdata import FrequencyResponseData from .freqplot import _determine_omega_vector # Process keyword arguments @@ -486,13 +490,18 @@ def frequency_response( responses = [] for sys_ in syslist: - # Add the Nyquist frequency for discrete time systems - omega_sys = omega_syslist.copy() - if sys_.isdtime(strict=True): - nyquistfrq = math.pi / sys_.dt - if not omega_range_given: - # Limit up to the Nyquist frequency - omega_sys = omega_sys[omega_sys < nyquistfrq] + if isinstance(sys_, FrequencyResponseData) and sys_.ifunc is None and \ + not omega_range_given: + omega_sys = sys_.omega # use system properties + else: + omega_sys = omega_syslist.copy() # use common omega vector + + # Add the Nyquist frequency for discrete time systems + if sys_.isdtime(strict=True): + nyquistfrq = math.pi / sys_.dt + if not omega_range_given: + # Limit up to the Nyquist frequency + omega_sys = omega_sys[omega_sys < nyquistfrq] # Compute the frequency response responses.append(sys_.frequency_response(omega_sys, squeeze=squeeze)) @@ -505,14 +514,25 @@ def frequency_response( # Alternative name (legacy) def freqresp(sys, omega): - """Legacy version of frequency_response.""" - warn("freqresp is deprecated; use frequency_response", DeprecationWarning) + """Legacy version of frequency_response. + + .. deprecated:: 0.9.0 + This function will be removed in a future version of python-control. + Use `frequency_response` instead. + + """ + warn("freqresp() is deprecated; use frequency_response()", FutureWarning) return frequency_response(sys, omega) def dcgain(sys): """Return the zero-frequency (or DC) gain of the given system. + Parameters + ---------- + sys : LTI + System for which the zero-frequency gain is computed. + Returns ------- gain : ndarray @@ -524,7 +544,7 @@ def dcgain(sys): -------- >>> G = ct.tf([1], [1, 2]) >>> ct.dcgain(G) # doctest: +SKIP - 0.5 + np.float(0.5) """ return sys.dcgain() @@ -535,11 +555,11 @@ def bandwidth(sys, dbdrop=-3): Parameters ---------- - sys: StateSpace or TransferFunction - Linear system + sys : StateSpace or TransferFunction + Linear system for which the bandwidth should be computed. dbdrop : float, optional By how much the gain drop in dB (default = -3) that defines the - bandwidth. Should be a negative scalar + bandwidth. Should be a negative scalar. Returns ------- diff --git a/control/mateqn.py b/control/mateqn.py index 05b47ffae..b73abdfcc 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -341,7 +341,7 @@ def dlyap(A, Q, C=None, E=None, method=None): # def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, - A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): + _As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E"): """Solves the continuous-time algebraic Riccati equation. X, L, G = care(A, B, Q, R=None) solves @@ -375,6 +375,9 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, 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'. + stabilizing : bool, optional + If `method` is 'slycot', unstabilized eigenvalues will be returned + in the initial elements of `L`. Not supported for 'scipy'. Returns ------- @@ -404,10 +407,10 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, m = B.shape[1] # Check to make sure input matrices are the right shape and type - _check_shape(A_s, A, n, n, square=True) - _check_shape(B_s, B, n, m) - _check_shape(Q_s, Q, n, n, square=True, symmetric=True) - _check_shape(R_s, R, m, m, square=True, symmetric=True) + _check_shape(_As, A, n, n, square=True) + _check_shape(_Bs, B, n, m) + _check_shape(_Qs, Q, n, n, square=True, symmetric=True) + _check_shape(_Rs, R, m, m, square=True, symmetric=True) # Solve the standard algebraic Riccati equation if S is None and E is None: @@ -454,8 +457,8 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2) # Check to make sure input matrices are the right shape and type - _check_shape(E_s, E, n, n, square=True) - _check_shape(S_s, S, n, m) + _check_shape(_Es, E, n, n, square=True) + _check_shape(_Ss, S, n, m) # See if we should solve this using SciPy if method == 'scipy': @@ -494,7 +497,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, return _ssmatrix(X), L, _ssmatrix(G) def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, - A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): + _As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E"): """Solves the discrete-time algebraic Riccati equation. @@ -529,6 +532,9 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, 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'. + stabilizing : bool, optional + If `method` is 'slycot', unstabilized eigenvalues will be returned + in the initial elements of `L`. Not supported for 'scipy'. Returns ------- @@ -558,14 +564,14 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, m = B.shape[1] # Check to make sure input matrices are the right shape and type - _check_shape(A_s, A, n, n, square=True) - _check_shape(B_s, B, n, m) - _check_shape(Q_s, Q, n, n, square=True, symmetric=True) - _check_shape(R_s, R, m, m, square=True, symmetric=True) + _check_shape(_As, A, n, n, square=True) + _check_shape(_Bs, B, n, m) + _check_shape(_Qs, Q, n, n, square=True, symmetric=True) + _check_shape(_Rs, R, m, m, square=True, symmetric=True) if E is not None: - _check_shape(E_s, E, n, n, square=True) + _check_shape(_Es, E, n, n, square=True) if S is not None: - _check_shape(S_s, S, n, m) + _check_shape(_Ss, S, n, m) # Figure out how to solve the problem if method == 'scipy': diff --git a/control/matlab/wrappers.py b/control/matlab/wrappers.py index 0384215a8..153342096 100644 --- a/control/matlab/wrappers.py +++ b/control/matlab/wrappers.py @@ -197,19 +197,19 @@ def _parse_freqplot_args(*args): # TODO: rewrite to call root_locus_map, without using legacy plot keyword def rlocus(*args, **kwargs): - """rlocus(sys[, klist, xlim, ylim, ...]) + """rlocus(sys[, gains, xlim, ylim, ...]) Root locus diagram. Calculate the root locus by finding the roots of 1 + k * G(s) where G is a linear system with transfer function num(s)/den(s) and each k is - an element of kvect. + an element of gains. Parameters ---------- sys : LTI object Linear input/output systems (SISO only, for now). - kvect : array_like, optional + gains : array_like, optional Gains to use in computing plot of closed-loop poles. xlim : tuple or list, optional Set limits of x axis, normally with tuple @@ -224,7 +224,7 @@ def rlocus(*args, **kwargs): Closed-loop root locations, arranged in which each row corresponds to a gain in gains. gains : ndarray - Gains used. Same as kvect keyword argument if provided. + Gains used. Same as gains keyword argument if provided. Notes ----- diff --git a/control/modelsimp.py b/control/modelsimp.py index cbaf242c3..d2eadc4c3 100644 --- a/control/modelsimp.py +++ b/control/modelsimp.py @@ -43,13 +43,15 @@ # External packages and modules import numpy as np import warnings -from .exception import ControlSlycot, ControlMIMONotImplemented, \ - ControlDimension +from .exception import ControlSlycot, ControlArgument, ControlDimension from .iosys import isdtime, isctime from .statesp import StateSpace from .statefbk import gram +from .timeresp import TimeResponseData -__all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal'] +__all__ = ['hankel_singular_values', 'balanced_reduction', 'model_reduction', + 'minimal_realization', 'eigensys_realization', 'markov', 'hsvd', + 'balred', 'modred', 'minreal', 'era'] # Hankel Singular Value Decomposition @@ -57,7 +59,7 @@ # The following returns the Hankel singular values, which are singular values # of the matrix formed by multiplying the controllability and observability # Gramians -def hsvd(sys): +def hankel_singular_values(sys): """Calculate the Hankel singular values. Parameters @@ -87,7 +89,7 @@ def hsvd(sys): >>> G = ct.tf2ss([1], [1, 2]) >>> H = ct.hsvd(G) >>> H[0] - 0.25 + np.float64(0.25) """ # TODO: implement for discrete time systems @@ -106,25 +108,26 @@ def hsvd(sys): return hsv[::-1] -def modred(sys, ELIM, method='matchdc'): - """ +def model_reduction(sys, ELIM, method='matchdc'): + """Model reduction by state elimination. + Model reduction of `sys` by eliminating the states in `ELIM` using a given method. Parameters ---------- - sys: StateSpace - Original system to reduce - ELIM: array - Vector of states to eliminate - method: string - Method of removing states in `ELIM`: either ``'truncate'`` or - ``'matchdc'``. + sys : StateSpace + Original system to reduce. + ELIM : array + Vector of states to eliminate. + method : string + Method of removing states in `ELIM`: either 'truncate' or + 'matchdc'. Returns ------- - rsys: StateSpace - A reduced order model + rsys : StateSpace + A reduced order model. Raises ------ @@ -216,8 +219,9 @@ def modred(sys, ELIM, method='matchdc'): return rsys -def balred(sys, orders, method='truncate', alpha=None): +def balanced_reduction(sys, orders, method='truncate', alpha=None): """Balanced reduced order model of sys of a given order. + States are eliminated based on Hankel singular value. If sys has unstable modes, they are removed, the balanced realization is done on the stable part, then @@ -229,14 +233,14 @@ def balred(sys, orders, method='truncate', alpha=None): Parameters ---------- - sys: StateSpace - Original system to reduce - orders: integer or array of integer + sys : StateSpace + Original system to reduce. + orders : integer or array of integer Desired order of reduced order model (if a vector, returns a vector - of systems) - method: string - Method of removing states, either ``'truncate'`` or ``'matchdc'``. - alpha: float + of systems). + method : string + Method of removing states, either ``'truncate'`` or ``'matchdc'``.. + alpha : float Redefines the stability boundary for eigenvalues of the system matrix A. By default for continuous-time systems, alpha <= 0 defines the stability boundary for the real part of A's eigenvalues @@ -246,7 +250,7 @@ def balred(sys, orders, method='truncate', alpha=None): Returns ------- - rsys: StateSpace + rsys : StateSpace A reduced order model or a list of reduced order models if orders is a list. @@ -340,8 +344,9 @@ def balred(sys, orders, method='truncate', alpha=None): return rsys -def minreal(sys, tol=None, verbose=True): - ''' +def minimal_realization(sys, tol=None, verbose=True): + """ Eliminate uncontrollable or unobservable states. + Eliminates uncontrollable or unobservable states in state-space models or cancelling pole-zero pairs in transfer functions. The output sysr has minimal order and the same response @@ -349,18 +354,19 @@ def minreal(sys, tol=None, verbose=True): Parameters ---------- - sys: StateSpace or TransferFunction - Original system - tol: real - Tolerance - verbose: bool - Print results if True + sys : StateSpace or TransferFunction + Original system. + tol : real + Tolerance. + verbose : bool + Print results if True. Returns ------- - rsys: StateSpace or TransferFunction - Cleaned model - ''' + rsys : StateSpace or TransferFunction + Cleaned model. + + """ sysr = sys.minreal(tol) if verbose: print("{nstates} states have been removed from the model".format( @@ -368,75 +374,190 @@ def minreal(sys, tol=None, verbose=True): return sysr -def era(YY, m, n, nin, nout, r): - """Calculate an ERA model of order `r` based on the impulse-response data - `YY`. +def _block_hankel(Y, m, n): + """Create a block Hankel matrix from impulse response.""" + q, p, _ = Y.shape + YY = Y.transpose(0,2,1) # transpose for reshape + + H = np.zeros((q*m,p*n)) + + for r in range(m): + # shift and add row to Hankel matrix + new_row = YY[:,r:r+n,:] + H[q*r:q*(r+1),:] = new_row.reshape((q,p*n)) + + return H + + +def eigensys_realization(arg, r, m=None, n=None, dt=True, transpose=False): + r"""eigensys_realization(YY, r) + + Calculate ERA model of order `r` based on impulse-response data `YY`. + + This function computes a discrete time system + + .. math:: + + x[k+1] &= A x[k] + B u[k] \\\\ + y[k] &= C x[k] + D u[k] + + for a given impulse-response data (see [1]_). + + The function can be called with 2 arguments: - .. note:: This function is not implemented yet. + * ``sysd, S = eigensys_realization(data, r)`` + * ``sysd, S = eigensys_realization(YY, r)`` + + where `data` is a `TimeResponseData` object, `YY` is a 1D or 3D + array, and r is an integer. Parameters ---------- - YY: array - `nout` x `nin` dimensional impulse-response data - m: integer - Number of rows in Hankel matrix - n: integer - Number of columns in Hankel matrix - nin: integer - Number of input variables - nout: integer - Number of output variables - r: integer - Order of model + YY : array_like + Impulse response from which the StateSpace model is estimated, 1D + or 3D array. + data : TimeResponseData + Impulse response from which the StateSpace model is estimated. + r : integer + Order of model. + m : integer, optional + Number of rows in Hankel matrix. Default is 2*r. + n : integer, optional + Number of columns in Hankel matrix. Default is 2*r. + dt : True or float, optional + True indicates discrete time with unspecified sampling time and a + positive float is discrete time with the specified sampling time. + It can be used to scale the StateSpace model in order to match the + unit-area impulse response of python-control. Default is True. + transpose : bool, optional + Assume that input data is transposed relative to the standard + :ref:`time-series-convention`. For TimeResponseData this parameter + is ignored. Default is False. Returns ------- - sys: StateSpace - A reduced order model sys=ss(Ar,Br,Cr,Dr) + sys : StateSpace + A reduced order model sys=StateSpace(Ar,Br,Cr,Dr,dt). + S : array + Singular values of Hankel matrix. Can be used to choose a good r + value. + + References + ---------- + .. [1] Samet Oymak and Necmiye Ozay, Non-asymptotic Identification of + LTI Systems from a Single Trajectory. + https://arxiv.org/abs/1806.05722 Examples -------- - >>> rsys = era(YY, m, n, nin, nout, r) # doctest: +SKIP + >>> T = np.linspace(0, 10, 100) + >>> _, YY = ct.impulse_response(ct.tf([1], [1, 0.5], True), T) + >>> sysd, _ = ct.eigensys_realization(YY, r=1) + >>> T = np.linspace(0, 10, 100) + >>> response = ct.impulse_response(ct.tf([1], [1, 0.5], True), T) + >>> sysd, _ = ct.eigensys_realization(response, r=1) """ - raise NotImplementedError('This function is not implemented yet.') - - -def markov(Y, U, m=None, transpose=False): - """Calculate the first `m` Markov parameters [D CB CAB ...] - from input `U`, output `Y`. + if isinstance(arg, TimeResponseData): + YY = np.array(arg.outputs, ndmin=3) + if arg.transpose: + YY = np.transpose(YY) + else: + YY = np.array(arg, ndmin=3) + if transpose: + YY = np.transpose(YY) + + q, p, l = YY.shape - This function computes the Markov parameters for a discrete time system + if m is None: + m = 2*r + if n is None: + n = 2*r + + if m*q < r or n*p < r: + raise ValueError("Hankel parameters are to small") + + if (l-1) < m+n: + raise ValueError("not enough data for requested number of parameters") + + H = _block_hankel(YY[:,:,1:], m, n+1) # Hankel matrix (q*m, p*(n+1)) + Hf = H[:,:-p] # first p*n columns of H + Hl = H[:,p:] # last p*n columns of H + + U,S,Vh = np.linalg.svd(Hf, True) + Ur =U[:,0:r] + Vhr =Vh[0:r,:] + + # balanced realizations + Sigma_inv = np.diag(1./np.sqrt(S[0:r])) + Ar = Sigma_inv @ Ur.T @ Hl @ Vhr.T @ Sigma_inv + Br = Sigma_inv @ Ur.T @ Hf[:,0:p]*dt # dt scaling for unit-area impulse + Cr = Hf[0:q,:] @ Vhr.T @ Sigma_inv + Dr = YY[:,:,0] + + return StateSpace(Ar,Br,Cr,Dr,dt), S + + +def markov(*args, m=None, transpose=False, dt=None, truncate=False): + """markov(Y, U, [, m]) + + Calculate the first `m` Markov parameters [D CB CAB ...] from data. + + This function computes the Markov parameters for a discrete time + system .. math:: x[k+1] &= A x[k] + B u[k] \\\\ y[k] &= C x[k] + D u[k] - given data for u and y. The algorithm assumes that that C A^k B = 0 for - k > m-2 (see [1]_). Note that the problem is ill-posed if the length of - the input data is less than the desired number of Markov parameters (a - warning message is generated in this case). + given data for u and y. The algorithm assumes that that C A^k B = 0 + for k > m-2 (see [1]_). Note that the problem is ill-posed if the + length of the input data is less than the desired number of Markov + parameters (a warning message is generated in this case). + + The function can be called with either 1, 2 or 3 arguments: + + * ``H = markov(data)`` + * ``H = markov(data, m)`` + * ``H = markov(Y, U)`` + * ``H = markov(Y, U, m)`` + + where `data` is a `TimeResponseData` object, `YY` is a 1D or 3D + array, and r is an integer. Parameters ---------- Y : array_like - Output data. If the array is 1D, the system is assumed to be single - input. If the array is 2D and transpose=False, the columns of `Y` - are taken as time points, otherwise the rows of `Y` are taken as - time points. + Output data. If the array is 1D, the system is assumed to be + single input. If the array is 2D and transpose=False, the columns + of `Y` are taken as time points, otherwise the rows of `Y` are + taken as time points. U : array_like Input data, arranged in the same way as `Y`. + data : TimeResponseData + Response data from which the Markov parameters where estimated. + Input and output data must be 1D or 2D array. m : int, optional - Number of Markov parameters to output. Defaults to len(U). + Number of Markov parameters to output. Defaults to len(U). + dt : True of float, optional + True indicates discrete time with unspecified sampling time and a + positive float is discrete time with the specified sampling time. + It can be used to scale the Markov parameters in order to match + the unit-area impulse response of python-control. Default is True + for array_like and dt=data.time[1]-data.time[0] for + TimeResponseData as input. + truncate : bool, optional + Do not use first m equation for least squares. Default is False. transpose : bool, optional Assume that input data is transposed relative to the standard - :ref:`time-series-convention`. Default value is False. + :ref:`time-series-convention`. For TimeResponseData this parameter + is ignored. Default is False. Returns ------- H : ndarray - First m Markov parameters, [D CB CAB ...] + First m Markov parameters, [D CB CAB ...]. References ---------- @@ -445,15 +566,6 @@ def markov(Y, U, m=None, transpose=False): and experiments. Journal of Guidance Control and Dynamics, 16(2), 320-329, 2012. http://doi.org/10.2514/3.21006 - Notes - ----- - Currently only works for SISO systems. - - This function does not currently comply with the Python Control Library - :ref:`time-series-convention` for representation of time series data. - Use `transpose=False` to make use of the standard convention (this - will be updated in a future release). - Examples -------- >>> T = np.linspace(0, 10, 100) @@ -462,78 +574,89 @@ def markov(Y, U, m=None, transpose=False): >>> H = ct.markov(Y, U, 3, transpose=False) """ - # Convert input parameters to 2D arrays (if they aren't already) - Umat = np.array(U, ndmin=2) - Ymat = np.array(Y, ndmin=2) - # If data is in transposed format, switch it around - if transpose: - Umat, Ymat = np.transpose(Umat), np.transpose(Ymat) - - # Make sure the system is a SISO system - if Umat.shape[0] != 1 or Ymat.shape[0] != 1: - raise ControlMIMONotImplemented + # Convert input parameters to 2D arrays (if they aren't already) + # Get the system description + if len(args) < 1: + raise ControlArgument("not enough input arguments") + + if isinstance(args[0], TimeResponseData): + data = args[0] + Umat = np.array(data.inputs, ndmin=2) + Ymat = np.array(data.outputs, ndmin=2) + if dt is None: + dt = data.time[1] - data.time[0] + if not np.allclose(np.diff(data.time), dt): + raise ValueError("response time values must be equally " + "spaced.") + transpose = data.transpose + if data.transpose and not data.issiso: + Umat, Ymat = np.transpose(Umat), np.transpose(Ymat) + if len(args) == 2: + m = args[1] + elif len(args) > 2: + raise ControlArgument("too many positional arguments") + else: + if len(args) < 2: + raise ControlArgument("not enough input arguments") + Umat = np.array(args[1], ndmin=2) + Ymat = np.array(args[0], ndmin=2) + if dt is None: + dt = True + if transpose: + Umat, Ymat = np.transpose(Umat), np.transpose(Ymat) + if len(args) == 3: + m = args[2] + elif len(args) > 3: + raise ControlArgument("too many positional arguments") # Make sure the number of time points match if Umat.shape[1] != Ymat.shape[1]: raise ControlDimension( "Input and output data are of differnent lengths") - n = Umat.shape[1] + l = Umat.shape[1] # If number of desired parameters was not given, set to size of input data if m is None: - m = Umat.shape[1] + m = l + + t = 0 + if truncate: + t = m + + q = Ymat.shape[0] # number of outputs + p = Umat.shape[0] # number of inputs # Make sure there is enough data to compute parameters - if m > n: + if m*p > (l-t): warnings.warn("Not enough data for requested number of parameters") + # the algorithm - Construct a matrix of control inputs to invert # - # Original algorithm (with mapping to standard order) - # - # RMM note, 24 Dec 2020: This algorithm sets the problem up correctly - # until the final column of the UU matrix is created, at which point it - # makes some modifications that I don't understand. This version of the - # algorithm does not seem to return the actual Markov parameters for a - # system. - # - # # Create the matrix of (shifted) inputs - # UU = np.transpose(Umat) - # for i in range(1, m-1): - # # Shift previous column down and add a zero at the top - # newCol = np.vstack((0, np.reshape(UU[0:n-1, i-1], (-1, 1)))) - # UU = np.hstack((UU, newCol)) - # - # # Shift previous column down and add a zero at the top - # Ulast = np.vstack((0, np.reshape(UU[0:n-1, m-2], (-1, 1)))) - # - # # Replace the elements of the last column new values (?) - # # Each row gets the sum of the rows above it (?) - # for i in range(n-1, 0, -1): - # Ulast[i] = np.sum(Ulast[0:i-1]) - # UU = np.hstack((UU, Ulast)) - # - # # Solve for the Markov parameters from Y = H @ UU - # # H = [[D], [CB], [CAB], ..., [C A^{m-3} B], [???]] - # H = np.linalg.lstsq(UU, np.transpose(Ymat))[0] - # - # # Markov parameters are in rows => transpose if needed - # return H if transpose else np.transpose(H) - - # - # New algorithm - Construct a matrix of control inputs to invert + # (q,l) = (q,p*m) @ (p*m,l) + # YY.T = H @ UU.T # # This algorithm sets up the following problem and solves it for # the Markov parameters # + # (l,q) = (l,p*m) @ (p*m,q) + # YY = UU @ H.T + # # [ y(0) ] [ u(0) 0 0 ] [ D ] # [ y(1) ] [ u(1) u(0) 0 ] [ C B ] # [ y(2) ] = [ u(2) u(1) u(0) ] [ C A B ] # [ : ] [ : : : : ] [ : ] - # [ y(n-1) ] [ u(n-1) u(n-2) u(n-3) ... u(n-m) ] [ C A^{m-2} B ] + # [ y(l-1) ] [ u(l-1) u(l-2) u(l-3) ... u(l-m) ] [ C A^{m-2} B ] + # + # truncated version t=m, do not use first m equation + # + # [ y(t) ] [ u(t) u(t-1) u(t-2) u(t-m) ] [ D ] + # [ y(t+1) ] [ u(t+1) u(t) u(t-1) u(t-m+1)] [ C B ] + # [ y(t+2) ] = [ u(t+2) u(t+1) u(t) u(t-m+2)] [ C B ] + # [ : ] [ : : : : ] [ : ] + # [ y(l-1) ] [ u(l-1) u(l-2) u(l-3) ... u(l-m) ] [ C A^{m-2} B ] # - # Note: if the number of Markov parameters (m) is less than the size of - # the input/output data (n), then this algorithm assumes C A^{j} B = 0 + # Note: This algorithm assumes C A^{j} B = 0 # for j > m-2. See equation (3) in # # J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification @@ -542,17 +665,34 @@ def markov(Y, U, m=None, transpose=False): # 320-329, 2012. http://doi.org/10.2514/3.21006 # + # Set up the full problem # Create matrix of (shifted) inputs - UU = Umat - for i in range(1, m): - # Shift previous column down and add a zero at the top - new_row = np.hstack((0, UU[i-1, 0:-1])) - UU = np.vstack((UU, new_row)) - UU = np.transpose(UU) - - # Invert and solve for Markov parameters - YY = np.transpose(Ymat) - H, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None) + UUT = np.zeros((p*m,(l))) + for i in range(m): + # Shift previous column down and keep zeros at the top + UUT[i*p:(i+1)*p,i:] = Umat[:,:l-i] + + # Truncate first t=0 or t=m time steps, transpose the problem for lsq + YY = Ymat[:,t:].T + UU = UUT[:,t:].T + + # Solve for the Markov parameters from YY = UU @ H.T + HT, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None) + H = HT.T/dt # scaling + + H = H.reshape(q,m,p) # output, time*input -> output, time, input + H = H.transpose(0,2,1) # output, input, time + + # for siso return a 1D array instead of a 3D array + if q == 1 and p == 1: + H = np.squeeze(H) # Return the first m Markov parameters - return H if transpose else np.transpose(H) + return H if not transpose else np.transpose(H) + +# Function aliases +hsvd = hankel_singular_values +balred = balanced_reduction +modred = model_reduction +minreal = minimal_realization +era = eigensys_realization diff --git a/control/nichols.py b/control/nichols.py index 1a5043cd4..ac42c9c37 100644 --- a/control/nichols.py +++ b/control/nichols.py @@ -13,17 +13,18 @@ nichols.nichols_grid """ -import numpy as np import matplotlib.pyplot as plt import matplotlib.transforms +import numpy as np +from . import config +from .ctrlplot import ControlPlot, _get_line_labels, _process_ax_keyword, \ + _process_legend_keywords, _process_line_labels, _update_plot_title from .ctrlutil import unwrap -from .freqplot import _default_frequency_range, _freqplot_defaults, \ - _get_line_labels +from .freqplot import _default_frequency_range, _freqplot_defaults from .lti import frequency_response from .statesp import StateSpace from .xferfcn import TransferFunction -from . import config __all__ = ['nichols_plot', 'nichols', 'nichols_grid'] @@ -34,8 +35,8 @@ def nichols_plot( - data, omega=None, *fmt, grid=None, title=None, - legend_loc='upper left', **kwargs): + data, omega=None, *fmt, grid=None, title=None, ax=None, + label=None, **kwargs): """Nichols plot for a system. Plots a Nichols plot for the system over a (optional) frequency range. @@ -52,23 +53,56 @@ def nichols_plot( The `omega` parameter must be present (use omega=None if needed). grid : boolean, optional True if the plot should include a Nichols-chart grid. Default is True. - legend_loc : str, optional - For plots with multiple lines, a legend will be included in the - given location. Default is 'upper left'. Use False to supress. **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional Additional keywords passed to `matplotlib` to specify line properties. Returns ------- - lines : array of Line2D - 1-D array of Line2D objects. The size of the array matches - the number of systems and the value of the array is a list of - Line2D objects for that system. + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: 1D array of :class:`matplotlib.lines.Line2D` objects. + The size of the array matches the number of systems and the + value of the array is a list of Line2D objects for that system. + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + * cplt.legend: legend object(s) contained in the plot + + See :class:`ControlPlot` for more detailed information. + + lines : array of Line2D + + Other Parameters + ---------------- + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. + label : str or array_like of str, optional + If present, replace automatically generated label(s) with given + label(s). If sysdata is a list, strings should be specified for each + system. + legend_loc : int or str, optional + Include a legend in the given location. Default is 'upper left', + with no legend for a single response. Use False to suppress legend. + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on the + plot or ``legend_loc`` has been specified. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). + """ # Get parameter values grid = config._get_param('nichols', 'grid', grid, True) - freqplot_rcParams = config._get_param( - 'freqplot', 'rcParams', kwargs, _freqplot_defaults, pop=True) + label = _process_line_labels(label) + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) # If argument was a singleton, turn it into a list if not isinstance(data, (tuple, list)): @@ -83,6 +117,10 @@ def nichols_plot( if any([resp.ninputs > 1 or resp.noutputs > 1 for resp in data]): raise NotImplementedError("MIMO Nichols plots not implemented") + fig, ax_nichols = _process_ax_keyword(ax, rcParams=rcParams, squeeze=True) + legend_loc, _, show_legend = _process_legend_keywords( + kwargs, None, 'upper left') + # Create a list of lines for the output out = np.empty(len(data), dtype=object) @@ -97,13 +135,13 @@ def nichols_plot( x = unwrap(np.degrees(phase), 360) y = 20*np.log10(mag) - # Decide on the system name + # Decide on the system name and label sysname = response.sysname if response.sysname is not None \ else f"Unknown-{idx_sys}" + label_ = sysname if label is None else label[idx] # Generate the plot - with plt.rc_context(freqplot_rcParams): - out[idx] = plt.plot(x, y, *fmt, label=sysname, **kwargs) + out[idx] = ax_nichols.plot(x, y, *fmt, label=label_, **kwargs) # Label the plot axes plt.xlabel('Phase [deg]') @@ -117,21 +155,23 @@ def nichols_plot( nichols_grid() # List of systems that are included in this plot - ax_nichols = plt.gca() lines, labels = _get_line_labels(ax_nichols) # Add legend if there is more than one system plotted - if len(labels) > 1 and legend_loc is not False: - with plt.rc_context(freqplot_rcParams): - ax_nichols.legend(lines, labels, loc=legend_loc) + if show_legend == True or (show_legend != False and len(labels) > 1): + with plt.rc_context(rcParams): + legend = ax_nichols.legend(lines, labels, loc=legend_loc) + else: + legend = None # Add the title - if title is None: - title = "Nichols plot for " + ", ".join(labels) - with plt.rc_context(freqplot_rcParams): - plt.suptitle(title) + if ax is None: + if title is None: + title = "Nichols plot for " + ", ".join(labels) + _update_plot_title( + title, fig=fig, rcParams=rcParams, use_existing=False) - return out + return ControlPlot(out, ax_nichols, fig, legend=legend) def _inner_extents(ax): @@ -163,8 +203,8 @@ def nichols_grid(cl_mags=None, cl_phases=None, line_style='dotted', ax=None, :doc:`Matplotlib linestyle \ ` ax : matplotlib.axes.Axes, optional - Axes to add grid to. If ``None``, use ``plt.gca()``. - label_cl_phases: bool, optional + Axes to add grid to. If ``None``, use ``matplotlib.pyplot.gca()``. + label_cl_phases : bool, optional If True, closed-loop phase lines will be labelled. Returns diff --git a/control/nlsys.py b/control/nlsys.py index c154c0818..accd24c0f 100644 --- a/control/nlsys.py +++ b/control/nlsys.py @@ -18,16 +18,17 @@ """ -import numpy as np -import scipy as sp import copy from warnings import warn +import numpy as np +import scipy as sp + from . import config -from .iosys import InputOutputSystem, _process_signal_list, \ - _process_iosys_keywords, isctime, isdtime, common_timebase, _parse_spec +from .iosys import InputOutputSystem, _parse_spec, _process_iosys_keywords, \ + _process_signal_list, common_timebase, isctime, isdtime from .timeresp import _check_convert_array, _process_time_response, \ - TimeResponseData + TimeResponseData, TimeResponseList __all__ = ['NonlinearIOSystem', 'InterconnectedSystem', 'nlsys', 'input_output_response', 'find_eqpt', 'linearize', @@ -65,7 +66,7 @@ class NonlinearIOSystem(InputOutputSystem): 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 `s[i]` (where `s` is one of `u`, `y`, or `x`). If + of the form 's[i]' (where 's' is one of 'u', 'y', or 'x'). If this parameter is not given or given as `None`, the relevant quantity will be determined when possible based on other information provided to functions using the system. @@ -91,9 +92,8 @@ class NonlinearIOSystem(InputOutputSystem): generic name is generated with a unique integer id. params : dict, optional - Parameter values for the systems. Passed to the evaluation - functions for the system as default values, overriding internal - defaults. + Parameter values for the system. Passed to the evaluation functions + for the system as default values, overriding internal defaults. See Also -------- @@ -132,14 +132,19 @@ def __init__(self, updfcn, outfcn=None, params=None, **kwargs): if updfcn is None: if self.nstates is None: self.nstates = 0 + self.updfcn = lambda t, x, u, params: np.zeros(0) else: raise ValueError( "states specified but no update function given.") if outfcn is None: - # No output function specified => outputs = states - if self.noutputs is None and self.nstates is not None: + if self.noutputs == 0: + self.outfcn = lambda t, x, u, params: np.zeros(0) + elif self.noutputs is None and self.nstates is not None: self.noutputs = self.nstates + if len(self.output_index) == 0: + # Use state names for outputs + self.output_index = self.state_index elif self.noutputs is not None and self.noutputs == self.nstates: # Number of outputs = number of states => all is OK pass @@ -364,9 +369,8 @@ def _rhs(self, t, x, u): user-friendly interface you may want to use :meth:`dynamics`. """ - xdot = self.updfcn(t, x, u, self._current_params) \ - if self.updfcn is not None else [] - return np.array(xdot).reshape((-1,)) + return np.asarray( + self.updfcn(t, x, u, self._current_params)).reshape(-1) def dynamics(self, t, x, u, params=None): """Compute the dynamics of a differential or difference equation. @@ -403,7 +407,8 @@ def dynamics(self, t, x, u, params=None): dx/dt or x[t+dt] : ndarray """ self._update_params(params) - return self._rhs(t, x, u) + return self._rhs( + t, np.asarray(x).reshape(-1), np.asarray(u).reshape(-1)) def _out(self, t, x, u): """Evaluate the output of a system at a given state, input, and time @@ -414,9 +419,17 @@ def _out(self, t, x, u): :meth:`output`. """ - y = self.outfcn(t, x, u, self._current_params) \ - if self.outfcn is not None else x - return np.array(y).reshape((-1,)) + # + # To allow lazy evaluation of the system size, we allow for the + # possibility that noutputs is left unspecified when the system + # is created => we have to check for that case here (and return + # the system state or a portion of it). + # + if self.outfcn is None: + return x if self.noutputs is None else x[:self.noutputs] + else: + return np.asarray( + self.outfcn(t, x, u, self._current_params)).reshape(-1) def output(self, t, x, u, params=None): """Compute the output of the system @@ -444,7 +457,8 @@ def output(self, t, x, u, params=None): y : ndarray """ self._update_params(params) - return self._out(t, x, u) + return self._out( + t, np.asarray(x).reshape(-1), np.asarray(u).reshape(-1)) def feedback(self, other=1, sign=-1, params=None): """Feedback interconnection between two input/output systems @@ -502,7 +516,7 @@ def feedback(self, other=1, sign=-1, params=None): return newsys def linearize(self, x0, u0, t=0, params=None, eps=1e-6, - name=None, copy_names=False, **kwargs): + copy_names=False, **kwargs): """Linearize an input/output system at a given state and input. Return the linearization of an input/output system at a given state @@ -517,26 +531,15 @@ def linearize(self, x0, u0, t=0, params=None, eps=1e-6, # numerical linearization use the `_rhs()` and `_out()` member # functions. # + # Process nominal states and inputs + x0, nstates = _process_vector_argument(x0, "x0", self.nstates) + u0, ninputs = _process_vector_argument(u0, "u0", self.ninputs) - # If x0 and u0 are specified as lists, concatenate the elements - x0 = _concatenate_list_elements(x0, 'x0') - u0 = _concatenate_list_elements(u0, 'u0') - - # Figure out dimensions if they were not specified. - nstates = _find_size(self.nstates, x0) - ninputs = _find_size(self.ninputs, u0) - - # Convert x0, u0 to arrays, if needed - if np.isscalar(x0): - x0 = np.ones((nstates,)) * x0 - if np.isscalar(u0): - u0 = np.ones((ninputs,)) * u0 + # Update the current parameters (prior to calling _out()) + self._update_params(params) # Compute number of outputs by evaluating the output function - noutputs = _find_size(self.noutputs, self._out(t, x0, u0)) - - # Update the current parameters - self._update_params(params) + noutputs = _find_size(self.noutputs, self._out(t, x0, u0), "outputs") # Compute the nominal value of the update law and output F0 = self._rhs(t, x0, u0) @@ -568,8 +571,6 @@ def linearize(self, x0, u0, t=0, params=None, eps=1e-6, # Set the system name, inputs, outputs, and states if copy_names: linsys._copy_names(self, prefix_suffix_name='linearized') - if name is not None: - linsys.name = name # re-init to include desired signal names if names were provided return StateSpace(linsys, **kwargs) @@ -706,10 +707,10 @@ def __init__(self, syslist, connections=None, inplist=None, outlist=None, # Create updfcn and outfcn def updfcn(t, x, u, params): - self.update_params(params) + self._update_params(params) return self._rhs(t, x, u) def outfcn(t, x, u, params): - self.update_params(params) + self._update_params(params) return self._out(t, x, u) # Initialize NonlinearIOSystem object @@ -1242,7 +1243,7 @@ def nlsys( 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 `s[i]` (where `s` is one of `u`, `y`, or `x`). If + of the form 's[i]' (where 's' is one of 'u', 'y', or 'x'). If this parameter is not given or given as `None`, the relevant quantity will be determined when possible based on other information provided to functions using the system. @@ -1268,9 +1269,8 @@ def nlsys( generic name is generated with a unique integer id. params : dict, optional - Parameter values for the systems. Passed to the evaluation - functions for the system as default values, overriding internal - defaults. + Parameter values for the system. Passed to the evaluation functions + for the system as default values, overriding internal defaults. Returns ------- @@ -1306,7 +1306,7 @@ def nlsys( def input_output_response( - sys, T, U=0., X0=0, params=None, + sys, T, U=0., X0=0, params=None, ignore_errors=False, transpose=False, return_x=False, squeeze=None, solve_ivp_kwargs=None, t_eval='T', **kwargs): """Compute the output response of a system to a given input. @@ -1316,33 +1316,30 @@ def input_output_response( Parameters ---------- - sys : InputOutputSystem - Input/output system to simulate. - + sys : NonlinearIOSystem or list of NonlinearIOSystem + I/O system(s) for which input/output response is simulated. T : array-like Time steps at which the input is defined; values must be evenly spaced. - U : array-like, list, or number, optional Input array giving input at each time `T` (default = 0). If a list is specified, each element in the list will be treated as a portion of the input and broadcast (if necessary) to match the time vector. - X0 : array-like, list, or number, optional Initial condition (default = 0). If a list is given, each element in the list will be flattened and stacked into the initial condition. If a smaller number of elements are given that the number of states in the system, the initial condition will be padded with zeros. - t_eval : array-list, optional List of times at which the time response should be computed. Defaults to ``T``. - return_x : bool, optional If True, return the state vector when assigning to a tuple (default = False). See :func:`forced_response` for more details. If True, return the values of the state at each time (default = False). - + params : dict, optional + Parameter values for the system. Passed to the evaluation functions + for the system as default values, overriding internal defaults. squeeze : bool, optional If True and if the system has a single output, return the system output as a 1D array rather than a 2D array. If False, return the @@ -1377,11 +1374,19 @@ def input_output_response( Other parameters ---------------- + ignore_errors : bool, optional + If ``False`` (default), errors during computation of the trajectory + will raise a ``RuntimeError`` exception. If ``True``, do not raise + an exception and instead set ``results.success`` to ``False`` and + place an error message in ``results.message``. solve_ivp_method : str, optional Set the method used by :func:`scipy.integrate.solve_ivp`. Defaults to 'RK45'. solve_ivp_kwargs : dict, optional Pass additional keywords to :func:`scipy.integrate.solve_ivp`. + transpose : bool, default=False + If True, transpose all input and output arrays (for backward + compatibility with MATLAB and :func:`scipy.signal.lsim`). Raises ------ @@ -1432,6 +1437,16 @@ def input_output_response( if kwargs: raise TypeError("unrecognized keyword(s): ", str(kwargs)) + # If passed a list, recursively call individual responses with given T + if isinstance(sys, (list, tuple)): + sysdata, responses = sys, [] + for sys in sysdata: + responses.append(input_output_response( + sys, T, U=U, X0=X0, params=params, transpose=transpose, + return_x=return_x, squeeze=squeeze, t_eval=t_eval, + solve_ivp_kwargs=solve_ivp_kwargs, **kwargs)) + return TimeResponseList(responses) + # Sanity checking on the input if not isinstance(sys, NonlinearIOSystem): raise TypeError("System of type ", type(sys), " not valid") @@ -1451,7 +1466,16 @@ def input_output_response( # Use the input time points as the output time points t_eval = T - # If we were passed a list of input, concatenate them (w/ broadcast) + # + # Process input argument + # + # The input argument is interpreted very flexibly, allowing the + # use of lists and/or tuples of mixed scalar and vector elements. + # + # Much of the processing here is similar to the processing in + # _process_vector_argument, but applied to a time series. + + # If we were passed a list of inputs, concatenate them (w/ broadcast) if isinstance(U, (tuple, list)) and len(U) != ntimepts: U_elements = [] for i, u in enumerate(U): @@ -1475,62 +1499,44 @@ def input_output_response( # Save the newly created input vector U = np.vstack(U_elements) + # Figure out the number of inputs + if sys.ninputs is None: + if isinstance(U, np.ndarray): + ninputs = U.shape[0] if U.size > 1 else U.size + else: + ninputs = 1 + else: + ninputs = sys.ninputs + # Make sure the input has the right shape - if sys.ninputs is None or sys.ninputs == 1: + if ninputs is None or ninputs == 1: legal_shapes = [(ntimepts,), (1, ntimepts)] else: - legal_shapes = [(sys.ninputs, ntimepts)] + legal_shapes = [(ninputs, ntimepts)] - U = _check_convert_array(U, legal_shapes, - 'Parameter ``U``: ', squeeze=False) + U = _check_convert_array( + U, legal_shapes, 'Parameter ``U``: ', squeeze=False) # Always store the input as a 2D array U = U.reshape(-1, ntimepts) ninputs = U.shape[0] - # If we were passed a list of initial states, concatenate them - X0 = _concatenate_list_elements(X0, 'X0') - - # If the initial state is too short, make it longer (NB: sys.nstates - # could be None if nstates comes from size of initial condition) - if sys.nstates and isinstance(X0, np.ndarray) and X0.size < sys.nstates: - if X0[-1] != 0: - warn("initial state too short; padding with zeros") - X0 = np.hstack([X0, np.zeros(sys.nstates - X0.size)]) - - # If we were passed a list of initial states, concatenate them - if isinstance(X0, (tuple, list)): - X0_list = [] - for i, x0 in enumerate(X0): - x0 = np.array(x0).reshape(-1) # convert everyting to 1D array - X0_list += x0.tolist() # add elements to initial state - - # Save the newly created input vector - X0 = np.array(X0_list) + # Process initial states + X0, nstates = _process_vector_argument(X0, "X0", sys.nstates) - # If the initial state is too short, make it longer (NB: sys.nstates - # could be None if nstates comes from size of initial condition) - if sys.nstates and isinstance(X0, np.ndarray) and X0.size < sys.nstates: - if X0[-1] != 0: - warn("initial state too short; padding with zeros") - X0 = np.hstack([X0, np.zeros(sys.nstates - X0.size)]) - - # Compute the number of states - nstates = _find_size(sys.nstates, X0) - - # create X0 if not given, test if X0 has correct shape - X0 = _check_convert_array(X0, [(nstates,), (nstates, 1)], - 'Parameter ``X0``: ', squeeze=True) + # Update the parameter values (prior to evaluating outfcn) + sys._update_params(params) # Figure out the number of outputs - if sys.noutputs is None: - # Evaluate the output function to find number of outputs - noutputs = np.shape(sys._out(T[0], X0, U[:, 0]))[0] + if sys.outfcn is None: + noutputs = nstates if sys.noutputs is None else sys.noutputs else: - noutputs = sys.noutputs + noutputs = np.shape(sys._out(T[0], X0, U[:, 0]))[0] - # Update the parameter values - sys._update_params(params) + if sys.noutputs is not None and sys.noutputs != noutputs: + raise RuntimeError( + f"inconsistent size of outputs; system specified {sys.noutputs}, " + f"output function returned {noutputs}") # # Define a function to evaluate the input at an arbitrary time @@ -1583,7 +1589,11 @@ def ivp_rhs(t, x): ivp_rhs, (T0, Tf), X0, t_eval=t_eval, vectorized=False, **solve_ivp_kwargs) if not soln.success: - raise RuntimeError("solve_ivp failed: " + soln.message) + message = "solve_ivp failed: " + soln.message + if not ignore_errors: + raise RuntimeError(message) + else: + message = None # Compute inputs and outputs for each time point u = np.zeros((ninputs, len(soln.t))) @@ -1639,7 +1649,7 @@ def ivp_rhs(t, x): u = np.transpose(np.array(u)) # Mark solution as successful - soln.success = True # No way to fail + soln.success, message = True, None # No way to fail else: # Neither ctime or dtime?? raise TypeError("Can't determine system type") @@ -1649,7 +1659,8 @@ def ivp_rhs(t, x): output_labels=sys.output_labels, input_labels=sys.input_labels, state_labels=sys.state_labels, sysname=sys.name, title="Input/output response for " + sys.name, - transpose=transpose, return_x=return_x, squeeze=squeeze) + transpose=transpose, return_x=return_x, squeeze=squeeze, + success=soln.success, message=message) def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, @@ -1662,6 +1673,8 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, Parameters ---------- + sys : NonlinearIOSystem + I/O system for which the equilibrium point is sought. x0 : list of initial state values Initial guess for the value of the state near the equilibrium point. u0 : list of input values, optional @@ -1732,17 +1745,9 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, from scipy.optimize import root # Figure out the number of states, inputs, and outputs - nstates = _find_size(sys.nstates, x0) - ninputs = _find_size(sys.ninputs, u0) - noutputs = _find_size(sys.noutputs, y0) - - # Convert x0, u0, y0 to arrays, if needed - if np.isscalar(x0): - x0 = np.ones((nstates,)) * x0 - if np.isscalar(u0): - u0 = np.ones((ninputs,)) * u0 - if np.isscalar(y0): - y0 = np.ones((ninputs,)) * y0 + x0, nstates = _process_vector_argument(x0, "x0", sys.nstates) + u0, ninputs = _process_vector_argument(u0, "u0", sys.ninputs) + y0, noutputs = _process_vector_argument(y0, "y0", sys.noutputs) # Make sure the input arguments match the sizes of the system if len(x0) != nstates or \ @@ -1930,7 +1935,7 @@ def linearize(sys, xeq, ueq=None, t=0, params=None, **kw): Parameters ---------- sys : InputOutputSystem - The system to be linearized + The system to be linearized. xeq : array The state at which the linearization will be evaluated (does not need to be an equilibrium state). @@ -1964,7 +1969,7 @@ def linearize(sys, xeq, ueq=None, t=0, params=None, **kw): Other Parameters ---------------- inputs : int, list of str or None, optional - Description of the system inputs. If not specified, the origional + Description of the system inputs. If not specified, the original system inputs are used. See :class:`InputOutputSystem` for more information. outputs : int, list of str or None, optional @@ -1977,15 +1982,16 @@ def linearize(sys, xeq, ueq=None, t=0, params=None, **kw): return sys.linearize(xeq, ueq, t=t, params=params, **kw) -def _find_size(sysval, vecval): +def _find_size(sysval, vecval, name="system component"): """Utility function to find the size of a system parameter If both parameters are not None, they must be consistent. """ if hasattr(vecval, '__len__'): if sysval is not None and sysval != len(vecval): - raise ValueError("Inconsistent information to determine size " - "of system component") + raise ValueError( + f"inconsistent information to determine size of {name}; " + f"expected {sysval} values, received {len(vecval)}") return len(vecval) # None or 0, which is a valid value for "a (sysval, ) vector of zeros". if not vecval: @@ -1993,7 +1999,7 @@ def _find_size(sysval, vecval): elif sysval == 1: # (1, scalar) is also a valid combination from legacy code return 1 - raise ValueError("can't determine size of system component") + raise ValueError(f"can't determine size of {name}") # Function to create an interconnected system @@ -2065,10 +2071,8 @@ def interconnect( inplist : list of input connections, optional List of connections for how the inputs for the overall system are - mapped to the subsystem inputs. The input specification is similar to - the form defined in the connection specification, except that - connections do not specify an input-spec, since these are the system - inputs. The entries for a connection are thus of the form: + mapped to the subsystem inputs. The entries for a connection are + of the form: [input-spec1, input-spec2, ...] @@ -2081,11 +2085,10 @@ def interconnect( outlist : list of output connections, optional List of connections for how the outputs from the subsystems are - mapped to overall system outputs. The output connection - description is the same as the form defined in the inplist - specification (including the optional gain term). Numbered outputs - must be chosen from the list of subsystem outputs, but named - outputs can also be contained in the list of subsystem inputs. + mapped to overall system outputs. The entries for a connection are + of the form: + + [output-spec1, output-spec2, ...] If an output connection contains more than one signal specification, then those signals are added together (multiplying by the any gain @@ -2098,7 +2101,7 @@ def interconnect( 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 `s[i]` (where `s` is one of `u`, `y`, or `x`). If this parameter + form 's[i]' (where 's' is one of 'u', 'y', or 'x'). If this parameter is not given or given as `None`, the relevant quantity will be determined when possible based on other information provided to functions using the system. @@ -2198,7 +2201,7 @@ def interconnect( ... inplist=['C'], outlist=['P']) A feedback system can also be constructed using the - :func:`~control.summing_block` function and the ability to + :func:`~control.summing_junction` function and the ability to automatically interconnect signals with the same names: >>> P = ct.tf(1, [1, 0], inputs='u', outputs='y') @@ -2241,7 +2244,7 @@ def interconnect( `outputs`, for more natural naming of SISO systems. """ - from .statesp import StateSpace, LinearICSystem, _convert_to_statespace + from .statesp import LinearICSystem, StateSpace, _convert_to_statespace from .xferfcn import TransferFunction dt = kwargs.pop('dt', None) # bypass normal 'dt' processing @@ -2404,15 +2407,22 @@ def interconnect( elif not found_system: raise ValueError("could not find signal %s" % sname) else: - # Regular signal specification - if not isinstance(connection, list): - dprint(f" converting item to list") - connection = [connection] - for spec in connection: - isys, indices, gain = _parse_spec(syslist, spec, 'input') + if isinstance(connection, list): + # Passed a list => create input map + dprint(f" detected input list") + signal_list = [] + for spec in connection: + isys, indices, gain = _parse_spec(syslist, spec, 'input') + for isig in indices: + signal_list.append((isys, isig, gain)) + dprint(f" adding input {(isys, isig, gain)} to list") + new_inplist.append(signal_list) + else: + # Passed a single signal name => add individual input(s) + isys, indices, gain = _parse_spec(syslist, connection, 'input') for isig in indices: - dprint(f" adding input {(isys, isig, gain)}") new_inplist.append((isys, isig, gain)) + dprint(f" adding input {(isys, isig, gain)}") inplist, inputs = new_inplist, new_inputs dprint(f" {inplist=}\n {inputs=}") @@ -2478,17 +2488,15 @@ def interconnect( elif not found_system: raise ValueError("could not find signal %s" % sname) else: - # Regular signal specification - if not isinstance(connection, list): - dprint(f" converting item to list") - connection = [connection] - for spec in connection: + # Utility function to find named output or input signal + def _find_output_or_input_signal(spec): + signal_list = [] try: # First trying looking in the output signals osys, indices, gain = _parse_spec(syslist, spec, 'output') for osig in indices: dprint(f" adding output {(osys, osig, gain)}") - new_outlist.append((osys, osig, gain)) + signal_list.append((osys, osig, gain)) except ValueError: # If not, see if we can find it in inputs isys, indices, gain = _parse_spec( @@ -2497,9 +2505,21 @@ def interconnect( for isig in indices: # Use string form to allow searching input list dprint(f" adding input {(isys, isig, gain)}") - new_outlist.append( + signal_list.append( (syslist[isys].name, syslist[isys].input_labels[isig], gain)) + return signal_list + + if isinstance(connection, list): + # Passed a list => create input map + dprint(f" detected output list") + signal_list = [] + for spec in connection: + signal_list += _find_output_or_input_signal(spec) + new_outlist.append(signal_list) + else: + new_outlist += _find_output_or_input_signal(connection) + outlist, outputs = new_outlist, new_outputs dprint(f" {outlist=}\n {outputs=}") @@ -2551,18 +2571,59 @@ def interconnect( return newsys -# Utility function to allow lists states, inputs -def _concatenate_list_elements(X, name='X'): - # If we were passed a list, concatenate the elements together - if isinstance(X, (tuple, list)): - X_list = [] - for i, x in enumerate(X): - x = np.array(x).reshape(-1) # convert everyting to 1D array - X_list += x.tolist() # add elements to initial state - return np.array(X_list) +def _process_vector_argument(arg, name, size): + """Utility function to process vector elements (states, inputs) + + Process state and input arguments to turn them into lists of the + appropriate length. + + Parameters + ---------- + arg : array_like + Value of the parameter passed to the function. Can be a list, + tuple, ndarray, scalar, or None. + name : string + Name of the argument being processed. Used in errors/warnings. + size : int or None + Size of the element. If None, size is determined by arg. + + Returns + ------- + val : array or None + Value of the element, zero-padded to proper length. + nelem : int or None + Number of elements in the returned value. + + Warns + ----- + UserWarning : "{name} too short; padding with zeros" + If argument is too short and last value in arg is not 0. + + """ + # Allow and expand list + if isinstance(arg, (tuple, list)): + val_list = [] + for i, v in enumerate(arg): + v = np.array(v).reshape(-1) # convert to 1D array + val_list += v.tolist() # add elements to list + val = np.array(val_list) + elif np.isscalar(arg) and size is not None: # extend scalars + val = np.ones((size, )) * arg + elif np.isscalar(arg) and size is None: # single scalar + val = np.array([arg]) + elif isinstance(arg, np.ndarray): + val = arg.reshape(-1) # convert to 1D array + else: + val = arg # return what we were given + + if size is not None and isinstance(val, np.ndarray) and val.size < size: + # If needed, extend the size of the vector to match desired size + if val[-1] != 0: + warn(f"{name} too short; padding with zeros") + val = np.hstack([val, np.zeros(size - val.size)]) - # Otherwise, do nothing - return X + nelem = _find_size(size, val, name) # determine size + return val, nelem # Utility function to create an I/O system from a static gain @@ -2574,13 +2635,14 @@ def _convert_static_iosystem(sys): # Convert sys1 to an I/O system if needed if isinstance(sys, (int, float, np.number)): return NonlinearIOSystem( - None, lambda t, x, u, params: sys * u, inputs=1, outputs=1) + None, lambda t, x, u, params: sys * u, + outputs=1, inputs=1, dt=None) elif isinstance(sys, np.ndarray): sys = np.atleast_2d(sys) return NonlinearIOSystem( None, lambda t, x, u, params: sys @ u, - outputs=sys.shape[0], inputs=sys.shape[1]) + outputs=sys.shape[0], inputs=sys.shape[1], dt=None) def connection_table(sys, show_names=False, column_width=32): """Print table of connections inside an interconnected system model. diff --git a/control/optimal.py b/control/optimal.py index ce80eccfc..0eb49c823 100644 --- a/control/optimal.py +++ b/control/optimal.py @@ -72,7 +72,7 @@ class OptimalControlProblem(): Method to use for carrying out the optimization. Currently supported methods are 'shooting' and 'collocation' (continuous time only). The default value is 'shooting' for discrete time systems and - 'collocation' for continuous time systems + 'collocation' for continuous time systems. initial_guess : (tuple of) 1D or 2D array_like Initial states and/or inputs to use as a guess for the optimal trajectory. For shooting methods, an array of inputs for each time @@ -732,6 +732,7 @@ def _simulate_states(self, x0, inputs): logging.debug("input =\n" + str(inputs)) # Simulate the system to get the state + # TODO: update to use response object; remove return_x _, _, states = ct.input_output_response( self.system, self.timepts, inputs, x0, return_x=True, solve_ivp_kwargs=self.solve_ivp_kwargs, t_eval=self.timepts) @@ -981,7 +982,7 @@ def solve_ocp( timepts : 1D array_like List of times at which the optimal input should be computed. - X0: array-like or number, optional + X0 : array-like or number, optional Initial condition (default = 0). cost : callable @@ -1022,6 +1023,16 @@ def solve_ocp( 1D input of shape (ninputs,) that will be broadcast by extension of the time axis. + basis : BasisFamily, optional + Use the given set of basis functions for the inputs instead of + setting the value of the input at each point in the timepts vector. + + trajectory_method : string, optional + Method to use for carrying out the optimization. Currently supported + methods are 'shooting' and 'collocation' (continuous time only). The + default value is 'shooting' for discrete time systems and + 'collocation' for continuous time systems. + log : bool, optional If `True`, turn on logging messages (using Python logging module). @@ -1061,6 +1072,11 @@ def solve_ocp( res.states : array Time evolution of the state vector (if return_states=True). + Other Parameters + ---------------- + minimize_method : str, optional + Set the method used by :func:`scipy.optimize.minimize`. + Notes ----- 1. For discrete time systems, the final value of the timepts vector @@ -1079,9 +1095,9 @@ def solve_ocp( """ # Process keyword arguments - if trajectory_constraints is None: - # Backwards compatibility - trajectory_constraints = kwargs.pop('constraints', []) + trajectory_constraints = config._process_legacy_keyword( + kwargs, 'constraints', 'trajectory_constraints', + trajectory_constraints) # Allow 'return_x` as a synonym for 'return_states' return_states = ct.config._get_param( @@ -1095,14 +1111,14 @@ def solve_ocp( raise ValueError("'minimize_method' specified more than once") warnings.warn( "'method' parameter is deprecated; assuming minimize_method", - DeprecationWarning) + FutureWarning) kwargs['minimize_method'] = method else: if kwargs.get('trajectory_method'): raise ValueError("'trajectory_method' specified more than once") warnings.warn( "'method' parameter is deprecated; assuming trajectory_method", - DeprecationWarning) + FutureWarning) kwargs['trajectory_method'] = method # Set up the optimal control problem @@ -1168,6 +1184,10 @@ def create_mpc_iosystem( inputs, outputs, states : int or list of str, optional Set the names of the inputs, outputs, and states, as described in :func:`~control.InputOutputSystem`. + log : bool, optional + If `True`, turn on logging messages (using Python logging module). + Use :py:func:`logging.basicConfig` to enable logging output + (e.g., to a file). name : string, optional System name (used for specifying signals). If unspecified, a generic name is generated with a unique integer id. @@ -1936,12 +1956,12 @@ def solve_oep( I/O system for which the optimal input will be computed. timepts : 1D array_like List of times at which the optimal input should be computed. - Y, U: 2D array_like + Y, U : 2D array_like Values of the outputs and inputs at each time point. trajectory_cost : callable Function that returns the cost given the current state and input. Called as `cost(y, u, x0)`. - X0: 1D array_like, optional + X0 : 1D array_like, optional Mean value of the initial condition (defaults to 0). trajectory_constraints : list of tuples, optional List of constraints that should hold at each point in the time vector. diff --git a/control/phaseplot.py b/control/phaseplot.py index d785a2221..abc050ffe 100644 --- a/control/phaseplot.py +++ b/control/phaseplot.py @@ -36,8 +36,9 @@ from scipy.integrate import odeint from . import config +from .ctrlplot import ControlPlot, _add_arrows_to_line2D, _get_color, \ + _process_ax_keyword, _update_plot_title from .exception import ControlNotImplemented -from .freqplot import _add_arrows_to_line2D from .nlsys import NonlinearIOSystem, find_eqpt, input_output_response __all__ = ['phase_plane_plot', 'phase_plot', 'box_grid'] @@ -52,7 +53,8 @@ def phase_plane_plot( sys, pointdata=None, timedata=None, gridtype=None, gridspec=None, plot_streamlines=True, plot_vectorfield=False, plot_equilpoints=True, - plot_separatrices=True, ax=None, **kwargs + plot_separatrices=True, ax=None, suppress_warnings=False, title=None, + **kwargs ): """Plot phase plane diagram. @@ -88,49 +90,78 @@ def phase_plane_plot( Parameters to pass to system. For an I/O system, `params` should be a dict of parameters and values. For a callable, `params` should be dict with key 'args' and value given by a tuple (passed to callable). - plot_streamlines : bool or dict + color : matplotlib color spec, optional + Plot all elements in the given color (use `plot_={'color': c}` + to set the color in one element of the phase plot. + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. + + Returns + ------- + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: array of list of :class:`matplotlib.artist.Artist` + objects: + + - lines[0] = list of Line2D objects (streamlines, separatrices). + - lines[1] = Quiver object (vector field arrows). + - lines[2] = list of Line2D objects (equilibrium points). + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + See :class:`ControlPlot` for more detailed information. + + + Other parameters + ---------------- + dir : str, optional + Direction to draw streamlines: 'forward' to flow forward in time + from the reference points, 'reverse' to flow backward in time, or + 'both' to flow both forward and backward. The amount of time to + simulate in each direction is given by the ``timedata`` argument. + plot_streamlines : bool or dict, optional If `True` (default) then plot streamlines based on the pointdata and gridtype. If set to a dict, pass on the key-value pairs in the dict as keywords to :func:`~control.phaseplot.streamlines`. - plot_vectorfield : bool or dict + plot_vectorfield : bool or dict, optional If `True` (default) then plot the vector field based on the pointdata and gridtype. If set to a dict, pass on the key-value pairs in the dict as keywords to :func:`~control.phaseplot.vectorfield`. - plot_equilpoints : bool or dict + plot_equilpoints : bool or dict, optional If `True` (default) then plot equilibrium points based in the phase plot boundary. If set to a dict, pass on the key-value pairs in the dict as keywords to :func:`~control.phaseplot.equilpoints`. - plot_separatrices : bool or dict + plot_separatrices : bool or dict, optional If `True` (default) then plot separatrices starting from each equilibrium point. If set to a dict, pass on the key-value pairs in the dict as keywords to :func:`~control.phaseplot.separatrices`. - color : str - Plot all elements in the given color (use `plot_={'color': c}` - to set the color in one element of the phase plot. - ax : Axes - Use the given axes for the plot instead of creating a new figure. - - Returns - ------- - out : list of list of Artists - out[0] = list of Line2D objects (streamlines and separatrices) - out[1] = Quiver object (vector field arrows) - out[2] = list of Line2D objects (equilibrium points) + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + suppress_warnings : bool, optional + If set to `True`, suppress warning messages in generating trajectories. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). """ # Process arguments params = kwargs.get('params', None) sys = _create_system(sys, params) pointdata = [-1, 1, -1, 1] if pointdata is None else pointdata + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) # Create axis if needed - if ax is None: - fig, ax = plt.gcf(), plt.gca() - else: - fig = None # don't modify figure + user_ax = ax + fig, ax = _process_ax_keyword(user_ax, squeeze=True, rcParams=rcParams) # Create copy of kwargs for later checking to find unused arguments initial_kwargs = dict(kwargs) + passed_kwargs = False # Utility function to create keyword arguments def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): @@ -141,7 +172,7 @@ def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): return new_kwargs # Create list for storing outputs - out = [[], None, None] + out = np.array([[], None, None], dtype=object) # Plot out the main elements if plot_streamlines: @@ -149,7 +180,8 @@ def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): kwargs, plot_streamlines, gridspec=gridspec, gridtype=gridtype, ax=ax) out[0] += streamlines( - sys, pointdata, timedata, check_kwargs=False, **kwargs_local) + sys, pointdata, timedata, _check_kwargs=False, + suppress_warnings=suppress_warnings, **kwargs_local) # Get rid of keyword arguments handled by streamlines for kw in ['arrows', 'arrow_size', 'arrow_style', 'color', @@ -164,7 +196,7 @@ def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): kwargs_local = _create_kwargs( kwargs, plot_separatrices, gridspec=gridspec, ax=ax) out[0] += separatrices( - sys, pointdata, check_kwargs=False, **kwargs_local) + sys, pointdata, _check_kwargs=False, **kwargs_local) # Get rid of keyword arguments handled by separatrices for kw in ['arrows', 'arrow_size', 'arrow_style', 'params']: @@ -174,7 +206,7 @@ def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): kwargs_local = _create_kwargs( kwargs, plot_vectorfield, gridspec=gridspec, ax=ax) out[1] = vectorfield( - sys, pointdata, check_kwargs=False, **kwargs_local) + sys, pointdata, _check_kwargs=False, **kwargs_local) # Get rid of keyword arguments handled by vectorfield for kw in ['color', 'params']: @@ -184,7 +216,7 @@ def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): kwargs_local = _create_kwargs( kwargs, plot_equilpoints, gridspec=gridspec, ax=ax) out[2] = equilpoints( - sys, pointdata, check_kwargs=False, **kwargs_local) + sys, pointdata, _check_kwargs=False, **kwargs_local) # Get rid of keyword arguments handled by equilpoints for kw in ['params']: @@ -194,16 +226,20 @@ def _create_kwargs(global_kwargs, local_kwargs, **other_kwargs): if initial_kwargs: raise TypeError("unrecognized keywords: ", str(initial_kwargs)) - if fig is not None: - fig.suptitle(f"Phase portrait for {sys.name}") + if user_ax is None: + if title is None: + title = f"Phase portrait for {sys.name}" + _update_plot_title(title, use_existing=False, rcParams=rcParams) ax.set_xlabel(sys.state_labels[0]) ax.set_ylabel(sys.state_labels[1]) + plt.tight_layout() - return out + return ControlPlot(out, ax, fig) def vectorfield( - sys, pointdata, gridspec=None, ax=None, check_kwargs=True, **kwargs): + sys, pointdata, gridspec=None, ax=None, suppress_warnings=False, + _check_kwargs=True, **kwargs): """Plot a vector field in the phase plane. This function plots a vector field for a two-dimensional state @@ -235,16 +271,27 @@ def vectorfield( Parameters to pass to system. For an I/O system, `params` should be a dict of parameters and values. For a callable, `params` should be dict with key 'args' and value given by a tuple (passed to callable). - color : str + color : matplotlib color spec, optional Plot the vector field in the given color. - ax : Axes + ax : matplotlib.axes.Axes Use the given axes for the plot, otherwise use the current axes. Returns ------- out : Quiver + Other parameters + ---------------- + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + suppress_warnings : bool, optional + If set to `True`, suppress warning messages in generating trajectories. + """ + # Process keywords + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + # Get system parameters params = kwargs.pop('params', None) @@ -262,10 +309,10 @@ def vectorfield( xlim, ylim, maxlim = _set_axis_limits(ax, pointdata) # Figure out the color to use - color = _get_color(kwargs, ax) + color = _get_color(kwargs, ax=ax) # Make sure all keyword arguments were processed - if check_kwargs and kwargs: + if _check_kwargs and kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) # Generate phase plane (quiver) data @@ -273,18 +320,19 @@ def vectorfield( sys._update_params(params) for i, x in enumerate(points): vfdata[i, :2] = x - vfdata[i, 2:] = sys._rhs(0, x, 0) + vfdata[i, 2:] = sys._rhs(0, x, np.zeros(sys.ninputs)) - out = ax.quiver( - vfdata[:, 0], vfdata[:, 1], vfdata[:, 2], vfdata[:, 3], - angles='xy', color=color) + with plt.rc_context(rcParams): + out = ax.quiver( + vfdata[:, 0], vfdata[:, 1], vfdata[:, 2], vfdata[:, 3], + angles='xy', color=color) return out def streamlines( - sys, pointdata, timedata=1, gridspec=None, gridtype=None, - dir=None, ax=None, check_kwargs=True, **kwargs): + sys, pointdata, timedata=1, gridspec=None, gridtype=None, dir=None, + ax=None, _check_kwargs=True, suppress_warnings=False, **kwargs): """Plot stream lines in the phase plane. This function plots stream lines for a two-dimensional state space @@ -315,20 +363,36 @@ def streamlines( If gridtype is 'circlegrid', then `gridspec` is a 2-tuple specifying the radius and number of points around each point in the `pointdata` array. + dir : str, optional + Direction to draw streamlines: 'forward' to flow forward in time + from the reference points, 'reverse' to flow backward in time, or + 'both' to flow both forward and backward. The amount of time to + simulate in each direction is given by the ``timedata`` argument. params : dict or list, optional Parameters to pass to system. For an I/O system, `params` should be a dict of parameters and values. For a callable, `params` should be dict with key 'args' and value given by a tuple (passed to callable). color : str Plot the streamlines in the given color. - ax : Axes + ax : matplotlib.axes.Axes Use the given axes for the plot, otherwise use the current axes. Returns ------- out : list of Line2D objects + Other parameters + ---------------- + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + suppress_warnings : bool, optional + If set to `True`, suppress warning messages in generating trajectories. + """ + # Process keywords + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + # Get system parameters params = kwargs.pop('params', None) @@ -351,10 +415,10 @@ def streamlines( xlim, ylim, maxlim = _set_axis_limits(ax, pointdata) # Figure out the color to use - color = _get_color(kwargs, ax) + color = _get_color(kwargs, ax=ax) # Make sure all keyword arguments were processed - if check_kwargs and kwargs: + if _check_kwargs and kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) # Create reverse time system, if needed @@ -373,22 +437,22 @@ def streamlines( timepts = _make_timepts(timedata, i) traj = _create_trajectory( sys, revsys, timepts, X0, params, dir, - gridtype=gridtype, gridspec=gridspec, xlim=xlim, ylim=ylim) + gridtype=gridtype, gridspec=gridspec, xlim=xlim, ylim=ylim, + suppress_warnings=suppress_warnings) - # Plot the trajectory + # Plot the trajectory (if there is one) if traj.shape[1] > 1: - out.append( - ax.plot(traj[0], traj[1], color=color)) - - # Add arrows to the lines at specified intervals - _add_arrows_to_line2D( - ax, out[-1][0], arrow_pos, arrowstyle=arrow_style, dir=1) + with plt.rc_context(rcParams): + out += ax.plot(traj[0], traj[1], color=color) + # Add arrows to the lines at specified intervals + _add_arrows_to_line2D( + ax, out[-1], arrow_pos, arrowstyle=arrow_style, dir=1) return out def equilpoints( - sys, pointdata, gridspec=None, color='k', ax=None, check_kwargs=True, + sys, pointdata, gridspec=None, color='k', ax=None, _check_kwargs=True, **kwargs): """Plot equilibrium points in the phase plane. @@ -422,14 +486,23 @@ def equilpoints( dict with key 'args' and value given by a tuple (passed to callable). color : str Plot the equilibrium points in the given color. - ax : Axes + ax : matplotlib.axes.Axes Use the given axes for the plot, otherwise use the current axes. Returns ------- out : list of Line2D objects + Other parameters + ---------------- + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + """ + # Process keywords + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + # Get system parameters params = kwargs.pop('params', None) @@ -448,7 +521,7 @@ def equilpoints( points, _ = _make_points(pointdata, gridspec, 'meshgrid') # Make sure all keyword arguments were processed - if check_kwargs and kwargs: + if _check_kwargs and kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) # Search for equilibrium points @@ -457,15 +530,15 @@ def equilpoints( # Plot the equilibrium points out = [] for xeq in equilpts: - out.append( - ax.plot(xeq[0], xeq[1], marker='o', color=color)) - + with plt.rc_context(rcParams): + out.append( + ax.plot(xeq[0], xeq[1], marker='o', color=color)) return out def separatrices( sys, pointdata, timedata=None, gridspec=None, ax=None, - check_kwargs=True, **kwargs): + _check_kwargs=True, suppress_warnings=False, **kwargs): """Plot separatrices in the phase plane. This function plots separatrices for a two-dimensional state space @@ -500,16 +573,31 @@ def separatrices( Parameters to pass to system. For an I/O system, `params` should be a dict of parameters and values. For a callable, `params` should be dict with key 'args' and value given by a tuple (passed to callable). - color : str - Plot the streamlines in the given color. - ax : Axes + color : matplotlib color spec, optional + Plot the separatrics in the given color. If a single color + specification is given, this is used for both stable and unstable + separatrices. If a tuple is given, the first element is used as + the color specification for stable separatrices and the second + elmeent for unstable separatrices. + ax : matplotlib.axes.Axes Use the given axes for the plot, otherwise use the current axes. Returns ------- out : list of Line2D objects + Other parameters + ---------------- + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + suppress_warnings : bool, optional + If set to `True`, suppress warning messages in generating trajectories. + """ + # Process keywords + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) + # Get system parameters params = kwargs.pop('params', None) @@ -546,7 +634,7 @@ def separatrices( stable_color = unstable_color = color # Make sure all keyword arguments were processed - if check_kwargs and kwargs: + if _check_kwargs and kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) # Create a "reverse time" system to use for simulation @@ -559,8 +647,9 @@ def separatrices( out = [] for i, xeq in enumerate(equilpts): # Plot the equilibrium points - out.append( - ax.plot(xeq[0], xeq[1], marker='o', color='k')) + with plt.rc_context(rcParams): + out.append( + ax.plot(xeq[0], xeq[1], marker='o', color='k')) # Figure out the linearization and eigenvectors evals, evecs = np.linalg.eig(sys.linearize(xeq, 0, params=params).A) @@ -586,25 +675,29 @@ def separatrices( if evals[j].real < 0: traj = _create_trajectory( sys, revsys, timepts, x0, params, 'reverse', - gridtype='boxgrid', xlim=xlim, ylim=ylim) + gridtype='boxgrid', xlim=xlim, ylim=ylim, + suppress_warnings=suppress_warnings) color = stable_color linestyle = '--' elif evals[j].real > 0: traj = _create_trajectory( sys, revsys, timepts, x0, params, 'forward', - gridtype='boxgrid', xlim=xlim, ylim=ylim) + gridtype='boxgrid', xlim=xlim, ylim=ylim, + suppress_warnings=suppress_warnings) color = unstable_color linestyle = '-' + # Plot the trajectory (if there is one) if traj.shape[1] > 1: - out.append(ax.plot( - traj[0], traj[1], color=color, linestyle=linestyle)) + with plt.rc_context(rcParams): + out.append(ax.plot( + traj[0], traj[1], color=color, linestyle=linestyle)) # Add arrows to the lines at specified intervals - _add_arrows_to_line2D( - ax, out[-1][0], arrow_pos, arrowstyle=arrow_style, - dir=1) - + with plt.rc_context(rcParams): + _add_arrows_to_line2D( + ax, out[-1][0], arrow_pos, arrowstyle=arrow_style, + dir=1) return out @@ -621,13 +714,13 @@ def boxgrid(xvals, yvals): Parameters ---------- - xvals, yvals: 1D array-like + xvals, yvals : 1D array-like Array of points defining the points on the lower and left edges of the box. Returns ------- - grid: 2D array + grid : 2D array Array with shape (p, 2) defining the points along the edges of the box, where p is the number of points around the edge. @@ -650,7 +743,7 @@ def meshgrid(xvals, yvals): Parameters ---------- - xvals, yvals: 1D array-like + xvals, yvals : 1D array-like Array of points defining the points on the lower and left edges of the box. @@ -861,34 +954,23 @@ def _parse_arrow_keywords(kwargs): return arrow_pos, arrow_style -def _get_color(kwargs, ax=None): - if 'color' in kwargs: - return kwargs.pop('color') - - # If we were passed an axis, try to increment color from previous - color_cycle = plt.rcParams['axes.prop_cycle'].by_key()['color'] - if ax is not None: - color_offset = 0 - if len(ax.lines) > 0: - last_color = ax.lines[-1].get_color() - if last_color in color_cycle: - color_offset = color_cycle.index(last_color) + 1 - return color_cycle[color_offset % len(color_cycle)] - else: - return None - - +# TODO: move to ctrlplot? def _create_trajectory( - sys, revsys, timepts, X0, params, dir, + sys, revsys, timepts, X0, params, dir, suppress_warnings=False, gridtype=None, gridspec=None, xlim=None, ylim=None): # Comput ethe forward trajectory if dir == 'forward' or dir == 'both': - fwdresp = input_output_response(sys, timepts, X0=X0, params=params) + fwdresp = input_output_response( + sys, timepts, X0=X0, params=params, ignore_errors=True) + if not fwdresp.success and not suppress_warnings: + warnings.warn(f"{X0=}, {fwdresp.message}") # Compute the reverse trajectory if dir == 'reverse' or dir == 'both': revresp = input_output_response( - revsys, timepts, X0=X0, params=params) + revsys, timepts, X0=X0, params=params, ignore_errors=True) + if not revresp.success and not suppress_warnings: + warnings.warn(f"{X0=}, {revresp.message}") # Create the trace to plot if dir == 'forward': @@ -898,7 +980,14 @@ def _create_trajectory( elif dir == 'both': traj = np.hstack([revresp.states[:, :1:-1], fwdresp.states]) - return traj + # Remove points outside the window (keep first point beyond boundary) + inrange = np.asarray( + (traj[0] >= xlim[0]) & (traj[0] <= xlim[1]) & + (traj[1] >= ylim[0]) & (traj[1] <= ylim[1])) + inrange[:-1] = inrange[:-1] | inrange[1:] # keep if next point in range + inrange[1:] = inrange[1:] | inrange[:-1] # keep if prev point in range + + return traj[:, inrange] def _make_timepts(timepts, i): @@ -924,6 +1013,9 @@ def phase_plot(odefun, X=None, Y=None, scale=1, X0=None, T=None, """(legacy) Phase plot for 2D dynamical systems. + .. deprecated:: 0.10.1 + This function is deprecated; use :func:`phase_plane_plot` instead. + Produces a vector field or stream line plot for a planar system. This function has been replaced by the :func:`~control.phase_plane_map` and :func:`~control.phase_plane_plot` functions. @@ -983,7 +1075,7 @@ def phase_plot(odefun, X=None, Y=None, scale=1, X0=None, T=None, """ # Generate a deprecation warning warnings.warn( - "phase_plot is deprecated; use phase_plot_plot instead", + "phase_plot() is deprecated; use phase_plane_plot() instead", FutureWarning) # @@ -1182,14 +1274,18 @@ def phase_plot(odefun, X=None, Y=None, scale=1, X0=None, T=None, def box_grid(xlimp, ylimp): """box_grid generate list of points on edge of box + .. deprecated:: 0.10.0 + Use :func:`phaseplot.boxgrid` instead. + list = box_grid([xmin xmax xnum], [ymin ymax ynum]) generates a list of points that correspond to a uniform grid at the end of the box defined by the corners [xmin ymin] and [xmax ymax]. + """ # Generate a deprecation warning warnings.warn( - "box_grid is deprecated; use phaseplot.boxgrid instead", + "box_grid() is deprecated; use phaseplot.boxgrid() instead", FutureWarning) return boxgrid( @@ -1203,6 +1299,3 @@ def _find(condition): Private implementation of deprecated matplotlib.mlab.find """ return np.nonzero(np.ravel(condition))[0] - - - diff --git a/control/pzmap.py b/control/pzmap.py index d7662d1d9..7b0f8d096 100644 --- a/control/pzmap.py +++ b/control/pzmap.py @@ -18,7 +18,11 @@ from numpy import cos, exp, imag, linspace, real, sin, sqrt from . import config -from .freqplot import _freqplot_defaults, _get_line_labels +from .config import _process_legacy_keyword +from .ctrlplot import ControlPlot, _get_color, _get_color_offset, \ + _get_line_labels, _process_ax_keyword, _process_legend_keywords, \ + _process_line_labels, _update_plot_title +from .freqplot import _freqplot_defaults from .grid import nogrid, sgrid, zgrid from .iosys import isctime, isdtime from .lti import LTI @@ -117,13 +121,6 @@ def plot(self, *args, **kwargs): and keywords. """ - # If this is a root locus plot, use rlocus defaults for grid - if self.loci is not None: - from .rlocus import _rlocus_defaults - kwargs = kwargs.copy() - kwargs['grid'] = config._get_param( - 'rlocus', 'grid', kwargs.get('grid', None), _rlocus_defaults) - return pole_zero_plot(self, *args, **kwargs) @@ -145,7 +142,7 @@ def pole_zero_map(sysdata): Parameters ---------- - sys : LTI system (StateSpace or TransferFunction) + sysdata : LTI system (StateSpace or TransferFunction) Linear system for which poles and zeros are computed. Returns @@ -175,10 +172,9 @@ def pole_zero_map(sysdata): # https://matplotlib.org/2.0.2/examples/axes_grid/demo_axisline_style.html # https://matplotlib.org/2.0.2/examples/axes_grid/demo_curvelinear_grid.html def pole_zero_plot( - data, plot=None, grid=None, title=None, marker_color=None, - marker_size=None, marker_width=None, legend_loc='upper right', - xlim=None, ylim=None, interactive=None, ax=None, scaling=None, - initial_gain=None, **kwargs): + data, plot=None, grid=None, title=None, color=None, marker_size=None, + marker_width=None, xlim=None, ylim=None, interactive=None, ax=None, + scaling=None, initial_gain=None, label=None, **kwargs): """Plot a pole/zero map for a linear system. If the system data include root loci, a root locus diagram for the @@ -189,7 +185,7 @@ def pole_zero_plot( Parameters ---------- - sysdata : List of PoleZeroData objects or LTI systems + data : List of PoleZeroData objects or LTI systems List of pole/zero response data objects generated by pzmap_response() or rootlocus_response() that are to be plotted. If a list of systems is given, the poles and zeros of those systems will be plotted. @@ -206,14 +202,25 @@ def pole_zero_plot( Returns ------- - lines : array of list of Line2D - Array of Line2D objects for each set of markers in the plot. The - shape of the array is given by (nsys, 2) where nsys is the number - of systems or responses passed to the function. The second index - specifies the pzmap object type: + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: Array of :class:`matplotlib.lines.Line2D` objects + for each set of markers in the plot. The shape of the array is + given by (`nsys`, 2) where `nsys` is the number of systems or + responses passed to the function. The second index specifies + the pzmap object type: + + - lines[idx, 0]: poles + - lines[idx, 1]: zeros + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. - * lines[idx, 0]: poles - * lines[idx, 1]: zeros + * cplt.legend: legend object(s) contained in the plot + + See :class:`ControlPlot` for more detailed information. poles, zeros: list of arrays (legacy) If the `plot` keyword is given, the system poles and zeros @@ -221,47 +228,68 @@ def pole_zero_plot( Other Parameters ---------------- - scaling : str or list, optional - Set the type of axis scaling. Can be 'equal' (default), 'auto', or - a list of the form [xmin, xmax, ymin, ymax]. - title : str, optional - Set the title of the plot. Defaults plot type and system name(s). + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. + color : matplotlib color spec, optional + Specify the color of the markers and lines. + initial_gain : float, optional + If given, the specified system gain will be marked on the plot. + interactive : bool, optional + Turn off interactive mode for root locus plots. + label : str or array_like of str, optional + If present, replace automatically generated label(s) with given + label(s). If data is a list, strings should be specified for each + system. + legend_loc : int or str, optional + Include a legend in the given location. Default is 'upper right', + with no legend for a single response. Use False to suppress legend. marker_color : str, optional Set the color of the markers used for poles and zeros. marker_size : int, optional Set the size of the markers used for poles and zeros. marker_width : int, optional Set the line width of the markers used for poles and zeros. - legend_loc : str, optional - For plots with multiple lines, a legend will be included in the - given location. Default is 'center right'. Use False to supress. + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. + scaling : str or list, optional + Set the type of axis scaling. Can be 'equal' (default), 'auto', or + a list of the form [xmin, xmax, ymin, ymax]. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on the + plot or ``legend_loc`` has been specified. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). xlim : list, optional Set the limits for the x axis. ylim : list, optional Set the limits for the y axis. - interactive : bool, optional - Turn off interactive mode for root locus plots. - initial_gain : float, optional - If given, the specified system gain will be marked on the plot. Notes ----- - By default, the pzmap function calls matplotlib.pyplot.axis('equal'), - which means that trying to reset the axis limits may not behave as - expected. To change the axis limits, use the `scaling` keyword of use - matplotlib.pyplot.gca().axis('auto') and then set the axis limits to - the desired values. + 1. By default, the pzmap function calls matplotlib.pyplot.axis('equal'), + which means that trying to reset the axis limits may not behave as + expected. To change the axis limits, use the `scaling` keyword of + use matplotlib.pyplot.gca().axis('auto') and then set the axis + limits to the desired values. + + 2. Pole/zero plots that use the continuous time omega-damping grid do + not work with the ``ax`` keyword argument, due to the way that axes + grids are implemented. The ``grid`` argument must be set to + ``False`` or ``'empty'`` when using the ``ax`` keyword argument. """ # Get parameter values - grid = config._get_param('pzmap', 'grid', grid, _pzmap_defaults) + label = _process_line_labels(label) marker_size = config._get_param('pzmap', 'marker_size', marker_size, 6) marker_width = config._get_param('pzmap', 'marker_width', marker_width, 1.5) - xlim_user, ylim_user = xlim, ylim - freqplot_rcParams = config._get_param( - 'freqplot', 'rcParams', kwargs, _freqplot_defaults, - pop=True, last=True) + user_color = _process_legacy_keyword(kwargs, 'marker_color', 'color', color) + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) user_ax = ax + xlim_user, ylim_user = xlim, ylim # If argument was a singleton, turn it into a tuple if not isinstance(data, (list, tuple)): @@ -288,8 +316,8 @@ def pole_zero_plot( # Legacy return value processing if plot is not None: warnings.warn( - "`pole_zero_plot` return values of poles, zeros is deprecated; " - "use pole_zero_map()", DeprecationWarning) + "pole_zero_plot() return value of poles, zeros is deprecated; " + "use pole_zero_map()", FutureWarning) # Extract out the values that we will eventually return poles = [response.poles for response in pzmap_responses] @@ -302,58 +330,49 @@ def pole_zero_plot( return poles, zeros # Initialize the figure - # TODO: turn into standard utility function (from plotutil.py?) - if user_ax is None: - fig = plt.gcf() - axs = fig.get_axes() - else: - fig = ax.figure - axs = [ax] - - if len(axs) > 1: - # Need to generate a new figure - fig, axs = plt.figure(), [] - - with plt.rc_context(freqplot_rcParams): - if grid and grid != 'empty': - plt.clf() - if all([isctime(dt=response.dt) for response in data]): - ax, fig = sgrid(scaling=scaling) - elif all([isdtime(dt=response.dt) for response in data]): - ax, fig = zgrid(scaling=scaling) - else: - raise ValueError( - "incompatible time bases; don't know how to grid") - # Store the limits for later use - xlim, ylim = ax.get_xlim(), ax.get_ylim() - elif len(axs) == 0: - if grid == 'empty': - # Leave off grid entirely + fig, ax = _process_ax_keyword( + user_ax, rcParams=rcParams, squeeze=True, create_axes=False) + legend_loc, _, show_legend = _process_legend_keywords( + kwargs, None, 'upper right') + + # Make sure there are no remaining keyword arguments + if kwargs: + raise TypeError("unrecognized keywords: ", str(kwargs)) + + if ax is None: + # Determine what type of grid to use + if rlocus_plot: + from .rlocus import _rlocus_defaults + grid = config._get_param('rlocus', 'grid', grid, _rlocus_defaults) + else: + grid = config._get_param('pzmap', 'grid', grid, _pzmap_defaults) + + # Create the axes with the appropriate grid + with plt.rc_context(rcParams): + if grid and grid != 'empty': + if all([isctime(dt=response.dt) for response in data]): + ax, fig = sgrid(scaling=scaling) + elif all([isdtime(dt=response.dt) for response in data]): + ax, fig = zgrid(scaling=scaling) + else: + raise ValueError( + "incompatible time bases; don't know how to grid") + # Store the limits for later use + xlim, ylim = ax.get_xlim(), ax.get_ylim() + elif grid == 'empty': ax = plt.axes() xlim = ylim = [np.inf, -np.inf] # use data to set limits else: - # draw stability boundary; use first response timebase ax, fig = nogrid(data[0].dt, scaling=scaling) xlim, ylim = ax.get_xlim(), ax.get_ylim() - else: - # Use the existing axes and any grid that is there - ax = axs[0] - - # Store the limits for later use - xlim, ylim = ax.get_xlim(), ax.get_ylim() - - # Issue a warning if the user tried to set the grid type - if grid: - warnings.warn("axis already exists; grid keyword ignored") + else: + # Store the limits for later use + xlim, ylim = ax.get_xlim(), ax.get_ylim() + if grid is not None: + warnings.warn("axis already exists; grid keyword ignored") - # Handle color cycle manually as all root locus segments - # of the same system are expected to be of the same color - color_cycle = plt.rcParams['axes.prop_cycle'].by_key()['color'] - color_offset = 0 - if len(ax.lines) > 0: - last_color = ax.lines[-1].get_color() - if last_color in color_cycle: - color_offset = color_cycle.index(last_color) + 1 + # Get color offset for the next line to be drawn + color_offset, color_cycle = _get_color_offset(ax) # Create a list of lines for the output out = np.empty( @@ -366,32 +385,33 @@ def pole_zero_plot( poles = response.poles zeros = response.zeros - # Get the color to use for this system - if marker_color is None: - color = color_cycle[(color_offset + idx) % len(color_cycle)] - else: - color = marker_color + # Get the color to use for this response + color = _get_color(user_color, offset=color_offset + idx) # Plot the locations of the poles and zeros if len(poles) > 0: - label = response.sysname if response.loci is None else None + if label is None: + label_ = response.sysname if response.loci is None else None + else: + label_ = label[idx] out[idx, 0] = ax.plot( real(poles), imag(poles), marker='x', linestyle='', markeredgecolor=color, markerfacecolor=color, markersize=marker_size, markeredgewidth=marker_width, - label=label) + color=color, label=label_) if len(zeros) > 0: out[idx, 1] = ax.plot( real(zeros), imag(zeros), marker='o', linestyle='', markeredgecolor=color, markerfacecolor='none', - markersize=marker_size, markeredgewidth=marker_width) + markersize=marker_size, markeredgewidth=marker_width, + color=color) # Plot the loci, if present if response.loci is not None: + label_ = response.sysname if label is None else label[idx] for locus in response.loci.transpose(): out[idx, 2] += ax.plot( - real(locus), imag(locus), color=color, - label=response.sysname) + real(locus), imag(locus), color=color, label=label_) # Compute the axis limits to use based on the response resp_xlim, resp_ylim = _compute_root_locus_limits(response) @@ -422,7 +442,7 @@ def pole_zero_plot( lines, labels = _get_line_labels(ax) # Add legend if there is more than one system plotted - if len(labels) > 1 and legend_loc is not False: + if show_legend or len(labels) > 1 and show_legend != False: if response.loci is None: # Use "x o" for the system label, via matplotlib tuple handler from matplotlib.legend_handler import HandlerTuple @@ -435,24 +455,28 @@ def pole_zero_plot( markeredgecolor=pole_line.get_markerfacecolor(), markerfacecolor='none', markersize=marker_size, markeredgewidth=marker_width) - handle = (pole_line, zero_line) - line_tuples.append(handle) + handle = (pole_line, zero_line) + line_tuples.append(handle) - with plt.rc_context(freqplot_rcParams): - ax.legend( + with plt.rc_context(rcParams): + legend = ax.legend( line_tuples, labels, loc=legend_loc, handler_map={tuple: HandlerTuple(ndivide=None)}) else: # Regular legend, with lines - with plt.rc_context(freqplot_rcParams): - ax.legend(lines, labels, loc=legend_loc) + with plt.rc_context(rcParams): + legend = ax.legend(lines, labels, loc=legend_loc) + else: + legend = None # Add the title if title is None: - title = "Pole/zero plot for " + ", ".join(labels) + title = ("Root locus plot for " if rlocus_plot + else "Pole/zero plot for ") + ", ".join(labels) if user_ax is None: - with plt.rc_context(freqplot_rcParams): - fig.suptitle(title) + _update_plot_title( + title, fig, rcParams=rcParams, frame='figure', + use_existing=False) # Add dispather to handle choosing a point on the diagram if interactive: @@ -474,7 +498,7 @@ def _click_dispatcher(event): _mark_root_locus_gain(ax, sys, K) # Display the parameters in the axes title - with plt.rc_context(freqplot_rcParams): + with plt.rc_context(rcParams): ax.set_title(_create_root_locus_label(sys, K, s)) ax.figure.canvas.draw() @@ -488,7 +512,7 @@ def _click_dispatcher(event): else: TypeError("system lists not supported with legacy return values") - return out + return ControlPlot(out, ax, fig, legend=legend) # Utility function to find gain corresponding to a click event @@ -496,7 +520,7 @@ def _find_root_locus_gain(event, sys, ax): # Get the current axis limits to set various thresholds xlim, ylim = ax.get_xlim(), ax.get_ylim() - # Catch type error when event click is in the figure but not in an axis + # Catch type error when event click is in the figure but not on curve try: s = complex(event.xdata, event.ydata) K = -1. / sys(s) @@ -504,11 +528,9 @@ def _find_root_locus_gain(event, sys, ax): complex(event.xdata + 0.05 * abs(xlim[1] - xlim[0]), event.ydata)) K_ylim = -1. / sys( complex(event.xdata, event.ydata + 0.05 * abs(ylim[1] - ylim[0]))) - except TypeError: - K = float('inf') - K_xlim = float('inf') - K_ylim = float('inf') + K, s = float('inf'), None + K_xlim = K_ylim = float('inf') # # Compute tolerances for deciding if we clicked on the root locus @@ -526,9 +548,8 @@ def _find_root_locus_gain(event, sys, ax): if abs(K.real) > 1e-8 and abs(K.imag / K.real) < gain_tolerance and \ event.inaxes == ax.axes and K.real > 0.: return K.real, s - else: - return None, s + return None, None # Mark points corresponding to a given gain on root locus plot diff --git a/control/rlocus.py b/control/rlocus.py index ea17ae942..189c2ccd0 100644 --- a/control/rlocus.py +++ b/control/rlocus.py @@ -21,9 +21,10 @@ import matplotlib.pyplot as plt import numpy as np import scipy.signal # signal processing toolbox -from numpy import array, imag, poly1d, real, row_stack, zeros_like +from numpy import array, imag, poly1d, real, vstack, zeros_like from . import config +from .ctrlplot import ControlPlot from .exception import ControlMIMONotImplemented from .iosys import isdtime from .lti import LTI @@ -42,15 +43,15 @@ def root_locus_map(sysdata, gains=None): """Compute the root locus map for an LTI system. Calculate the root locus by finding the roots of 1 + k * G(s) where G - is a linear system with transfer function num(s)/den(s) and each k is - an element of kvect. + is a linear system and k varies over a range of gains. Parameters ---------- - sys : LTI system or list of LTI systems + sysdata : LTI system or list of LTI systems Linear input/output systems (SISO only, for now). - kvect : array_like, optional - Gains to use in computing plot of closed-loop poles. + gains : array_like, optional + Gains to use in computing plot of closed-loop poles. If not given, + gains are chosen to include the main features of the root locus map. Returns ------- @@ -98,20 +99,20 @@ def root_locus_map(sysdata, gains=None): def root_locus_plot( - sysdata, kvect=None, grid=None, plot=None, **kwargs): + sysdata, gains=None, grid=None, plot=None, **kwargs): """Root locus plot. Calculate the root locus by finding the roots of 1 + k * G(s) where G - is a linear system with transfer function num(s)/den(s) and each k is - an element of kvect. + is a linear system and k varies over a range of gains. Parameters ---------- sysdata : PoleZeroMap or LTI object or list Linear input/output systems (SISO only, for now). - kvect : array_like, optional - Gains to use in computing plot of closed-loop poles. + gains : array_like, optional + Gains to use in computing plot of closed-loop poles. If not given, + gains are chosen to include the main features of the root locus map. xlim : tuple or list, optional Set limits of x axis, normally with tuple (see :doc:`matplotlib:api/axes_api`). @@ -126,29 +127,57 @@ def root_locus_plot( for continuous time systems, unit circle for discrete time systems. If `empty`, do not draw any additonal lines. Default value is set by config.default['rlocus.grid']. - ax : :class:`matplotlib.axes.Axes` - Axes on which to create root locus plot initial_gain : float, optional Mark the point on the root locus diagram corresponding to the given gain. + color : matplotlib color spec, optional + Specify the color of the markers and lines. Returns ------- - lines : array of list of Line2D - Array of Line2D objects for each set of markers in the plot. The - shape of the array is given by (nsys, 3) where nsys is the number - of systems or responses passed to the function. The second index - specifies the object type: + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: - * lines[idx, 0]: poles - * lines[idx, 1]: zeros - * lines[idx, 2]: loci + * cplt.lines: Array of :class:`matplotlib.lines.Line2D` objects + for each set of markers in the plot. The shape of the array is + given by (nsys, 3) where nsys is the number of systems or + responses passed to the function. The second index specifies + the object type: + + - lines[idx, 0]: poles + - lines[idx, 1]: zeros + - lines[idx, 2]: loci + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + See :class:`ControlPlot` for more detailed information. roots, gains : ndarray - (legacy) If the `plot` keyword is given, returns the - closed-loop root locations, arranged such that each row - corresponds to a gain in gains, and the array of gains (ame as - kvect keyword argument if provided). + (legacy) If the `plot` keyword is given, returns the closed-loop + root locations, arranged such that each row corresponds to a gain, + and the array of gains (same as `gains` keyword argument if provided). + + Other Parameters + ---------------- + ax : matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified and + the current figure has a single axes, that axes is used. + Otherwise, a new figure is created. + label : str or array_like of str, optional + If present, replace automatically generated label(s) with the given + label(s). If sysdata is a list, strings should be specified for each + system. + legend_loc : int or str, optional + Include a legend in the given location. Default is 'center right', + with no legend for a single response. Use False to suppress legend. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on the + plot or ``legend_loc`` has been specified. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). Notes ----- @@ -160,13 +189,14 @@ def root_locus_plot( """ from .pzmap import pole_zero_plot - # Set default parameters - grid = config._get_param('rlocus', 'grid', grid, _rlocus_defaults) + # Legacy parameters + for oldkey in ['kvect', 'k']: + gains = config._process_legacy_keyword(kwargs, oldkey, 'gains', gains) if isinstance(sysdata, list) and all( [isinstance(sys, LTI) for sys in sysdata]) or \ isinstance(sysdata, LTI): - responses = root_locus_map(sysdata, gains=kvect) + responses = root_locus_map(sysdata, gains=gains) else: responses = sysdata @@ -178,20 +208,20 @@ def root_locus_plot( # if plot is not None: warnings.warn( - "`root_locus` return values of roots, gains is deprecated; " - "use root_locus_map()", DeprecationWarning) + "root_locus() return value of roots, gains is deprecated; " + "use root_locus_map()", FutureWarning) if plot is False: return responses.loci, responses.gains # Plot the root loci - out = responses.plot(grid=grid, **kwargs) + cplt = responses.plot(grid=grid, **kwargs) # Legacy processing: return locations of poles and zeros as a tuple if plot is True: return responses.loci, responses.gains - return out + return ControlPlot(cplt.lines, cplt.axes, cplt.figure) def _default_gains(num, den, xlim, ylim): @@ -419,7 +449,7 @@ def _RLFindRoots(nump, denp, kvect): curroots.sort() roots.append(curroots) - return row_stack(roots) + return vstack(roots) def _RLSortRoots(roots): diff --git a/control/robust.py b/control/robust.py index 75930e59e..f9283af48 100644 --- a/control/robust.py +++ b/control/robust.py @@ -52,13 +52,17 @@ def h2syn(P, nmeas, ncon): Parameters ---------- - P: partitioned lti plant (State-space sys) - nmeas: number of measurements (input to controller) - ncon: number of control inputs (output from controller) + P : StateSpace + Partitioned LTI plant (state-space system). + nmeas : int + Number of measurements (input to controller). + ncon : int + Number of control inputs (output from controller). Returns ------- - K: controller to stabilize P (State-space sys) + K : StateSpace + Controller to stabilize `P`. Raises ------ @@ -73,7 +77,7 @@ def h2syn(P, nmeas, ncon): -------- >>> # Unstable first order SISI system >>> G = ct.tf([1], [1, -1], inputs=['u'], outputs=['y']) - >>> max(G.poles()) < 0 # Is G stable? + >>> all(G.poles() < 0) # Is G stable? False >>> # Create partitioned system with trivial unity systems @@ -87,7 +91,7 @@ def h2syn(P, nmeas, ncon): >>> # Synthesize H2 optimal stabilizing controller >>> K = ct.h2syn(P, nmeas=1, ncon=1) >>> T = ct.feedback(G, K, sign=1) - >>> max(T.poles()) < 0 # Is T stable? + >>> all(T.poles() < 0) # Is T stable? True """ @@ -121,25 +125,32 @@ def h2syn(P, nmeas, ncon): def hinfsyn(P, nmeas, ncon): + # TODO: document significance of rcond """H_{inf} control synthesis for plant P. Parameters ---------- - P: partitioned lti plant - nmeas: number of measurements (input to controller) - ncon: number of control inputs (output from controller) + P : StateSpace + Partitioned LTI plant (state-space system). + nmeas : int + Number of measurements (input to controller). + ncon : int + Number of control inputs (output from controller). Returns ------- - K: controller to stabilize P (State-space sys) - CL: closed loop system (State-space sys) - gam: infinity norm of closed loop system - rcond: 4-vector, reciprocal condition estimates of: - 1: control transformation matrix - 2: measurement transformation matrix - 3: X-Riccati equation - 4: Y-Riccati equation - TODO: document significance of rcond + K : StateSpace + Controller to stabilize `P`. + CL : StateSpace + Closed loop system. + gam : float + Infinity norm of closed loop system. + rcond : list + 4-vector, reciprocal condition estimates of: + 1: control transformation matrix + 2: measurement transformation matrix + 3: X-Riccati equation + 4: Y-Riccati equation Raises ------ @@ -154,7 +165,7 @@ def hinfsyn(P, nmeas, ncon): -------- >>> # Unstable first order SISI system >>> G = ct.tf([1], [1,-1], inputs=['u'], outputs=['y']) - >>> max(G.poles()) < 0 + >>> all(G.poles() < 0) False >>> # Create partitioned system with trivial unity systems @@ -167,7 +178,7 @@ def hinfsyn(P, nmeas, ncon): >>> # Synthesize Hinf optimal stabilizing controller >>> K, CL, gam, rcond = ct.hinfsyn(P, nmeas=1, ncon=1) >>> T = ct.feedback(G, K, sign=1) - >>> max(T.poles()) < 0 + >>> all(T.poles() < 0) True """ @@ -263,18 +274,18 @@ def augw(g, w1=None, w2=None, w3=None): Parameters ---------- - g: LTI object, ny-by-nu - Plant - w1: None, scalar, or k1-by-ny LTI object - Weighting on S - w2: None, scalar, or k2-by-nu LTI object - Weighting on KS - w3: None, scalar, or k3-by-ny LTI object - Weighting on T + g : LTI object, ny-by-nu + Plant. + w1 : None, scalar, or k1-by-ny LTI object + Weighting on S. + w2 : None, scalar, or k2-by-nu LTI object + Weighting on KS. + w3 : None, scalar, or k3-by-ny LTI object + Weighting on T. Returns ------- - p: StateSpace + p : StateSpace Plant augmented with weightings, suitable for submission to hinfsyn or h2syn. @@ -375,20 +386,20 @@ def mixsyn(g, w1=None, w2=None, w3=None): Parameters ---------- - g: LTI - The plant for which controller must be synthesized - w1: None, or scalar or k1-by-ny LTI - Weighting on S = (1+G*K)**-1 - w2: None, or scalar or k2-by-nu LTI - Weighting on K*S - w3: None, or scalar or k3-by-ny LTI - Weighting on T = G*K*(1+G*K)**-1; + g : LTI + The plant for which controller must be synthesized. + w1 : None, or scalar or k1-by-ny LTI + Weighting on S = (1+G*K)**-1. + w2 : None, or scalar or k2-by-nu LTI + Weighting on K*S. + w3 : None, or scalar or k3-by-ny LTI + Weighting on T = G*K*(1+G*K)**-1. Returns ------- - k: StateSpace - Synthesized controller; - cl: StateSpace + k : StateSpace + Synthesized controller. + cl : StateSpace Closed system mapping evaluation inputs to evaluation outputs. Let p be the augmented plant, with:: @@ -400,10 +411,10 @@ def mixsyn(g, w1=None, w2=None, w3=None): info: tuple gamma: scalar - H-infinity norm of cl + H-infinity norm of cl. rcond: array - Estimates of reciprocal condition numbers - computed during synthesis. See hinfsyn for details + Estimates of reciprocal condition numbers computed during + synthesis. See hinfsyn for details. If a weighting w is scalar, it will be replaced by I*w, where I is ny-by-ny for w1 and w3, and nu-by-nu for w2. @@ -411,6 +422,7 @@ def mixsyn(g, w1=None, w2=None, w3=None): See Also -------- hinfsyn, augw + """ nmeas = g.noutputs ncon = g.ninputs diff --git a/control/sisotool.py b/control/sisotool.py index aca36e2d1..f34b210c6 100644 --- a/control/sisotool.py +++ b/control/sisotool.py @@ -136,7 +136,7 @@ def sisotool(sys, initial_gain=None, xlim_rlocus=None, ylim_rlocus=None, # ax=fig.axes[1]) ax_rlocus = fig.axes[1] root_locus_map(sys[0, 0]).plot( - xlim=xlim_rlocus, ylim=ylim_rlocus, grid=rlocus_grid, + xlim=xlim_rlocus, ylim=ylim_rlocus, initial_gain=initial_gain, ax=ax_rlocus) if rlocus_grid is False: # Need to generate grid manually, since root_locus_plot() won't @@ -324,30 +324,30 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r', ---------- plant : :class:`LTI` (:class:`TransferFunction` or :class:`StateSpace` system) The dynamical system to be controlled. - gain : string (optional) + gain : string, optional Which gain to vary by `deltaK`. Must be one of `'P'`, `'I'`, or `'D'` (proportional, integral, or derative). - sign : int (optional) + sign : int, optional The sign of deltaK gain perturbation. - input : string (optional) + input_signal : string, optional The input used for the step response; must be `'r'` (reference) or `'d'` (disturbance) (see figure above). - Kp0, Ki0, Kd0 : float (optional) + Kp0, Ki0, Kd0 : float, optional Initial values for proportional, integral, and derivative gains, respectively. - deltaK : float (optional) + deltaK : float, optional Perturbation value for gain specified by the `gain` keywoard. - tau : float (optional) + tau : float, optional The time constant associated with the pole in the continuous-time derivative term. This is required to make the derivative transfer function proper. - C_ff : float or :class:`LTI` system (optional) + C_ff : float or :class:`LTI` system, optional Feedforward controller. If :class:`LTI`, must have timebase that is compatible with plant. - derivative_in_feedback_path : bool (optional) + derivative_in_feedback_path : bool, optional Whether to place the derivative term in feedback transfer function `C_b` instead of the forward transfer function `C_f`. - plot : bool (optional) + plot : bool, optional Whether to create Sisotool interactive plot. Returns diff --git a/control/statefbk.py b/control/statefbk.py index 15bba5454..16eeb36ee 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -150,7 +150,7 @@ def place_varga(A, B, p, dtime=False, alpha=None): """Place closed loop eigenvalues. K = place_varga(A, B, p, dtime=False, alpha=None) - Required Parameters + Parameters ---------- A : 2D array_like Dynamics matrix @@ -158,14 +158,10 @@ def place_varga(A, B, p, dtime=False, alpha=None): Input matrix p : 1D array_like Desired eigenvalue locations - - Optional Parameters - --------------- - dtime : bool + dtime : bool, optional False for continuous time pole placement or True for discrete time. The default is dtime=False. - - alpha : double scalar + alpha : float, optional If `dtime` is false then place_varga will leave the eigenvalues with real part less than alpha untouched. If `dtime` is true then place_varga will leave eigenvalues with modulus less than alpha @@ -179,26 +175,27 @@ def place_varga(A, B, p, dtime=False, alpha=None): K : 2D array (or matrix) Gain such that A - B K has eigenvalues given in p. - Algorithm - --------- + See Also + -------- + place, acker + + Notes + ----- This function is a wrapper for the slycot function sb01bd, which - implements the pole placement algorithm of Varga [1]. In contrast to the + implements the pole placement algorithm of Varga [1]_. In contrast to the algorithm used by place(), the Varga algorithm can place multiple poles at the same location. The placement, however, may not be as robust. - [1] Varga A. "A Schur method for pole assignment." IEEE Trans. Automatic - Control, Vol. AC-26, pp. 517-519, 1981. + References + ---------- + .. [1] Varga A. "A Schur method for pole assignment." IEEE Trans. Automatic + Control, Vol. AC-26, pp. 517-519, 1981. Examples -------- >>> A = [[-1, -1], [0, 1]] >>> B = [[0], [1]] - >>> K = place_varga(A, B, [-2, -5]) - - See Also: - -------- - place, acker - + >>> K = ct.place_varga(A, B, [-2, -5]) """ # Make sure that SLICOT is installed @@ -269,6 +266,10 @@ def acker(A, B, poles): ------- K : 2D array (or matrix) Gains such that A - B K has given eigenvalues + + See Also + -------- + place, place_varga """ # Convert the inputs to matrices @@ -437,7 +438,7 @@ def lqr(*args, **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") + X, L, G = care(A, B, Q, R, N, None, method=method, _Ss="N") return G, X, L @@ -574,7 +575,7 @@ def dlqr(*args, **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") + S, E, K = dare(A, B, Q, R, N, method=method, _Ss="N") return _ssmatrix(K), _ssmatrix(S), E @@ -715,10 +716,10 @@ def create_statefbk_iosystem( specified as either integer offsets or as estimator/system output signal names. If not specified, defaults to the system states. - inputs, outputs : str, or list of str, optional + inputs, outputs, states : 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 - original system. + system. If not given, the inputs, outputs, and states are the same + as the original system. name : string, optional System name. If unspecified, a generic name is generated @@ -1010,7 +1011,7 @@ def ctrb(A, B, t=None): >>> G = ct.tf2ss([1], [1, 2, 3]) >>> C = ct.ctrb(G.A, G.B) >>> np.linalg.matrix_rank(C) - 2 + np.int64(2) """ @@ -1052,7 +1053,7 @@ def obsv(A, C, t=None): >>> G = ct.tf2ss([1], [1, 2, 3]) >>> C = ct.obsv(G.A, G.C) >>> np.linalg.matrix_rank(C) - 2 + np.int64(2) """ diff --git a/control/statesp.py b/control/statesp.py index e14a8358a..aa1c7221b 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -48,26 +48,28 @@ """ import math +from copy import deepcopy +from warnings import warn +from collections.abc import Iterable + import numpy as np -from numpy import any, asarray, concatenate, cos, delete, \ - empty, exp, eye, isinf, ones, pad, sin, zeros, squeeze -from numpy.random import rand, randn -from numpy.linalg import solve, eigvals, matrix_rank -from numpy.linalg.linalg import LinAlgError import scipy as sp import scipy.linalg -from scipy.signal import cont2discrete +from numpy import (any, asarray, concatenate, cos, delete, empty, exp, eye, + isinf, ones, pad, sin, squeeze, zeros) +from numpy.linalg import LinAlgError, eigvals, matrix_rank, solve +from numpy.random import rand, randn from scipy.signal import StateSpace as signalStateSpace -from warnings import warn +from scipy.signal import cont2discrete -from .exception import ControlSlycot, slycot_check, ControlMIMONotImplemented +from . import config +from .exception import ControlMIMONotImplemented, ControlSlycot, slycot_check from .frdata import FrequencyResponseData +from .iosys import (InputOutputSystem, _process_dt_keyword, + _process_iosys_keywords, _process_signal_list, + common_timebase, isdtime, issiso) from .lti import LTI, _process_frequency_response -from .iosys import InputOutputSystem, common_timebase, isdtime, issiso, \ - _process_iosys_keywords, _process_dt_keyword, _process_signal_list -from .nlsys import NonlinearIOSystem, InterconnectedSystem -from . import config -from copy import deepcopy +from .nlsys import InterconnectedSystem, NonlinearIOSystem try: from slycot import ab13dd @@ -288,9 +290,9 @@ def __init__(self, *args, **kwargs): raise ValueError("A and B must have the same number of rows.") if self.nstates != C.shape[1]: raise ValueError("A and C must have the same number of columns.") - if self.ninputs != B.shape[1]: + if self.ninputs != B.shape[1] or self.ninputs != D.shape[1]: raise ValueError("B and D must have the same number of columns.") - if self.noutputs != C.shape[0]: + if self.noutputs != C.shape[0] or self.noutputs != D.shape[0]: raise ValueError("C and D must have the same number of rows.") # @@ -353,13 +355,13 @@ def __init__(self, *args, **kwargs): def _get_states(self): warn("The StateSpace `states` attribute will be deprecated in a " "future release. Use `nstates` instead.", - DeprecationWarning, stacklevel=2) + FutureWarning, stacklevel=2) return self.nstates def _set_states(self, value): warn("The StateSpace `states` attribute will be deprecated in a " "future release. Use `nstates` instead.", - DeprecationWarning, stacklevel=2) + FutureWarning, stacklevel=2) self.nstates = value #: Deprecated attribute; use :attr:`nstates` instead. @@ -904,7 +906,7 @@ def freqresp(self, omega): warn("StateSpace.freqresp(omega) will be removed in a " "future release of python-control; use " "sys.frequency_response(omega), or freqresp(sys, omega) in the " - "MATLAB compatibility module instead", DeprecationWarning) + "MATLAB compatibility module instead", FutureWarning) return self.frequency_response(omega) # Compute poles and zeros @@ -1214,17 +1216,23 @@ def append(self, other): def __getitem__(self, indices): """Array style access""" - if len(indices) != 2: + if not isinstance(indices, Iterable) or len(indices) != 2: raise IOError('must provide indices of length 2 for state space') - outdx = indices[0] if isinstance(indices[0], list) else [indices[0]] - inpdx = indices[1] if isinstance(indices[1], list) else [indices[1]] + outdx, inpdx = indices + + # Convert int to slice to ensure that numpy doesn't drop the dimension + if isinstance(outdx, int): outdx = slice(outdx, outdx+1, 1) + if isinstance(inpdx, int): inpdx = slice(inpdx, inpdx+1, 1) + + if not isinstance(outdx, slice) or not isinstance(inpdx, slice): + raise TypeError(f"system indices must be integers or slices") + sysname = config.defaults['iosys.indexed_system_name_prefix'] + \ self.name + config.defaults['iosys.indexed_system_name_suffix'] return StateSpace( self.A, self.B[:, inpdx], self.C[outdx, :], self.D[outdx, inpdx], - self.dt, name=sysname, - inputs=[self.input_labels[i] for i in list(inpdx)], - outputs=[self.output_labels[i] for i in list(outdx)]) + self.dt, name=sysname, + inputs=self.input_labels[inpdx], outputs=self.output_labels[outdx]) def sample(self, Ts, method='zoh', alpha=None, prewarp_frequency=None, name=None, copy_names=True, **kwargs): @@ -1568,6 +1576,10 @@ def ss(*args, **kwargs): name : string, optional System name (used for specifying signals). If unspecified, a generic name is generated with a unique integer id. + 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' (SISO only). Returns ------- @@ -1606,8 +1618,8 @@ def ss(*args, **kwargs): if len(args) > 0 and (hasattr(args[0], '__call__') or args[0] is None) \ and not isinstance(args[0], (InputOutputSystem, LTI)): # Function as first (or second) argument => assume nonlinear IO system - warn("using ss to create nonlinear I/O systems is deprecated; " - "use nlsys()", DeprecationWarning) + warn("using ss() to create nonlinear I/O systems is deprecated; " + "use nlsys()", FutureWarning) return NonlinearIOSystem(*args, **kwargs) elif len(args) == 4 or len(args) == 5: @@ -1655,7 +1667,7 @@ def ss2io(*args, **kwargs): Create an :class:`~control.StateSpace` system with the given signal and system names. See :func:`~control.ss` for more details. """ - warn("ss2io is deprecated; use ss()", DeprecationWarning) + warn("ss2io() is deprecated; use ss()", FutureWarning) return StateSpace(*args, **kwargs) @@ -1731,7 +1743,7 @@ def tf2io(*args, **kwargs): (2, 2, 8) """ - warn("tf2io is deprecated; use tf2ss() or tf()", DeprecationWarning) + warn("tf2io() is deprecated; use tf2ss() or tf()", FutureWarning) return tf2ss(*args, **kwargs) @@ -1907,15 +1919,12 @@ def rss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs): 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 `s[i]` (where `s` is one of `u`, `y`, or `x`). - outputs : int, list of str, or None - Description of the system outputs. Same format as `inputs`. - states : int, list of str, or None - Description of the system states. Same format as `inputs`. + states, outputs, inputs : int, list of str, or None + Description of the system states, outputs, and 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 's[i]' (where 's' is one of 'x', + 'y', or 'u'). strictly_proper : bool, optional If set to 'True', returns a proper system (no direct term). dt : None, True or float, optional @@ -2221,9 +2230,10 @@ def _convert_to_statespace(sys, use_prefix_suffix=False, method=None): by the calling function. """ - from .xferfcn import TransferFunction import itertools + from .xferfcn import TransferFunction + if isinstance(sys, StateSpace): return sys diff --git a/control/stochsys.py b/control/stochsys.py index fe11a4fb5..b31083f19 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -177,7 +177,7 @@ def lqe(*args, **kwargs): # Compute the result (dimension and symmetry checking done in care()) P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN, method=method, - B_s="C", Q_s="QN", R_s="RN", S_s="NN") + _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN") return _ssmatrix(LT.T), _ssmatrix(P), E @@ -297,7 +297,7 @@ def dlqe(*args, **kwargs): # Compute the result (dimension and symmetry checking done in dare()) P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method, - B_s="C", Q_s="QN", R_s="RN", S_s="NN") + _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN") return _ssmatrix(LT.T), _ssmatrix(P), E @@ -604,6 +604,21 @@ def white_noise(T, Q, dt=0): covariance Q at each point in time (without any scaling based on the sample time). + Parameters + ---------- + T : 1D array_like + Array of linearly spaced times. + Q : 2D array_like + Noise intensity matrix of dimension nxn. + dt : float, optional + If 0, generate continuous time noise signal, otherwise discrete time. + + Returns + ------- + V : array + Noise signal indexed as `V[i, j]` where `i` is the signal index and + `j` is the time index. + """ # Convert input arguments to arrays T = np.atleast_1d(T) diff --git a/control/sysnorm.py b/control/sysnorm.py index f5e583dcf..6737dc5c0 100644 --- a/control/sysnorm.py +++ b/control/sysnorm.py @@ -117,7 +117,7 @@ def norm(system, p=2, tol=1e-6, print_warning=True, method=None): >>> round(ct.norm(Gc, 2), 3) 0.5 >>> round(ct.norm(Gc, 'inf', tol=1e-5, method='scipy'), 3) - 1.0 + np.float64(1.0) """ if not isinstance(system, (ct.StateSpace, ct.TransferFunction)): diff --git a/control/tests/bdalg_test.py b/control/tests/bdalg_test.py index 2ed793ef2..5629f27f9 100644 --- a/control/tests/bdalg_test.py +++ b/control/tests/bdalg_test.py @@ -269,7 +269,7 @@ def test_feedback_args(self, tsys): def testConnect(self, tsys): sys = append(tsys.sys2, tsys.sys3) # two siso systems - with pytest.warns(DeprecationWarning, match="use `interconnect`"): + with pytest.warns(FutureWarning, match="use interconnect()"): # should not raise error connect(sys, [[1, 2], [2, -2]], [2], [1, 2]) connect(sys, [[1, 2], [2, 0]], [2], [1, 2]) @@ -316,3 +316,49 @@ def testConnect(self, tsys): connect(sys, Q, [2], [1, 0]) with pytest.raises(IndexError): connect(sys, Q, [2], [1, -1]) + + +@pytest.mark.parametrize( + "op, nsys, ninputs, noutputs, nstates", [ + (ctrl.series, 2, 1, 1, 4), + (ctrl.parallel, 2, 1, 1, 4), + (ctrl.feedback, 2, 1, 1, 4), + (ctrl.append, 2, 2, 2, 4), + (ctrl.negate, 1, 1, 1, 2), + ]) +def test_bdalg_update_names(op, nsys, ninputs, noutputs, nstates): + syslist = [ctrl.rss(2, 1, 1), ctrl.rss(2, 1, 1)] + inputs = ['in1', 'in2'] + outputs = ['out1', 'out2'] + states = ['x1', 'x2', 'x3', 'x4'] + + newsys = op( + *syslist[:nsys], name='newsys', inputs=inputs[:ninputs], + outputs=outputs[:noutputs], states=states[:nstates]) + assert newsys.name == 'newsys' + assert newsys.ninputs == ninputs + assert newsys.input_labels == inputs[:ninputs] + assert newsys.noutputs == noutputs + assert newsys.output_labels == outputs[:noutputs] + assert newsys.nstates == nstates + assert newsys.state_labels == states[:nstates] + + +def test_bdalg_udpate_names_errors(): + sys1 = ctrl.rss(2, 1, 1) + sys2 = ctrl.rss(2, 1, 1) + + with pytest.raises(ValueError, match="number of inputs does not match"): + sys = ctrl.series(sys1, sys2, inputs=2) + + with pytest.raises(ValueError, match="number of outputs does not match"): + sys = ctrl.series(sys1, sys2, outputs=2) + + with pytest.raises(ValueError, match="number of states does not match"): + sys = ctrl.series(sys1, sys2, states=2) + + with pytest.raises(ValueError, match="number of states does not match"): + sys = ctrl.series(ctrl.tf(sys1), ctrl.tf(sys2), states=2) + + with pytest.raises(TypeError, match="unrecognized keywords"): + sys = ctrl.series(sys1, sys2, dt=1) diff --git a/control/tests/conftest.py b/control/tests/conftest.py index 2330e3818..004b96058 100644 --- a/control/tests/conftest.py +++ b/control/tests/conftest.py @@ -65,10 +65,10 @@ def legacy_plot_signature(): import warnings warnings.filterwarnings( 'ignore', message='passing systems .* is deprecated', - category=DeprecationWarning) + category=FutureWarning) warnings.filterwarnings( - 'ignore', message='.* return values of .* is deprecated', - category=DeprecationWarning) + 'ignore', message='.* return value of .* is deprecated', + category=FutureWarning) yield warnings.resetwarnings() diff --git a/control/tests/ctrlplot_test.py b/control/tests/ctrlplot_test.py new file mode 100644 index 000000000..78ec7d895 --- /dev/null +++ b/control/tests/ctrlplot_test.py @@ -0,0 +1,831 @@ +# ctrlplot_test.py - test out control plotting utilities +# RMM, 27 Jun 2024 + +import inspect +import itertools +import warnings + +import matplotlib.pyplot as plt +import numpy as np +import pytest + +import control as ct + +# List of all plotting functions +resp_plot_fcns = [ + # response function plotting function + (ct.frequency_response, ct.bode_plot), + (ct.frequency_response, ct.nichols_plot), + (ct.singular_values_response, ct.singular_values_plot), + (ct.gangof4_response, ct.gangof4_plot), + (ct.describing_function_response, ct.describing_function_plot), + (None, ct.phase_plane_plot), + (ct.pole_zero_map, ct.pole_zero_plot), + (ct.nyquist_response, ct.nyquist_plot), + (ct.root_locus_map, ct.root_locus_plot), + (ct.initial_response, ct.time_response_plot), + (ct.step_response, ct.time_response_plot), + (ct.impulse_response, ct.time_response_plot), + (ct.forced_response, ct.time_response_plot), + (ct.input_output_response, ct.time_response_plot), +] + +nolabel_plot_fcns = [ct.describing_function_plot, ct.phase_plane_plot] +legacy_plot_fcns = [ct.gangof4_plot] +multiaxes_plot_fcns = [ct.bode_plot, ct.gangof4_plot, ct.time_response_plot] +deprecated_fcns = [ct.phase_plot] + + +# Utility function to make sure legends are OK +def assert_legend(cplt, expected_texts): + # Check to make sure the labels are OK in legend + legend = None + for ax in cplt.axes.flatten(): + legend = ax.get_legend() + if legend is not None: + break + if expected_texts is None: + assert legend is None + else: + assert legend is not None + legend_texts = [entry.get_text() for entry in legend.get_texts()] + assert legend_texts == expected_texts + + +def setup_plot_arguments(resp_fcn, plot_fcn, compute_time_response=True): + # Create some systems to use + sys1 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[1]") + sys1c = ct.rss(2, 1, 1, strictly_proper=True, name="sys[1]_C") + sys2 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[2]") + + # Set up arguments + kwargs = resp_kwargs = plot_kwargs = meth_kwargs = {} + argsc = None + match resp_fcn, plot_fcn: + case ct.describing_function_response, _: + sys1 = ct.tf([1], [1, 2, 2, 1], name="sys[1]") + sys2 = ct.tf([1.1], [1, 2, 2, 1], name="sys[2]") + F = ct.descfcn.saturation_nonlinearity(1) + amp = np.linspace(1, 4, 10) + args1 = (sys1, F, amp) + args2 = (sys2, F, amp) + resp_kwargs = plot_kwargs = {'refine': False} + + case ct.gangof4_response, _: + args1 = (sys1, sys1c) + args2 = (sys2, sys1c) + default_labels = ["P=sys[1]", "P=sys[2]"] + + case ct.frequency_response, ct.nichols_plot: + args1 = (sys1, None) # to allow *fmt in linestyle test + args2 = (sys2, ) + meth_kwargs = {'plot_type': 'nichols'} + + case ct.frequency_response, ct.bode_plot: + args1 = (sys1, None) # to allow *fmt in linestyle test + args2 = (sys2, ) + + case ct.singular_values_response, ct.singular_values_plot: + args1 = (sys1, None) # to allow *fmt in linestyle test + args2 = (sys2, ) + + case ct.root_locus_map, ct.root_locus_plot: + args1 = (sys1, ) + args2 = (sys2, ) + plot_kwargs = {'interactive': False} + + case (ct.forced_response | ct.input_output_response, _): + timepts = np.linspace(1, 10) + U = np.sin(timepts) + if compute_time_response: + args1 = (resp_fcn(sys1, timepts, U), ) + args2 = (resp_fcn(sys2, timepts, U), ) + argsc = (resp_fcn([sys1, sys2], timepts, U), ) + else: + args1 = (sys1, timepts, U) + args2 = (sys2, timepts, U) + argsc = None + + case (ct.impulse_response | ct.initial_response | ct.step_response, _): + if compute_time_response: + args1 = (resp_fcn(sys1), ) + args2 = (resp_fcn(sys2), ) + argsc = (resp_fcn([sys1, sys2]), ) + else: + args1 = (sys1, ) + args2 = (sys2, ) + argsc = ([sys1, sys2], ) + + case _, _: + args1 = (sys1, ) + args2 = (sys2, ) + + return args1, args2, argsc, kwargs, meth_kwargs, plot_kwargs, resp_kwargs + + +# Make sure we didn't miss any plotting functions +def test_find_respplot_functions(): + # Get the list of plotting functions + plot_fcns = {respplot[1] for respplot in resp_plot_fcns} + + # Look through every object in the package + found = 0 + for name, obj in inspect.getmembers(ct): + # Skip anything that is outside of this module + if inspect.getmodule(obj) is not None and \ + not inspect.getmodule(obj).__name__.startswith('control'): + # Skip anything that isn't part of the control package + continue + + # Only look for non-deprecated functions ending in 'plot' + if not inspect.isfunction(obj) or name[-4:] != 'plot' or \ + obj in deprecated_fcns: + continue + + # Make sure that we have this on our list of functions + assert obj in plot_fcns + found += 1 + + assert found == len(plot_fcns) + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_plot_ax_processing(resp_fcn, plot_fcn): + # Set up arguments + args, _, _, kwargs, meth_kwargs, plot_kwargs, resp_kwargs = \ + setup_plot_arguments(resp_fcn, plot_fcn, compute_time_response=False) + get_line_color = lambda cplt: cplt.lines.reshape(-1)[0][0].get_color() + match resp_fcn, plot_fcn: + case None, ct.phase_plane_plot: + get_line_color = None + warnings.warn("ct.phase_plane_plot returns nonstandard lines") + + # Call the plot through the response function + if resp_fcn is not None: + resp = resp_fcn(*args, **kwargs, **resp_kwargs) + cplt1 = resp.plot(**kwargs, **meth_kwargs) + else: + # No response function available; just plot the data + cplt1 = plot_fcn(*args, **kwargs, **plot_kwargs) + assert isinstance(cplt1, ct.ControlPlot) + + # Call the plot directly, plotting on top of previous plot + if plot_fcn == ct.time_response_plot: + # Can't call the time_response_plot() with system => reuse data + cplt2 = plot_fcn(resp, **kwargs, **plot_kwargs) + else: + cplt2 = plot_fcn(*args, **kwargs, **plot_kwargs) + assert isinstance(cplt2, ct.ControlPlot) + + # Plot should have landed on top of previous plot, in different colors + assert cplt2.figure == cplt1.figure + assert np.all(cplt2.axes == cplt1.axes) + assert len(cplt2.lines[0]) == len(cplt1.lines[0]) + if get_line_color is not None: + assert get_line_color(cplt2) != get_line_color(cplt1) + + # Pass axes explicitly + if resp_fcn is not None: + cplt3 = resp.plot(**kwargs, **meth_kwargs, ax=cplt1.axes) + else: + cplt3 = plot_fcn(*args, **kwargs, **plot_kwargs, ax=cplt1.axes) + assert cplt3.figure == cplt1.figure + + # Plot should have landed on top of previous plot, in different colors + assert np.all(cplt3.axes == cplt1.axes) + assert len(cplt3.lines[0]) == len(cplt1.lines[0]) + if get_line_color is not None: + assert get_line_color(cplt3) != get_line_color(cplt1) + assert get_line_color(cplt3) != get_line_color(cplt2) + + # + # Plot on a user-contructed figure + # + + # Store modified properties from previous figure + cplt_titlesize = cplt3.figure._suptitle.get_fontsize() + cplt_labelsize = \ + cplt3.axes.reshape(-1)[0].get_yticklabels()[0].get_fontsize() + + # Set up some axes with a known title + fig, axs = plt.subplots(2, 3) + title = "User-constructed figure" + plt.suptitle(title) + titlesize = fig._suptitle.get_fontsize() + assert titlesize != cplt_titlesize + labelsize = axs[0, 0].get_yticklabels()[0].get_fontsize() + assert labelsize != cplt_labelsize + + # Figure out what to pass as the ax keyword + match resp_fcn, plot_fcn: + case _, ct.bode_plot: + ax = [axs[0, 1], axs[1, 1]] + + case ct.gangof4_response, _: + ax = [axs[0, 1], axs[0, 2], axs[1, 1], axs[1, 2]] + + case (ct.forced_response | ct.input_output_response, _): + ax = [axs[0, 1], axs[1, 1]] + + case _, _: + ax = [axs[0, 1]] + + # Call the plotting function, passing the axes + if resp_fcn is not None: + resp = resp_fcn(*args, **kwargs, **resp_kwargs) + cplt4 = resp.plot(**kwargs, **meth_kwargs, ax=ax) + else: + # No response function available; just plot the data + cplt4 = plot_fcn(*args, **kwargs, **plot_kwargs, ax=ax) + + # Check to make sure original settings did not change + assert fig._suptitle.get_text() == title + assert fig._suptitle.get_fontsize() == titlesize + assert ax[0].get_yticklabels()[0].get_fontsize() == labelsize + + # Make sure that docstring documents ax keyword + if plot_fcn not in legacy_plot_fcns: + if plot_fcn in multiaxes_plot_fcns: + assert "ax : array of matplotlib.axes.Axes, optional" \ + in plot_fcn.__doc__ + else: + assert "ax : matplotlib.axes.Axes, optional" in plot_fcn.__doc__ + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_plot_label_processing(resp_fcn, plot_fcn): + # Set up arguments + args1, args2, argsc, kwargs, meth_kwargs, plot_kwargs, resp_kwargs = \ + setup_plot_arguments(resp_fcn, plot_fcn) + default_labels = ["sys[1]", "sys[2]"] + expected_labels = ["sys1_", "sys2_"] + match resp_fcn, plot_fcn: + case ct.gangof4_response, _: + default_labels = ["P=sys[1]", "P=sys[2]"] + + if plot_fcn in nolabel_plot_fcns: + pytest.skip(f"labels not implemented for {plot_fcn}") + + # Generate the first plot, with default labels + cplt1 = plot_fcn(*args1, **kwargs, **plot_kwargs) + assert isinstance(cplt1, ct.ControlPlot) + assert_legend(cplt1, None) + + # Generate second plot with default labels + cplt2 = plot_fcn(*args2, **kwargs, **plot_kwargs) + assert isinstance(cplt2, ct.ControlPlot) + assert_legend(cplt2, default_labels) + plt.close() + + # Generate both plots at the same time + if len(args1) == 1 and plot_fcn != ct.time_response_plot: + cplt = plot_fcn([*args1, *args2], **kwargs, **plot_kwargs) + assert isinstance(cplt, ct.ControlPlot) + assert_legend(cplt, default_labels) + elif len(args1) == 1 and plot_fcn == ct.time_response_plot: + # Use TimeResponseList.plot() to generate combined response + cplt = argsc[0].plot(**kwargs, **meth_kwargs) + assert isinstance(cplt, ct.ControlPlot) + assert_legend(cplt, default_labels) + plt.close() + + # Generate plots sequentially, with updated labels + cplt1 = plot_fcn( + *args1, **kwargs, **plot_kwargs, label=expected_labels[0]) + assert isinstance(cplt1, ct.ControlPlot) + assert_legend(cplt1, None) + + cplt2 = plot_fcn( + *args2, **kwargs, **plot_kwargs, label=expected_labels[1]) + assert isinstance(cplt2, ct.ControlPlot) + assert_legend(cplt2, expected_labels) + plt.close() + + # Generate both plots at the same time, with updated labels + if len(args1) == 1 and plot_fcn != ct.time_response_plot: + cplt = plot_fcn( + [*args1, *args2], **kwargs, **plot_kwargs, + label=expected_labels) + assert isinstance(cplt, ct.ControlPlot) + assert_legend(cplt, expected_labels) + elif len(args1) == 1 and plot_fcn == ct.time_response_plot: + # Use TimeResponseList.plot() to generate combined response + cplt = argsc[0].plot( + **kwargs, **meth_kwargs, label=expected_labels) + assert isinstance(cplt, ct.ControlPlot) + assert_legend(cplt, expected_labels) + plt.close() + + # Make sure that docstring documents label + if plot_fcn not in legacy_plot_fcns: + assert "label : str or array_like of str, optional" in plot_fcn.__doc__ + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_plot_linestyle_processing(resp_fcn, plot_fcn): + # Create some systems to use + sys1 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[1]") + sys1c = ct.rss(4, 1, 1, strictly_proper=True, name="sys[1]_C") + sys2 = ct.rss(4, 1, 1, strictly_proper=True, name="sys[2]") + + # Set up arguments + args1, args2, _, kwargs, meth_kwargs, plot_kwargs, resp_kwargs = \ + setup_plot_arguments(resp_fcn, plot_fcn) + default_labels = ["sys[1]", "sys[2]"] + expected_labels = ["sys1_", "sys2_"] + match resp_fcn, plot_fcn: + case ct.gangof4_response, _: + default_labels = ["P=sys[1]", "P=sys[2]"] + + # Set line color + cplt1 = plot_fcn(*args1, **kwargs, **plot_kwargs, color='r') + assert cplt1.lines.reshape(-1)[0][0].get_color() == 'r' + + # Second plot, new line color + cplt2 = plot_fcn(*args2, **kwargs, **plot_kwargs, color='g') + assert cplt2.lines.reshape(-1)[0][0].get_color() == 'g' + + # Make sure that docstring documents line properties + if plot_fcn not in legacy_plot_fcns: + assert "line properties" in plot_fcn.__doc__ or \ + "color : matplotlib color spec, optional" in plot_fcn.__doc__ + + # Set other characteristics if documentation says we can + if "line properties" in plot_fcn.__doc__: + cplt = plot_fcn(*args1, **kwargs, **plot_kwargs, linewidth=5) + assert cplt.lines.reshape(-1)[0][0].get_linewidth() == 5 + + # If fmt string is allowed, use it to set line color and style + if "*fmt" in plot_fcn.__doc__: + cplt = plot_fcn(*args1, 'r--', **kwargs, **plot_kwargs) + assert cplt.lines.reshape(-1)[0][0].get_color() == 'r' + assert cplt.lines.reshape(-1)[0][0].get_linestyle() == '--' + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_siso_plot_legend_processing(resp_fcn, plot_fcn): + # Set up arguments + args1, args2, argsc, kwargs, meth_kwargs, plot_kwargs, resp_kwargs = \ + setup_plot_arguments(resp_fcn, plot_fcn) + default_labels = ["sys[1]", "sys[2]"] + match resp_fcn, plot_fcn: + case ct.gangof4_response, _: + # Multi-axes plot => test in next function + return + + if plot_fcn in nolabel_plot_fcns: + # Make sure that using legend keywords generates an error + with pytest.raises(TypeError, match="unexpected|unrecognized"): + cplt = plot_fcn(*args1, legend_loc=None) + with pytest.raises(TypeError, match="unexpected|unrecognized"): + cplt = plot_fcn(*args1, legend_map=None) + with pytest.raises(TypeError, match="unexpected|unrecognized"): + cplt = plot_fcn(*args1, show_legend=None) + return + + # Single system, with forced legend + cplt = plot_fcn(*args1, **kwargs, **plot_kwargs, show_legend=True) + assert_legend(cplt, default_labels[:1]) + plt.close() + + # Single system, with forced location + cplt = plot_fcn(*args1, **kwargs, **plot_kwargs, legend_loc=10) + assert cplt.axes[0, 0].get_legend()._loc == 10 + plt.close() + + # Generate two plots, but turn off legends + if len(args1) == 1 and plot_fcn != ct.time_response_plot: + cplt = plot_fcn( + [*args1, *args2], **kwargs, **plot_kwargs, show_legend=False) + assert_legend(cplt, None) + elif len(args1) == 1 and plot_fcn == ct.time_response_plot: + # Use TimeResponseList.plot() to generate combined response + cplt = argsc[0].plot(**kwargs, **meth_kwargs, show_legend=False) + assert_legend(cplt, None) + plt.close() + + # Make sure that docstring documents legend_loc, show_legend + assert "legend_loc : int or str, optional" in plot_fcn.__doc__ + assert "show_legend : bool, optional" in plot_fcn.__doc__ + + # Make sure that single axes plots generate an error with legend_map + if plot_fcn not in multiaxes_plot_fcns: + with pytest.raises(TypeError, match="unexpected"): + cplt = plot_fcn(*args1, legend_map=False) + else: + assert "legend_map : array of str" in plot_fcn.__doc__ + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_mimo_plot_legend_processing(resp_fcn, plot_fcn): + # Generate the response that we will use for plotting + match resp_fcn, plot_fcn: + case ct.frequency_response, ct.bode_plot: + resp = ct.frequency_response([ct.rss(4, 2, 2), ct.rss(3, 2, 2)]) + case ct.step_response, ct.time_response_plot: + resp = ct.step_response([ct.rss(4, 2, 2), ct.rss(3, 2, 2)]) + case ct.gangof4_response, ct.gangof4_plot: + resp = ct.gangof4_response(ct.rss(4, 1, 1), ct.rss(3, 1, 1)) + case _, ct.time_response_plot: + # Skip remaining time response plots to avoid duplicate tests + return + case _, _: + # Skip everything else that doesn't support multi-axes plots + assert plot_fcn not in multiaxes_plot_fcns + return + + # Generate a standard plot with legend in the center + cplt1 = resp.plot(legend_loc=10) + assert cplt1.axes.ndim == 2 + for legend_idx, ax in enumerate(cplt1.axes.flatten()): + if ax.get_legend() is not None: + break; + assert legend_idx != 0 # Make sure legend is not in first subplot + assert ax.get_legend()._loc == 10 + plt.close() + + # Regenerate the plot with no legend + cplt2 = resp.plot(show_legend=False) + for ax in cplt2.axes.flatten(): + if ax.get_legend() is not None: + break; + assert ax.get_legend() is None + plt.close() + + # Regenerate the plot with no legend in a different way + cplt2 = resp.plot(legend_loc=False) + for ax in cplt2.axes.flatten(): + if ax.get_legend() is not None: + break; + assert ax.get_legend() is None + plt.close() + + # Regenerate the plot with no legend in a different way + cplt2 = resp.plot(legend_map=False) + for ax in cplt2.axes.flatten(): + if ax.get_legend() is not None: + break; + assert ax.get_legend() is None + plt.close() + + # Put the legend in a different (first) subplot + legend_map = np.full(cplt2.shape, None, dtype=object) + legend_map[0, 0] = 5 + legend_map[-1, -1] = 6 + cplt3 = resp.plot(legend_map=legend_map) + assert cplt3.axes[0, 0].get_legend()._loc == 5 + assert cplt3.axes[-1, -1].get_legend()._loc == 6 + plt.close() + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_plot_title_processing(resp_fcn, plot_fcn): + # Create some systems to use + sys1 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[1]") + sys1c = ct.rss(4, 1, 1, strictly_proper=True, name="sys[1]_C") + sys2 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[2]") + + # Set up arguments + args1, args2, argsc, kwargs, meth_kwargs, plot_kwargs, resp_kwargs = \ + setup_plot_arguments(resp_fcn, plot_fcn) + default_title = "sys[1], sys[2]" + expected_title = "sys1_, sys2_" + match resp_fcn, plot_fcn: + case ct.gangof4_response, _: + default_title = "P=sys[1], C=sys[1]_C, P=sys[2], C=sys[1]_C" + + # Store the expected title prefix + match resp_fcn, plot_fcn: + case _, ct.bode_plot: + title_prefix = "Bode plot for " + case _, ct.nichols_plot: + title_prefix = "Nichols plot for " + case _, ct.singular_values_plot: + title_prefix = "Singular values for " + case _, ct.gangof4_plot: + title_prefix = "Gang of Four for " + case _, ct.describing_function_plot: + title_prefix = "Nyquist plot for " + case _, ct.phase_plane_plot: + title_prefix = "Phase portrait for " + case _, ct.pole_zero_plot: + title_prefix = "Pole/zero plot for " + case _, ct.nyquist_plot: + title_prefix = "Nyquist plot for " + case _, ct.root_locus_plot: + title_prefix = "Root locus plot for " + case ct.initial_response, _: + title_prefix = "Initial response for " + case ct.step_response, _: + title_prefix = "Step response for " + case ct.impulse_response, _: + title_prefix = "Impulse response for " + case ct.forced_response, _: + title_prefix = "Forced response for " + case ct.input_output_response, _: + title_prefix = "Input/output response for " + case _: + raise RuntimeError(f"didn't recognize {resp_fnc}, {plot_fnc}") + + # Generate the first plot, with default title + cplt1 = plot_fcn(*args1, **kwargs, **plot_kwargs) + assert cplt1.figure._suptitle._text.startswith(title_prefix) + + # Skip functions not intended for sequential calling + if plot_fcn not in nolabel_plot_fcns: + # Generate second plot with default title + cplt2 = plot_fcn(*args2, **kwargs, **plot_kwargs) + assert cplt1.figure._suptitle._text == title_prefix + default_title + plt.close() + + # Generate both plots at the same time + if len(args1) == 1 and plot_fcn != ct.time_response_plot: + cplt = plot_fcn([*args1, *args2], **kwargs, **plot_kwargs) + assert cplt.figure._suptitle._text == title_prefix + default_title + elif len(args1) == 1 and plot_fcn == ct.time_response_plot: + # Use TimeResponseList.plot() to generate combined response + cplt = argsc[0].plot(**kwargs, **meth_kwargs) + assert cplt.figure._suptitle._text == title_prefix + default_title + plt.close() + + # Generate plots sequentially, with updated titles + cplt1 = plot_fcn( + *args1, **kwargs, **plot_kwargs, title="My first title") + cplt2 = plot_fcn( + *args2, **kwargs, **plot_kwargs, title="My new title") + assert cplt2.figure._suptitle._text == "My new title" + plt.close() + + # Update using set_plot_title + cplt2.set_plot_title("Another title") + assert cplt2.figure._suptitle._text == "Another title" + plt.close() + + # Generate the plots with no title + cplt = plot_fcn( + *args1, **kwargs, **plot_kwargs, title=False) + assert cplt.figure._suptitle == None + plt.close() + + # Make sure that docstring documents title + if plot_fcn not in legacy_plot_fcns: + assert "title : str, optional" in plot_fcn.__doc__ + + +@pytest.mark.parametrize("plot_fcn", multiaxes_plot_fcns) +@pytest.mark.usefixtures('mplcleanup') +def test_tickmark_label_processing(plot_fcn): + # Generate the response that we will use for plotting + top_row, bot_row = 0, -1 + match plot_fcn: + case ct.bode_plot: + resp = ct.frequency_response(ct.rss(4, 2, 2)) + top_row = 1 + case ct.time_response_plot: + resp = ct.step_response(ct.rss(4, 2, 2)) + case ct.gangof4_plot: + resp = ct.gangof4_response(ct.rss(4, 1, 1), ct.rss(3, 1, 1)) + case _: + pytest.fail("unknown plot_fcn") + + # Turn off axis sharing => all axes have ticklabels + cplt = resp.plot(sharex=False, sharey=False) + for i, j in itertools.product( + range(cplt.axes.shape[0]), range(cplt.axes.shape[1])): + assert len(cplt.axes[i, j].get_xticklabels()) > 0 + assert len(cplt.axes[i, j].get_yticklabels()) > 0 + plt.clf() + + # Turn on axis sharing => only outer axes have ticklabels + cplt = resp.plot(sharex=True, sharey=True) + for i, j in itertools.product( + range(cplt.axes.shape[0]), range(cplt.axes.shape[1])): + if i < cplt.axes.shape[0] - 1: + assert len(cplt.axes[i, j].get_xticklabels()) == 0 + else: + assert len(cplt.axes[i, j].get_xticklabels()) > 0 + + if j > 0: + assert len(cplt.axes[i, j].get_yticklabels()) == 0 + else: + assert len(cplt.axes[i, j].get_yticklabels()) > 0 + + +@pytest.mark.parametrize("resp_fcn, plot_fcn", resp_plot_fcns) +@pytest.mark.usefixtures('mplcleanup', 'editsdefaults') +def test_rcParams(resp_fcn, plot_fcn): + # Create some systems to use + sys1 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[1]") + sys1c = ct.rss(4, 1, 1, strictly_proper=True, name="sys[1]_C") + sys2 = ct.rss(2, 1, 1, strictly_proper=True, name="sys[2]") + + # Set up arguments + args1, args2, argsc, kwargs, meth_kwargs, plot_kwargs, resp_kwargs = \ + setup_plot_arguments(resp_fcn, plot_fcn) + default_title = "sys[1], sys[2]" + expected_title = "sys1_, sys2_" + match resp_fcn, plot_fcn: + case ct.gangof4_response, _: + default_title = "P=sys[1], C=sys[1]_C, P=sys[2], C=sys[1]_C" + + # Create new set of rcParams + my_rcParams = {} + for key in ct.ctrlplot.rcParams: + match plt.rcParams[key]: + case 8 | 9 | 10: + my_rcParams[key] = plt.rcParams[key] + 1 + case 'medium': + my_rcParams[key] = 11.5 + case 'large': + my_rcParams[key] = 9.5 + case _: + raise ValueError(f"unknown rcParam type for {key}") + checked_params = my_rcParams.copy() # make sure we check everything + + # Generate a figure with the new rcParams + if plot_fcn not in nolabel_plot_fcns: + cplt = plot_fcn( + *args1, **kwargs, **plot_kwargs, rcParams=my_rcParams, + show_legend=True) + else: + cplt = plot_fcn(*args1, **kwargs, **plot_kwargs, rcParams=my_rcParams) + + # Check lower left figure (should always have ticks, labels) + ax, fig = cplt.axes[-1, 0], cplt.figure + + # Check to make sure new settings were used + assert ax.xaxis.get_label().get_fontsize() == my_rcParams['axes.labelsize'] + assert ax.yaxis.get_label().get_fontsize() == my_rcParams['axes.labelsize'] + checked_params.pop('axes.labelsize') + + assert ax.title.get_fontsize() == my_rcParams['axes.titlesize'] + checked_params.pop('axes.titlesize') + + assert ax.get_xticklabels()[0].get_fontsize() == \ + my_rcParams['xtick.labelsize'] + checked_params.pop('xtick.labelsize') + + assert ax.get_yticklabels()[0].get_fontsize() == \ + my_rcParams['ytick.labelsize'] + checked_params.pop('ytick.labelsize') + + assert fig._suptitle.get_fontsize() == my_rcParams['figure.titlesize'] + checked_params.pop('figure.titlesize') + + if plot_fcn not in nolabel_plot_fcns: + for ax in cplt.axes.flatten(): + legend = ax.get_legend() + if legend is not None: + break + assert legend is not None + assert legend.get_texts()[0].get_fontsize() == \ + my_rcParams['legend.fontsize'] + checked_params.pop('legend.fontsize') + + # Make sure we checked everything + assert not checked_params + plt.close() + + # Change the default rcParams + ct.ctrlplot.rcParams.update(my_rcParams) + if plot_fcn not in nolabel_plot_fcns: + cplt = plot_fcn( + *args1, **kwargs, **plot_kwargs, show_legend=True) + else: + cplt = plot_fcn(*args1, **kwargs, **plot_kwargs) + + # Check everything + ax, fig = cplt.axes[-1, 0], cplt.figure + assert ax.xaxis.get_label().get_fontsize() == my_rcParams['axes.labelsize'] + assert ax.yaxis.get_label().get_fontsize() == my_rcParams['axes.labelsize'] + assert ax.title.get_fontsize() == my_rcParams['axes.titlesize'] + assert ax.get_xticklabels()[0].get_fontsize() == \ + my_rcParams['xtick.labelsize'] + assert ax.get_yticklabels()[0].get_fontsize() == \ + my_rcParams['ytick.labelsize'] + assert fig._suptitle.get_fontsize() == my_rcParams['figure.titlesize'] + if plot_fcn not in nolabel_plot_fcns: + for ax in cplt.axes.flatten(): + legend = ax.get_legend() + if legend is not None: + break + assert legend is not None + assert legend.get_texts()[0].get_fontsize() == \ + my_rcParams['legend.fontsize'] + plt.close() + + # Make sure that resetting parameters works correctly + ct.reset_defaults() + for key in ct.ctrlplot.rcParams: + assert ct.defaults['ctrlplot.rcParams'][key] != my_rcParams[key] + assert ct.ctrlplot.rcParams[key] != my_rcParams[key] + + +def test_deprecation_warnings(): + sys = ct.rss(2, 2, 2) + lines = ct.step_response(sys).plot(overlay_traces=True) + with pytest.warns(FutureWarning, match="deprecated"): + assert len(lines[0, 0]) == 2 + + cplt = ct.step_response(sys).plot() + with pytest.warns(FutureWarning, match="deprecated"): + axs = ct.get_plot_axes(cplt) + assert np.all(axs == cplt.axes) + + with pytest.warns(FutureWarning, match="deprecated"): + axs = ct.get_plot_axes(cplt.lines) + assert np.all(axs == cplt.axes) + + with pytest.warns(FutureWarning, match="deprecated"): + ct.suptitle("updated title") + assert cplt.figure._suptitle.get_text() == "updated title" + + +def test_ControlPlot_init(): + sys = ct.rss(2, 2, 2) + cplt = ct.step_response(sys).plot() + + # Create a ControlPlot from data, without the axes or figure + cplt_raw = ct.ControlPlot(cplt.lines) + assert np.all(cplt_raw.lines == cplt.lines) + assert np.all(cplt_raw.axes == cplt.axes) + assert cplt_raw.figure == cplt.figure + + +def test_pole_zero_subplots(savefig=False): + ax_array = ct.pole_zero_subplots(2, 1, grid=[True, False]) + sys1 = ct.tf([1, 2], [1, 2, 3], name='sys1') + sys2 = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys2') + ct.root_locus_plot([sys1, sys2], ax=ax_array[0, 0]) + cplt = ct.root_locus_plot([sys1, sys2], ax=ax_array[1, 0]) + with pytest.warns(UserWarning, match="Tight layout not applied"): + cplt.set_plot_title("Root locus plots (w/ specified axes)") + if savefig: + plt.savefig("ctrlplot-pole_zero_subplots.png") + + # Single type of of grid for all axes + ax_array = ct.pole_zero_subplots(2, 2, grid='empty') + assert ax_array[0, 0].xaxis.get_label().get_text() == '' + + # Discrete system grid + ax_array = ct.pole_zero_subplots(2, 2, grid=True, dt=1) + assert ax_array[0, 0].xaxis.get_label().get_text() == 'Real' + assert ax_array[0, 0].get_lines()[0].get_color() == 'grey' + + +if __name__ == "__main__": + # + # Interactive mode: generate plots for manual viewing + # + # Running this script in python (or better ipython) will show a + # collection of figures that should all look OK on the screeen. + # + + # In interactive mode, turn on ipython interactive graphics + plt.ion() + + # Start by clearing existing figures + plt.close('all') + + # + # Combination plot + # + + P = ct.tf([0.02], [1, 0.1, 0.01]) # servomechanism + C1 = ct.tf([1, 1], [1, 0]) # unstable + L1 = P * C1 + C2 = ct.tf([1, 0.05], [1, 0]) # stable + L2 = P * C2 + + plt.rcParams.update(ct.rcParams) + fig = plt.figure(figsize=[7, 4]) + ax_mag = fig.add_subplot(2, 2, 1) + ax_phase = fig.add_subplot(2, 2, 3) + ax_nyquist = fig.add_subplot(1, 2, 2) + + ct.bode_plot( + [L1, L2], ax=[ax_mag, ax_phase], + label=["$L_1$ (unstable)", "$L_2$ (unstable)"], + show_legend=False) + ax_mag.set_title("Bode plot for $L_1$, $L_2$") + ax_mag.tick_params(labelbottom=False) + fig.align_labels() + + ct.nyquist_plot(L1, ax=ax_nyquist, label="$L_1$ (unstable)") + ct.nyquist_plot( + L2, ax=ax_nyquist, label="$L_2$ (stable)", + max_curve_magnitude=22, legend_loc='upper right') + ax_nyquist.set_title("Nyquist plot for $L_1$, $L_2$") + + fig.suptitle("Loop analysis for servomechanism control design") + plt.tight_layout() + plt.savefig('ctrlplot-servomech.png') + + plt.figure() + test_pole_zero_subplots(savefig=True) diff --git a/control/tests/descfcn_test.py b/control/tests/descfcn_test.py index ceeff1123..a5f7a06c2 100644 --- a/control/tests/descfcn_test.py +++ b/control/tests/descfcn_test.py @@ -7,14 +7,15 @@ """ -import pytest +import math +import matplotlib.pyplot as plt import numpy as np +import pytest + import control as ct -import math -import matplotlib.pyplot as plt -from control.descfcn import saturation_nonlinearity, \ - friction_backlash_nonlinearity, relay_hysteresis_nonlinearity +from control.descfcn import friction_backlash_nonlinearity, \ + relay_hysteresis_nonlinearity, saturation_nonlinearity # Static function via a class @@ -187,13 +188,13 @@ def test_describing_function_plot(): assert len(response.intersections) == 1 assert len(plt.gcf().get_axes()) == 0 # make sure there is no plot - out = response.plot() + cplt = response.plot() assert len(plt.gcf().get_axes()) == 1 # make sure there is a plot - assert len(out[0]) == 4 and len(out[1]) == 1 + assert len(cplt.lines[0]) == 4 and len(cplt.lines[1]) == 1 # Call plot directly - out = ct.describing_function_plot(H_larger, F_saturation, amp, omega) - assert len(out[0]) == 4 and len(out[1]) == 1 + cplt = ct.describing_function_plot(H_larger, F_saturation, amp, omega) + assert len(cplt.lines[0]) == 4 and len(cplt.lines[1]) == 1 def test_describing_function_exceptions(): @@ -231,3 +232,8 @@ def test_describing_function_exceptions(): with pytest.raises(AttributeError, match="no property|unexpected keyword"): response = ct.describing_function_response(H_simple, F_saturation, amp) response.plot(unknown=None) + + # Describing function plot for non-describing function object + resp = ct.frequency_response(H_simple) + with pytest.raises(TypeError, match="data must be DescribingFunction"): + cplt = ct.describing_function_plot(resp) diff --git a/control/tests/docstrings_test.py b/control/tests/docstrings_test.py new file mode 100644 index 000000000..28bd0f38c --- /dev/null +++ b/control/tests/docstrings_test.py @@ -0,0 +1,307 @@ +# docstrings_test.py - test for undocumented arguments +# RMM, 28 Jul 2024 +# +# This unit test looks through all functions in the package and attempts to +# identify arguments that are not documented. It will check anything that +# is an explicitly listed argument, as well as attempt to find keyword +# arguments that are extracted using kwargs.pop(), config._get_param(), or +# config.use_legacy_defaults. + +import inspect +import warnings + +import pytest +import re + +import control +import control.flatsys + +# List of functions that we can skip testing (special cases) +function_skiplist = [ + control.ControlPlot.reshape, # needed for legacy interface + control.phase_plot, # legacy function + control.drss, # documention in rss +] + +# Checksums to use for checking whether a docstring has changed +function_docstring_hash = { + control.append: '48548c4c4e0083312b3ea9e56174b0b5', + control.describing_function_plot: '95f894706b1d3eeb3b854934596af09f', + control.dlqe: '9db995ed95c2214ce97074b0616a3191', + control.dlqr: '896cfa651dbbd80e417635904d13c9d6', + control.lqe: '567bf657538935173f2e50700ba87168', + control.lqr: 'a3e0a85f781fc9c0f69a4b7da4f0bd22', + control.frd: '099464bf2d14f25a8769ef951adf658b', + control.margin: 'f02b3034f5f1d44ce26f916cc3e51600', + control.parallel: '025c5195a34c57392223374b6244a8c4', + control.series: '9aede1459667738f05cf4fc46603a4f6', + control.ss: '1b9cfad5dbdf2f474cfdeadf5cb1ad80', + control.ss2tf: '48ff25d22d28e7b396e686dd5eb58831', + control.tf: '53a13f4a7f75a31c81800e10c88730ef', + control.tf2ss: '086a3692659b7321c2af126f79f4bc11', + control.markov: '753309de348132ef238e78ac756412c1', + control.gangof4: '0e52eb6cf7ce024f9a41f3ae3ebf04f7', +} + +# List of keywords that we can skip testing (special cases) +keyword_skiplist = { + control.input_output_response: ['method'], + control.nyquist_plot: ['color'], # separate check + control.optimal.solve_ocp: ['method', 'return_x'], # deprecated + control.sisotool: ['kvect'], # deprecated + control.nyquist_response: ['return_contour'], # deprecated + control.create_estimator_iosystem: ['state_labels'], # deprecated + control.bode_plot: ['sharex', 'sharey', 'margin_info'], # deprecated + control.eigensys_realization: ['arg'], # quasi-positional +} + +# Decide on the level of verbosity (use -rP when running pytest) +verbose = 1 + +@pytest.mark.parametrize("module, prefix", [ + (control, ""), (control.flatsys, "flatsys."), + (control.optimal, "optimal."), (control.phaseplot, "phaseplot.") +]) +def test_parameter_docs(module, prefix): + checked = set() # Keep track of functions we have checked + + # Look through every object in the package + if verbose > 1: + print(f"Checking module {module}") + for name, obj in inspect.getmembers(module): + # Skip anything that is outside of this module + if inspect.getmodule(obj) is not None and ( + not inspect.getmodule(obj).__name__.startswith('control') + or prefix != "" and inspect.getmodule(obj) != module): + # Skip anything that isn't part of the control package + continue + + if inspect.isclass(obj): + if verbose > 1: + print(f" Checking class {name}") + # Check member functions within the class + test_parameter_docs(obj, prefix + name + '.') + + if inspect.isfunction(obj): + # Skip anything that is inherited, hidden, deprecated, or checked + if inspect.isclass(module) and name not in module.__dict__ \ + or name.startswith('_') or obj in function_skiplist or \ + obj in checked: + continue + else: + checked.add(obj) + + # Get the docstring (skip w/ warning if there isn't one) + if verbose > 1: + print(f" Checking function {name}") + if obj.__doc__ is None: + warnings.warn( + f"{module.__name__}.{name} is missing docstring") + continue + else: + docstring = inspect.getdoc(obj) + source = inspect.getsource(obj) + + # Skip deprecated functions + if ".. deprecated::" in docstring: + if verbose > 1: + print(" [deprecated]") + continue + elif re.search(name + r"(\(\))? is deprecated", docstring) or \ + "function is deprecated" in docstring: + if verbose > 1: + print(" [deprecated, but not numpydoc compliant]") + elif verbose: + print(f" {name} deprecation is not numpydoc compliant") + warnings.warn(f"{name} deprecated, but not numpydoc compliant") + continue + + elif re.search(name + r"(\(\))? is deprecated", source): + if verbose: + print(f" {name} is deprecated, but not documented") + warnings.warn(f"{name} deprecated, but not documented") + continue + + # Get the signature for the function + sig = inspect.signature(obj) + + # Go through each parameter and make sure it is in the docstring + for argname, par in sig.parameters.items(): + + # Look for arguments that we can skip + if argname == 'self' or argname[0] == '_' or \ + obj in keyword_skiplist and argname in keyword_skiplist[obj]: + continue + + # Check for positional arguments + if par.kind == inspect.Parameter.VAR_POSITIONAL: + if obj in function_docstring_hash: + import hashlib + hash = hashlib.md5( + (docstring + source).encode('utf-8')).hexdigest() + if function_docstring_hash[obj] != hash: + pytest.fail( + f"source/docstring for {name}() modified; " + f"recheck docstring and update hash to " + f"{hash=}") + continue + + # Too complicated to check + if f"*{argname}" not in docstring: + if verbose: + print(f" {name} has positional arguments; " + "check manually") + warnings.warn( + f"{name} {argname} has positional arguments; " + "docstring not checked") + continue + + # Check for keyword arguments (then look at code for parsing) + elif par.kind == inspect.Parameter.VAR_KEYWORD: + # See if we documented the keyward argumnt directly + # if f"**{argname} :" in docstring: + # continue + + # Look for direct kwargs argument access + kwargnames = set() + for _, kwargname in re.findall( + argname + r"(\[|\.pop\(|\.get\()'([\w]+)'", + source): + if verbose > 2: + print(" Found direct keyword argument", + kwargname) + kwargnames.add(kwargname) + + # Look for kwargs accessed via _get_param + for kwargname in re.findall( + r"_get_param\(\s*'\w*',\s*'([\w]+)',\s*" + argname, + source): + if verbose > 2: + print(" Found config keyword argument", + {kwargname}) + kwargnames.add(kwargname) + + # Look for kwargs accessed via _process_legacy_keyword + for kwargname in re.findall( + r"_process_legacy_keyword\([\s]*" + argname + + r",[\s]*'[\w]+',[\s]*'([\w]+)'", source): + if verbose > 2: + print(" Found legacy keyword argument", + {kwargname}) + kwargnames.add(kwargname) + + for kwargname in kwargnames: + if obj in keyword_skiplist and \ + kwargname in keyword_skiplist[obj]: + continue + if verbose > 3: + print(f" Checking keyword argument {kwargname}") + _check_parameter_docs( + name, kwargname, inspect.getdoc(obj), + prefix=prefix) + + # Make sure this argument is documented properly in docstring + else: + if verbose > 3: + print(f" Checking argument {argname}") + _check_parameter_docs( + name, argname, docstring, prefix=prefix) + + +@pytest.mark.parametrize("module, prefix", [ + (control, ""), (control.flatsys, "flatsys."), + (control.optimal, "optimal."), (control.phaseplot, "phaseplot.") +]) +def test_deprecated_functions(module, prefix): + checked = set() # Keep track of functions we have checked + + # Look through every object in the package + for name, obj in inspect.getmembers(module): + # Skip anything that is outside of this module + if inspect.getmodule(obj) is not None and ( + not inspect.getmodule(obj).__name__.startswith('control') + or prefix != "" and inspect.getmodule(obj) != module): + # Skip anything that isn't part of the control package + continue + + if inspect.isclass(obj): + # Check member functions within the class + test_deprecated_functions(obj, prefix + name + '.') + + if inspect.isfunction(obj): + # Skip anything that is inherited, hidden, or checked + if inspect.isclass(module) and name not in module.__dict__ \ + or name[0] == '_' or obj in checked: + continue + else: + checked.add(obj) + + # Get the docstring (skip w/ warning if there isn't one) + if obj.__doc__ is None: + warnings.warn( + f"{module.__name__}.{name} is missing docstring") + continue + else: + docstring = inspect.getdoc(obj) + source = inspect.getsource(obj) + + # Look for functions marked as deprecated in doc string + if ".. deprecated::" in docstring: + # Make sure a FutureWarning is issued + if not re.search("FutureWarning", source): + pytest.fail( + f"{name} deprecated but does not issue FutureWarning") + else: + if re.search(name + r"(\(\))? is deprecated", docstring) or \ + re.search(name + r"(\(\))? is deprecated", source): + pytest.fail( + f"{name} deprecated but w/ non-standard docs/warnings") + assert name != 'ss2io' + + +# Utility function to check for an argument in a docstring +def _check_parameter_docs(funcname, argname, docstring, prefix=""): + funcname = prefix + funcname + + # Find the "Parameters" section of docstring, where we start searching + if not (match := re.search(r"\nParameters\n----", docstring)): + pytest.fail(f"{funcname} docstring missing Parameters section") + else: + start = match.start() + + # Find the "Returns" section of the docstring (to be skipped, if present) + match_returns = re.search(r"\nReturns\n----", docstring) + + # Find the "Other Parameters" section of the docstring, if present + match_other = re.search(r"\nOther Parameters\n----", docstring) + + # Remove the returns section from docstring, in case output arguments + # match input argument names (it happens...) + if match_other and match_returns: + docstring = docstring[start:match_returns.start()] + \ + docstring[match_other.start():] + else: + docstring = docstring[start:] + + # Look for the parameter name in the docstring + if match := re.search( + "\n" + r"((\w+|\.{3}), )*" + argname + r"(, (\w+|\.{3}))*:", + docstring): + # Found the string, but not in numpydoc form + if verbose: + print(f" {funcname}: {argname} docstring missing space") + warnings.warn(f"{funcname} '{argname}' docstring missing space") + + elif not (match := re.search( + "\n" + r"((\w+|\.{3}), )*" + argname + r"(, (\w+|\.{3}))* :", + docstring)): + if verbose: + print(f" {funcname}: {argname} not documented") + pytest.fail(f"{funcname} '{argname}' not documented") + + # Make sure there isn't another instance + second_match = re.search( + "\n" + r"((\w+|\.{3}), )*" + argname + r"(, (\w+|\.{3}))*[ ]*:", + docstring[match.end():]) + if second_match: + pytest.fail(f"{funcname} '{argname}' documented twice") diff --git a/control/tests/frd_test.py b/control/tests/frd_test.py index 987121987..bae0ec47b 100644 --- a/control/tests/frd_test.py +++ b/control/tests/frd_test.py @@ -12,7 +12,7 @@ import control as ct from control.statesp import StateSpace from control.xferfcn import TransferFunction -from control.frdata import FRD, _convert_to_FRD, FrequencyResponseData +from control.frdata import frd, _convert_to_frd, FrequencyResponseData from control import bdalg, evalfr, freqplot from control.tests.conftest import slycotonly from control.exception import pandas_check @@ -25,35 +25,39 @@ class TestFRD: def testBadInputType(self): """Give the constructor invalid input types.""" with pytest.raises(ValueError): - FRD() + frd() with pytest.raises(TypeError): - FRD([1]) + frd([1]) def testInconsistentDimension(self): with pytest.raises(TypeError): - FRD([1, 1], [1, 2, 3]) + frd([1, 1], [1, 2, 3]) - def testSISOtf(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testSISOtf(self, frd_fcn): # get a SISO transfer function h = TransferFunction([1], [1, 2, 2]) omega = np.logspace(-1, 2, 10) - frd = FRD(h, omega) - assert isinstance(frd, FRD) + sys = frd_fcn(h, omega) + assert isinstance(sys, FrequencyResponseData) - mag1, phase1, omega1 = frd.frequency_response([1.0]) + mag1, phase1, omega1 = sys.frequency_response([1.0]) mag2, phase2, omega2 = h.frequency_response([1.0]) np.testing.assert_array_almost_equal(mag1, mag2) np.testing.assert_array_almost_equal(phase1, phase2) np.testing.assert_array_almost_equal(omega1, omega2) - def testOperators(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testOperators(self, frd_fcn): # get two SISO transfer functions h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(h1, omega) - f2 = FRD(h2, omega) + f1 = frd_fcn(h1, omega) + f2 = frd_fcn(h2, omega) np.testing.assert_array_almost_equal( (f1 + f2).frequency_response(chkpts)[0], @@ -90,14 +94,16 @@ def testOperators(self): (1.3 / f2).frequency_response(chkpts)[1], (1.3 / h2).frequency_response(chkpts)[1]) - def testOperatorsTf(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testOperatorsTf(self, frd_fcn): # get two SISO transfer functions h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(h1, omega) - f2 = FRD(h2, omega) + f1 = frd_fcn(h1, omega) + f2 = frd_fcn(h2, omega) f2 # reference to avoid pyflakes error np.testing.assert_array_almost_equal( @@ -121,14 +127,16 @@ def testOperatorsTf(self): (h1 / h2).frequency_response(chkpts)[1]) # the reverse does not work - def testbdalg(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testbdalg(self, frd_fcn): # get two SISO transfer functions h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(h1, omega) - f2 = FRD(h2, omega) + f1 = frd_fcn(h1, omega) + f2 = frd_fcn(h2, omega) np.testing.assert_array_almost_equal( (bdalg.series(f1, f2)).frequency_response(chkpts)[0], @@ -158,11 +166,13 @@ def testbdalg(self): # (bdalg.connect(f3, Q, [2], [1])).frequency_response(chkpts)[0], # (bdalg.connect(h3, Q, [2], [1])).frequency_response(chkpts)[0]) - def testFeedback(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testFeedback(self, frd_fcn): h1 = TransferFunction([1], [1, 2, 2]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(h1, omega) + f1 = frd_fcn(h1, omega) np.testing.assert_array_almost_equal( f1.feedback(1).frequency_response(chkpts)[0], h1.feedback(1).frequency_response(chkpts)[0]) @@ -179,30 +189,36 @@ def testFeedback2(self): def testAuto(self): omega = np.logspace(-1, 2, 10) - f1 = _convert_to_FRD(1, omega) - f2 = _convert_to_FRD(np.array([[1, 0], [0.1, -1]]), omega) - f2 = _convert_to_FRD([[1, 0], [0.1, -1]], omega) + f1 = _convert_to_frd(1, omega) + f2 = _convert_to_frd(np.array([[1, 0], [0.1, -1]]), omega) + f2 = _convert_to_frd([[1, 0], [0.1, -1]], omega) f1, f2 # reference to avoid pyflakes error - def testNyquist(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testNyquist(self, frd_fcn): h1 = TransferFunction([1], [1, 2, 2]) omega = np.logspace(-1, 2, 40) - f1 = FRD(h1, omega, smooth=True) + f1 = frd_fcn(h1, omega, smooth=True) freqplot.nyquist(f1, np.logspace(-1, 2, 100)) # plt.savefig('/dev/null', format='svg') plt.figure(2) freqplot.nyquist(f1, f1.omega) + plt.figure(3) + freqplot.nyquist(f1) # plt.savefig('/dev/null', format='svg') @slycotonly - def testMIMO(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testMIMO(self, frd_fcn): sys = StateSpace([[-0.5, 0.0], [0.0, -1.0]], [[1.0, 0.0], [0.0, 1.0]], [[1.0, 0.0], [0.0, 1.0]], [[0.0, 0.0], [0.0, 0.0]]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(sys, omega) + f1 = frd_fcn(sys, omega) np.testing.assert_array_almost_equal( sys.frequency_response(chkpts)[0], f1.frequency_response(chkpts)[0]) @@ -211,15 +227,17 @@ def testMIMO(self): f1.frequency_response(chkpts)[1]) @slycotonly - def testMIMOfb(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testMIMOfb(self, frd_fcn): sys = StateSpace([[-0.5, 0.0], [0.0, -1.0]], [[1.0, 0.0], [0.0, 1.0]], [[1.0, 0.0], [0.0, 1.0]], [[0.0, 0.0], [0.0, 0.0]]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(sys, omega).feedback([[0.1, 0.3], [0.0, 1.0]]) - f2 = FRD(sys.feedback([[0.1, 0.3], [0.0, 1.0]]), omega) + f1 = frd_fcn(sys, omega).feedback([[0.1, 0.3], [0.0, 1.0]]) + f2 = frd_fcn(sys.feedback([[0.1, 0.3], [0.0, 1.0]]), omega) np.testing.assert_array_almost_equal( f1.frequency_response(chkpts)[0], f2.frequency_response(chkpts)[0]) @@ -228,7 +246,9 @@ def testMIMOfb(self): f2.frequency_response(chkpts)[1]) @slycotonly - def testMIMOfb2(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testMIMOfb2(self, frd_fcn): sys = StateSpace(np.array([[-2.0, 0, 0], [0, -1, 1], [0, 0, -3]]), @@ -237,8 +257,8 @@ def testMIMOfb2(self): omega = np.logspace(-1, 2, 10) chkpts = omega[::3] K = np.array([[1, 0.3, 0], [0.1, 0, 0]]) - f1 = FRD(sys, omega).feedback(K) - f2 = FRD(sys.feedback(K), omega) + f1 = frd_fcn(sys, omega).feedback(K) + f2 = frd_fcn(sys.feedback(K), omega) np.testing.assert_array_almost_equal( f1.frequency_response(chkpts)[0], f2.frequency_response(chkpts)[0]) @@ -247,15 +267,17 @@ def testMIMOfb2(self): f2.frequency_response(chkpts)[1]) @slycotonly - def testMIMOMult(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testMIMOMult(self, frd_fcn): sys = StateSpace([[-0.5, 0.0], [0.0, -1.0]], [[1.0, 0.0], [0.0, 1.0]], [[1.0, 0.0], [0.0, 1.0]], [[0.0, 0.0], [0.0, 0.0]]) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(sys, omega) - f2 = FRD(sys, omega) + f1 = frd_fcn(sys, omega) + f2 = frd_fcn(sys, omega) np.testing.assert_array_almost_equal( (f1*f2).frequency_response(chkpts)[0], (sys*sys).frequency_response(chkpts)[0]) @@ -264,7 +286,9 @@ def testMIMOMult(self): (sys*sys).frequency_response(chkpts)[1]) @slycotonly - def testMIMOSmooth(self): + @pytest.mark.parametrize( + "frd_fcn", [ct.frd, ct.FRD, ct.FrequencyResponseData]) + def testMIMOSmooth(self, frd_fcn): sys = StateSpace([[-0.5, 0.0], [0.0, -1.0]], [[1.0, 0.0], [0.0, 1.0]], [[1.0, 0.0], [0.0, 1.0], [1.0, 1.0]], @@ -272,8 +296,8 @@ def testMIMOSmooth(self): sys2 = np.array([[1, 0, 0], [0, 1, 0]]) * sys omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(sys, omega, smooth=True) - f2 = FRD(sys2, omega, smooth=True) + f1 = frd_fcn(sys, omega, smooth=True) + f2 = frd_fcn(sys2, omega, smooth=True) np.testing.assert_array_almost_equal( (f1*f2).frequency_response(chkpts)[0], (sys*sys2).frequency_response(chkpts)[0]) @@ -294,55 +318,55 @@ def testAgainstOctave(self): np.eye(3), np.zeros((3, 2))) omega = np.logspace(-1, 2, 10) chkpts = omega[::3] - f1 = FRD(sys, omega) + f1 = frd(sys, omega) np.testing.assert_array_almost_equal( (f1.frequency_response([1.0])[0] * np.exp(1j * f1.frequency_response([1.0])[1])).reshape(3, 2), np.array([[0.4 - 0.2j, 0], [0, 0.1 - 0.2j], [0, 0.3 - 0.1j]])) def test_string_representation(self, capsys): - sys = FRD([1, 2, 3], [4, 5, 6]) + sys = frd([1, 2, 3], [4, 5, 6]) print(sys) # Just print without checking def test_frequency_mismatch(self, recwarn): # recwarn: there may be a warning before the error! # Overlapping but non-equal frequency ranges - sys1 = FRD([1, 2, 3], [4, 5, 6]) - sys2 = FRD([2, 3, 4], [5, 6, 7]) + sys1 = frd([1, 2, 3], [4, 5, 6]) + sys2 = frd([2, 3, 4], [5, 6, 7]) with pytest.raises(NotImplementedError): - FRD.__add__(sys1, sys2) + sys = sys1 + sys2 # One frequency range is a subset of another - sys1 = FRD([1, 2, 3], [4, 5, 6]) - sys2 = FRD([2, 3], [4, 5]) + sys1 = frd([1, 2, 3], [4, 5, 6]) + sys2 = frd([2, 3], [4, 5]) with pytest.raises(NotImplementedError): - FRD.__add__(sys1, sys2) + sys = sys1 + sys2 def test_size_mismatch(self): - sys1 = FRD(ct.rss(2, 2, 2), np.logspace(-1, 1, 10)) + sys1 = frd(ct.rss(2, 2, 2), np.logspace(-1, 1, 10)) # Different number of inputs - sys2 = FRD(ct.rss(3, 1, 2), np.logspace(-1, 1, 10)) + sys2 = frd(ct.rss(3, 1, 2), np.logspace(-1, 1, 10)) with pytest.raises(ValueError): - FRD.__add__(sys1, sys2) + sys = sys1 + sys2 # Different number of outputs - sys2 = FRD(ct.rss(3, 2, 1), np.logspace(-1, 1, 10)) + sys2 = frd(ct.rss(3, 2, 1), np.logspace(-1, 1, 10)) with pytest.raises(ValueError): - FRD.__add__(sys1, sys2) + sys = sys1 + sys2 # Inputs and outputs don't match with pytest.raises(ValueError): - FRD.__mul__(sys2, sys1) + sys = sys2 * sys1 # Feedback mismatch with pytest.raises(ValueError): - FRD.feedback(sys2, sys1) + ct.feedback(sys2, sys1) def test_operator_conversion(self): sys_tf = ct.tf([1], [1, 2, 1]) - frd_tf = FRD(sys_tf, np.logspace(-1, 1, 10)) - frd_2 = FRD(2 * np.ones(10), np.logspace(-1, 1, 10)) + frd_tf = frd(sys_tf, np.logspace(-1, 1, 10)) + frd_2 = frd(2 * np.ones(10), np.logspace(-1, 1, 10)) # Make sure that we can add, multiply, and feedback constants sys_add = frd_tf + 2 @@ -381,18 +405,18 @@ def test_operator_conversion(self): np.testing.assert_array_almost_equal(sys_rdiv.fresp, chk_rdiv.fresp) sys_pow = frd_tf**2 - chk_pow = FRD(sys_tf**2, np.logspace(-1, 1, 10)) + chk_pow = frd(sys_tf**2, np.logspace(-1, 1, 10)) np.testing.assert_array_almost_equal(sys_pow.omega, chk_pow.omega) np.testing.assert_array_almost_equal(sys_pow.fresp, chk_pow.fresp) sys_pow = frd_tf**-2 - chk_pow = FRD(sys_tf**-2, np.logspace(-1, 1, 10)) + chk_pow = frd(sys_tf**-2, np.logspace(-1, 1, 10)) np.testing.assert_array_almost_equal(sys_pow.omega, chk_pow.omega) np.testing.assert_array_almost_equal(sys_pow.fresp, chk_pow.fresp) # Assertion error if we try to raise to a non-integer power with pytest.raises(ValueError): - FRD.__pow__(frd_tf, 0.5) + frd_tf**0.5 # Selected testing on transfer function conversion sys_add = frd_2 + sys_tf @@ -400,18 +424,18 @@ def test_operator_conversion(self): np.testing.assert_array_almost_equal(sys_add.omega, chk_add.omega) np.testing.assert_array_almost_equal(sys_add.fresp, chk_add.fresp) - # Input/output mismatch size mismatch in rmul - sys1 = FRD(ct.rss(2, 2, 2), np.logspace(-1, 1, 10)) + # Input/output mismatch size mismatch in rmul + sys1 = frd(ct.rss(2, 2, 2), np.logspace(-1, 1, 10)) with pytest.raises(ValueError): - FRD.__rmul__(frd_2, sys1) + FrequencyResponseData.__rmul__(frd_2, sys1) # Make sure conversion of something random generates exception with pytest.raises(TypeError): - FRD.__add__(frd_tf, 'string') + FrequencyResponseData.__add__(frd_tf, 'string') def test_eval(self): sys_tf = ct.tf([1], [1, 2, 1]) - frd_tf = FRD(sys_tf, np.logspace(-1, 1, 3)) + frd_tf = frd(sys_tf, np.logspace(-1, 1, 3)) np.testing.assert_almost_equal(sys_tf(1j), frd_tf.eval(1)) np.testing.assert_almost_equal(sys_tf(1j), frd_tf(1j)) @@ -429,45 +453,55 @@ def test_eval(self): def test_freqresp_deprecated(self): sys_tf = ct.tf([1], [1, 2, 1]) - frd_tf = FRD(sys_tf, np.logspace(-1, 1, 3)) - with pytest.warns(DeprecationWarning): + frd_tf = frd(sys_tf, np.logspace(-1, 1, 3)) + with pytest.warns(FutureWarning): frd_tf.freqresp(1.) def test_repr_str(self): # repr printing array = np.array - sys0 = FrequencyResponseData([1.0, 0.9+0.1j, 0.1+2j, 0.05+3j], - [0.1, 1.0, 10.0, 100.0]) - sys1 = FrequencyResponseData(sys0.fresp, sys0.omega, smooth=True) + sys0 = ct.frd( + [1.0, 0.9+0.1j, 0.1+2j, 0.05+3j], + [0.1, 1.0, 10.0, 100.0], name='sys0') + sys1 = ct.frd( + sys0.fresp, sys0.omega, smooth=True, name='sys1') ref0 = "FrequencyResponseData(" \ "array([[[1. +0.j , 0.9 +0.1j, 0.1 +2.j , 0.05+3.j ]]])," \ " array([ 0.1, 1. , 10. , 100. ]))" ref1 = ref0[:-1] + ", smooth=True)" - sysm = FrequencyResponseData( - np.matmul(array([[1],[2]]), sys0.fresp), sys0.omega) + sysm = ct.frd( + np.matmul(array([[1], [2]]), sys0.fresp), sys0.omega, name='sysm') assert repr(sys0) == ref0 assert repr(sys1) == ref1 + sys0r = eval(repr(sys0)) np.testing.assert_array_almost_equal(sys0r.fresp, sys0.fresp) np.testing.assert_array_almost_equal(sys0r.omega, sys0.omega) + sys1r = eval(repr(sys1)) np.testing.assert_array_almost_equal(sys1r.fresp, sys1.fresp) np.testing.assert_array_almost_equal(sys1r.omega, sys1.omega) assert(sys1.ifunc is not None) - refs = """Frequency response data + refs = """: {sysname} +Inputs (1): ['u[0]'] +Outputs (1): ['y[0]'] + Freq [rad/s] Response ------------ --------------------- 0.100 1 +0j 1.000 0.9 +0.1j 10.000 0.1 +2j 100.000 0.05 +3j""" - assert str(sys0) == refs - assert str(sys1) == refs + assert str(sys0) == refs.format(sysname='sys0') + assert str(sys1) == refs.format(sysname='sys1') # print multi-input system - refm = """Frequency response data + refm = """: sysm +Inputs (2): ['u[0]', 'u[1]'] +Outputs (1): ['y[0]'] + Input 1 to output 1: Freq [rad/s] Response ------------ --------------------- @@ -488,7 +522,9 @@ def test_unrecognized_keyword(self): h = TransferFunction([1], [1, 2, 2]) omega = np.logspace(-1, 2, 10) with pytest.raises(TypeError, match="unrecognized keyword"): - frd = FRD(h, omega, unknown=None) + sys = FrequencyResponseData(h, omega, unknown=None) + with pytest.raises(TypeError, match="unrecognized keyword"): + sys = ct.frd(h, omega, unknown=None) def test_named_signals(): @@ -496,8 +532,8 @@ def test_named_signals(): h1 = TransferFunction([1], [1, 2, 2]) h2 = TransferFunction([1], [0.1, 1]) omega = np.logspace(-1, 2, 10) - f1 = FRD(h1, omega) - f2 = FRD(h2, omega) + f1 = frd(h1, omega) + f2 = frd(h2, omega) # Make sure that systems were properly named assert f1.name == 'sys[2]' @@ -508,7 +544,7 @@ def test_named_signals(): assert f1.output_labels == ['y[0]'] # Change names - f1 = FRD(h1, omega, name='mysys', inputs='u0', outputs='y0') + f1 = frd(h1, omega, name='mysys', inputs='u0', outputs='y0') assert f1.name == 'mysys' assert f1.ninputs == 1 assert f1.input_labels == ['u0'] @@ -521,7 +557,7 @@ def test_to_pandas(): # Create a SISO frequency response h1 = TransferFunction([1], [1, 2, 2]) omega = np.logspace(-1, 2, 10) - resp = FRD(h1, omega) + resp = frd(h1, omega) # Convert to pandas df = resp.to_pandas() diff --git a/control/tests/freqplot_test.py b/control/tests/freqplot_test.py index 5383f28a7..4b98167d8 100644 --- a/control/tests/freqplot_test.py +++ b/control/tests/freqplot_test.py @@ -1,13 +1,14 @@ # freqplot_test.py - test out frequency response plots # RMM, 23 Jun 2023 -import pytest -import control as ct import matplotlib as mpl import matplotlib.pyplot as plt import numpy as np +import pytest + +import control as ct +from control.tests.conftest import editsdefaults, slycotonly -from control.tests.conftest import slycotonly pytestmark = pytest.mark.usefixtures("mplcleanup") # @@ -55,15 +56,19 @@ (True, True, None, 'row', True, False, False, False), (True, True, 'row', None, None, False, False, True), ]) +@pytest.mark.usefixtures("editsdefaults") def test_response_plots( sys, pltmag, pltphs, shrmag, shrphs, shrfrq, secsys, ovlout, ovlinp, clear=True): + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + # Save up the keyword arguments kwargs = dict( plot_magnitude=pltmag, plot_phase=pltphs, share_magnitude=shrmag, share_phase=shrphs, share_frequency=shrfrq, - overlay_outputs=ovlout, overlay_inputs=ovlinp + overlay_outputs=ovlout, overlay_inputs=ovlinp, ) # Create the response @@ -78,21 +83,22 @@ def test_response_plots( # Plot the frequency response plt.figure() - out = response.plot(**kwargs) + cplt = response.plot(**kwargs) # Check the shape if ovlout and ovlinp: - assert out.shape == (pltmag + pltphs, 1) + assert cplt.lines.shape == (pltmag + pltphs, 1) elif ovlout: - assert out.shape == (pltmag + pltphs, sys.ninputs) + assert cplt.lines.shape == (pltmag + pltphs, sys.ninputs) elif ovlinp: - assert out.shape == (sys.noutputs * (pltmag + pltphs), 1) + assert cplt.lines.shape == (sys.noutputs * (pltmag + pltphs), 1) else: - assert out.shape == (sys.noutputs * (pltmag + pltphs), sys.ninputs) + assert cplt.lines.shape == \ + (sys.noutputs * (pltmag + pltphs), sys.ninputs) # Make sure all of the outputs are of the right type nlines_plotted = 0 - for ax_lines in np.nditer(out, flags=["refs_ok"]): + for ax_lines in np.nditer(cplt.lines, flags=["refs_ok"]): for line in ax_lines.item() or []: assert isinstance(line, mpl.lines.Line2D) nlines_plotted += 1 @@ -120,13 +126,12 @@ def test_response_plots( assert len(ax.get_lines()) > 1 # Update the title so we can see what is going on - fig = out[0, 0][0].axes.figure - fig.suptitle( - fig._suptitle._text + + cplt.set_plot_title( + cplt.figure._suptitle._text + f" [{sys.noutputs}x{sys.ninputs}, pm={pltmag}, pp={pltphs}," f" sm={shrmag}, sp={shrphs}, sf={shrfrq}]", # TODO: ", " # f"oo={ovlout}, oi={ovlinp}, ss={secsys}]", # TODO: add back - fontsize='small') + frame='figure') # Get rid of the figure to free up memory if clear: @@ -136,8 +141,8 @@ def test_response_plots( # Use the manaul response to verify that different settings are working def test_manual_response_limits(): # Default response: limits should be the same across rows - out = manual_response.plot() - axs = ct.get_plot_axes(out) + cplt = manual_response.plot() + axs = cplt.axes for i in range(manual_response.noutputs): for j in range(1, manual_response.ninputs): # Everything in the same row should have the same limits @@ -150,7 +155,11 @@ def test_manual_response_limits(): @pytest.mark.parametrize( "plt_fcn", [ct.bode_plot, ct.nichols_plot, ct.singular_values_plot]) +@pytest.mark.usefixtures("editsdefaults") def test_line_styles(plt_fcn): + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + # Define a couple of systems for testing sys1 = ct.tf([1], [1, 2, 1], name='sys1') sys2 = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys2') @@ -181,6 +190,12 @@ def test_basic_freq_plots(savefigs=False): if savefigs: plt.savefig('freqplot-siso_bode-default.png') + plt.figure() + omega = np.logspace(-2, 2, 500) + ct.frequency_response([sys1, sys2], omega).plot(initial_phase=0) + if savefigs: + plt.savefig('freqplot-siso_bode-omega.png') + # Basic MIMO Bode plot plt.figure() sys_mimo = ct.tf( @@ -213,6 +228,24 @@ def test_basic_freq_plots(savefigs=False): if savefigs: plt.savefig('freqplot-siso_nichols-default.png') + # Nyquist plot - default settings + plt.figure() + sys = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys') + ct.nyquist(sys) + if savefigs: + plt.savefig('freqplot-nyquist-default.png') + + # Nyquist plot - custom settings + plt.figure() + sys = ct.tf([1, 0.2], [1, 0, 1]) * ct.tf([1], [1, 0]) + nyqresp = ct.nyquist_response(sys) + nyqresp.plot( + max_curve_magnitude=6, max_curve_offset=1, + arrows=[0, 0.15, 0.3, 0.6, 0.7, 0.925], label='sys') + print("Encirclements =", nyqresp.count) + if savefigs: + plt.savefig('freqplot-nyquist-custom.png') + def test_gangof4_plots(savefigs=False): proc = ct.tf([1], [1, 1, 1], name="process") @@ -230,7 +263,11 @@ def test_gangof4_plots(savefigs=False): (ct.nyquist_response, ct.freqplot.NyquistResponseData), (ct.singular_values_response, ct.FrequencyResponseData), ]) +@pytest.mark.usefixtures("editsdefaults") def test_first_arg_listable(response_cmd, return_type): + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + sys = ct.rss(2, 1, 1) # If we pass a single system, should get back a single system @@ -262,10 +299,14 @@ def test_first_arg_listable(response_cmd, return_type): assert isinstance(result[0], return_type) +@pytest.mark.usefixtures("editsdefaults") def test_bode_share_options(): + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + # Default sharing should share along rows and cols for mag and phase - lines = ct.bode_plot(manual_response) - axs = ct.get_plot_axes(lines) + cplt = ct.bode_plot(manual_response) + axs = cplt.axes for i in range(axs.shape[0]): for j in range(axs.shape[1]): # Share y limits along rows @@ -276,8 +317,8 @@ def test_bode_share_options(): # Sharing along y axis for mag but not phase plt.figure() - lines = ct.bode_plot(manual_response, share_phase='none') - axs = ct.get_plot_axes(lines) + cplt = ct.bode_plot(manual_response, share_phase='none') + axs = cplt.axes for i in range(int(axs.shape[0] / 2)): for j in range(axs.shape[1]): if i != 0: @@ -289,8 +330,8 @@ def test_bode_share_options(): # Turn off sharing for magnitude and phase plt.figure() - lines = ct.bode_plot(manual_response, sharey='none') - axs = ct.get_plot_axes(lines) + cplt = ct.bode_plot(manual_response, sharey='none') + axs = cplt.axes for i in range(int(axs.shape[0] / 2)): for j in range(axs.shape[1]): if i != 0: @@ -304,7 +345,7 @@ def test_bode_share_options(): # Turn off sharing in x axes plt.figure() - lines = ct.bode_plot(manual_response, sharex='none') + cplt = ct.bode_plot(manual_response, sharex='none') # TODO: figure out what to check @@ -314,14 +355,18 @@ def test_freqplot_plot_type(plot_type): response = ct.singular_values_response(ct.rss(2, 1, 1)) else: response = ct.frequency_response(ct.rss(2, 1, 1)) - lines = response.plot(plot_type=plot_type) + cplt = response.plot(plot_type=plot_type) if plot_type == 'bode': - assert lines.shape == (2, 1) + assert cplt.lines.shape == (2, 1) else: - assert lines.shape == (1, ) + assert cplt.lines.shape == (1, ) @pytest.mark.parametrize("plt_fcn", [ct.bode_plot, ct.singular_values_plot]) +@pytest.mark.usefixtures("editsdefaults") def test_freqplot_omega_limits(plt_fcn): + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + # Utility function to check visible limits def _get_visible_limits(ax): xticks = np.array(ax.get_xticks()) @@ -334,18 +379,225 @@ def _get_visible_limits(ax): ct.tf([1], [1, 2, 1]), np.logspace(-1, 1)) # Generate a plot without overridding the limits - lines = plt_fcn(response) - ax = ct.get_plot_axes(lines) + cplt = plt_fcn(response) + ax = cplt.axes np.testing.assert_allclose( _get_visible_limits(ax.reshape(-1)[0]), np.array([0.1, 10])) # Now reset the limits - lines = plt_fcn(response, omega_limits=(1, 100)) - ax = ct.get_plot_axes(lines) + cplt = plt_fcn(response, omega_limits=(1, 100)) + ax = cplt.axes np.testing.assert_allclose( _get_visible_limits(ax.reshape(-1)[0]), np.array([1, 100])) +def test_gangof4_trace_labels(): + P1 = ct.rss(2, 1, 1, name='P1') + P2 = ct.rss(3, 1, 1, name='P2') + C1 = ct.rss(1, 1, 1, name='C1') + C2 = ct.rss(1, 1, 1, name='C2') + + # Make sure default labels are as expected + cplt = ct.gangof4_response(P1, C1).plot() + cplt = ct.gangof4_response(P2, C2).plot() + axs = cplt.axes + legend = axs[0, 1].get_legend().get_texts() + assert legend[0].get_text() == 'P=P1, C=C1' + assert legend[1].get_text() == 'P=P2, C=C2' + plt.close() + + # Suffix truncation + cplt = ct.gangof4_response(P1, C1).plot() + cplt = ct.gangof4_response(P2, C1).plot() + axs = cplt.axes + legend = axs[0, 1].get_legend().get_texts() + assert legend[0].get_text() == 'P=P1' + assert legend[1].get_text() == 'P=P2' + plt.close() + + # Prefix turncation + cplt = ct.gangof4_response(P1, C1).plot() + cplt = ct.gangof4_response(P1, C2).plot() + axs = cplt.axes + legend = axs[0, 1].get_legend().get_texts() + assert legend[0].get_text() == 'C=C1' + assert legend[1].get_text() == 'C=C2' + plt.close() + + # Override labels + cplt = ct.gangof4_response(P1, C1).plot(label='xxx, line1, yyy') + cplt = ct.gangof4_response(P2, C2).plot(label='xxx, line2, yyy') + axs = cplt.axes + legend = axs[0, 1].get_legend().get_texts() + assert legend[0].get_text() == 'xxx, line1, yyy' + assert legend[1].get_text() == 'xxx, line2, yyy' + plt.close() + + +@pytest.mark.parametrize( + "plt_fcn", [ct.bode_plot, ct.singular_values_plot, ct.nyquist_plot]) +@pytest.mark.usefixtures("editsdefaults") +def test_freqplot_line_labels(plt_fcn): + sys1 = ct.rss(2, 1, 1, name='sys1') + sys2 = ct.rss(3, 1, 1, name='sys2') + + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + + # Make sure default labels are as expected + cplt = plt_fcn([sys1, sys2]) + axs = cplt.axes + if axs.ndim == 1: + legend = axs[0].get_legend().get_texts() + else: + legend = axs[0, 0].get_legend().get_texts() + assert legend[0].get_text() == 'sys1' + assert legend[1].get_text() == 'sys2' + plt.close() + + # Override labels all at once + cplt = plt_fcn([sys1, sys2], label=['line1', 'line2']) + axs = cplt.axes + if axs.ndim == 1: + legend = axs[0].get_legend().get_texts() + else: + legend = axs[0, 0].get_legend().get_texts() + assert legend[0].get_text() == 'line1' + assert legend[1].get_text() == 'line2' + plt.close() + + # Override labels one at a time + cplt = plt_fcn(sys1, label='line1') + cplt = plt_fcn(sys2, label='line2') + axs = cplt.axes + if axs.ndim == 1: + legend = axs[0].get_legend().get_texts() + else: + legend = axs[0, 0].get_legend().get_texts() + assert legend[0].get_text() == 'line1' + assert legend[1].get_text() == 'line2' + plt.close() + + +@pytest.mark.skip(reason="line label override not yet implemented") +@pytest.mark.parametrize("kwargs, labels", [ + ({}, ['sys1', 'sys2']), + ({'overlay_outputs': True}, [ + 'x sys1 out1 y', 'x sys1 out2 y', 'x sys2 out1 y', 'x sys2 out2 y']), +]) +def test_line_labels_bode(kwargs, labels): + # Multi-dimensional data + sys1 = ct.rss(2, 2, 2) + sys2 = ct.rss(3, 2, 2) + + # Check out some errors first + with pytest.raises(ValueError, match="number of labels must match"): + ct.bode_plot([sys1, sys2], label=['line1']) + + cplt = ct.bode_plot([sys1, sys2], label=labels, **kwargs) + axs = cplt.axes + legend_texts = axs[0, -1].get_legend().get_texts() + for i, legend in enumerate(legend_texts): + assert legend.get_text() == labels[i] + plt.close() + + +@pytest.mark.parametrize( + "plt_fcn", [ + ct.bode_plot, ct.singular_values_plot, ct.nyquist_plot, + ct.nichols_plot]) +@pytest.mark.parametrize( + "ninputs, noutputs", [(1, 1), (1, 2), (2, 1), (2, 3)]) +@pytest.mark.usefixtures("editsdefaults") +def test_freqplot_ax_keyword(plt_fcn, ninputs, noutputs): + if plt_fcn in [ct.nyquist_plot, ct.nichols_plot] and \ + (ninputs != 1 or noutputs != 1): + pytest.skip("MIMO not implemented for Nyquist/Nichols") + + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + + # System to use + sys = ct.rss(4, ninputs, noutputs) + + # Create an initial figure + cplt1 = plt_fcn(sys) + + # Draw again on the same figure, using array + axs = cplt1.axes + cplt2 = plt_fcn(sys, ax=axs) + np.testing.assert_equal(cplt1.axes, cplt2.axes) + + # Pass things in as a list instead + axs_list = axs.tolist() + cplt3 = plt_fcn(sys, ax=axs) + np.testing.assert_equal(cplt1.axes, cplt3.axes) + + # Flatten the list + axs_list = axs.squeeze().tolist() + cplt4 = plt_fcn(sys, ax=axs_list) + np.testing.assert_equal(cplt1.axes, cplt4.axes) + + +def test_mixed_systypes(): + s = ct.tf('s') + sys_tf = ct.tf( + (0.02 * s**3 - 0.1 * s) / (s**4 + s**3 + s**2 + 0.25 * s + 0.04), + name='tf') + sys_ss = ct.ss(sys_tf * 2, name='ss') + sys_frd1 = ct.frd(sys_tf / 2, np.logspace(-1, 1, 15), name='frd1') + sys_frd2 = ct.frd(sys_tf / 4, np.logspace(-3, 2, 20), name='frd2') + + # Simple case: compute responses separately and plot + resp_tf = ct.frequency_response(sys_tf) + resp_ss = ct.frequency_response(sys_ss) + plt.figure() + cplt = ct.bode_plot( + [resp_tf, resp_ss, sys_frd1, sys_frd2], plot_phase=False) + cplt.set_plot_title("bode_plot([resp_tf, resp_ss, sys_frd1, sys_frd2])") + + # Same thing, but using frequency response + plt.figure() + resp = ct.frequency_response([sys_tf, sys_ss, sys_frd1, sys_frd2]) + cplt = resp.plot(plot_phase=False) + cplt.set_plot_title( + "frequency_response([sys_tf, sys_ss, sys_frd1, sys_frd2])") + + # Same thing, but using bode_plot + plt.figure() + cplt = ct.bode_plot([sys_tf, sys_ss, sys_frd1, sys_frd2], plot_phase=False) + cplt.set_plot_title("bode_plot([sys_tf, sys_ss, sys_frd1, sys_frd2])") + + +def test_suptitle(): + sys = ct.rss(2, 2, 2, strictly_proper=True) + + # Default location: center of axes + cplt = ct.bode_plot(sys) + assert plt.gcf()._suptitle._x != 0.5 + + # Try changing the the title + cplt.set_plot_title("New title") + assert plt.gcf()._suptitle._text == "New title" + + # Change the location of the title + cplt.set_plot_title("New title", frame='figure') + assert plt.gcf()._suptitle._x == 0.5 + + # Change the location of the title back + cplt.set_plot_title("New title", frame='axes') + assert plt.gcf()._suptitle._x != 0.5 + + # Bad frame + with pytest.raises(ValueError, match="unknown"): + cplt.set_plot_title("New title", frame='nowhere') + + # Bad keyword + with pytest.raises( + TypeError, match="unexpected keyword|no property"): + cplt.set_plot_title("New title", unknown=None) + + @pytest.mark.parametrize("plt_fcn", [ct.bode_plot, ct.singular_values_plot]) def test_freqplot_errors(plt_fcn): if plt_fcn == ct.bode_plot: @@ -365,6 +617,15 @@ def test_freqplot_errors(plt_fcn): with pytest.raises(ValueError, match="invalid limits"): plt_fcn(response, omega_limits=[1e2, 1e-2]) +def test_freqresplist_unknown_kw(): + sys1 = ct.rss(2, 1, 1) + sys2 = ct.rss(2, 1, 1) + resp = ct.frequency_response([sys1, sys2]) + assert isinstance(resp, ct.FrequencyResponseList) + + with pytest.raises(AttributeError, match="unexpected keyword"): + resp.plot(unknown=True) + if __name__ == "__main__": # @@ -405,6 +666,9 @@ def test_freqplot_errors(plt_fcn): for args in test_cases: test_response_plots(*args, ovlinp=False, ovlout=False, clear=False) + # Reset title_frame to the default value + ct.reset_defaults() + # Define and run a selected set of interesting tests # TODO: TBD (see timeplot_test.py for format) @@ -415,5 +679,4 @@ def test_freqplot_errors(plt_fcn): # Run a few more special cases to show off capabilities (and save some # of them for use in the documentation). # - - pass + test_mixed_systypes() diff --git a/control/tests/freqresp_test.py b/control/tests/freqresp_test.py index 18c59384d..3ff1a51c5 100644 --- a/control/tests/freqresp_test.py +++ b/control/tests/freqresp_test.py @@ -60,7 +60,7 @@ def test_freqresp_siso(ss_siso): ctrl.frequency_response(ss_siso, omega) -@pytest.mark.filterwarnings("ignore:freqresp is deprecated") +@pytest.mark.filterwarnings(r"ignore:freqresp\(\) is deprecated") @slycotonly def test_freqresp_mimo_legacy(ss_mimo): """Test MIMO frequency response calls""" @@ -112,7 +112,7 @@ def test_nyquist_basic(ss_siso): # Check known warnings happened as expected assert len(record) == 2 assert re.search("encirclements was a non-integer", str(record[0].message)) - assert re.search("return values .* deprecated", str(record[1].message)) + assert re.search("return value .* deprecated", str(record[1].message)) response = nyquist_response(tf_siso, omega=np.logspace(-1, 1, 10)) assert len(response.contour) == 10 @@ -709,3 +709,25 @@ def test_singular_values_plot_mpl_superimpose_nyq(ss_mimo_ct, ss_mimo_dt): assert(len(nyquist_line[0]) == 2) assert(nyquist_line[0][0] == nyquist_line[0][1]) assert(nyquist_line[0][0] == np.pi/sys_dt.dt) + + +def test_freqresp_omega_limits(): + sys = ctrl.rss(4, 1, 1) + + # Generate a standard frequency response (no limits specified) + resp0 = ctrl.frequency_response(sys) + + # Regenerate the response using omega_limits + resp1 = ctrl.frequency_response( + sys, omega_limits=[resp0.omega[0], resp0.omega[-1]]) + np.testing.assert_equal(resp0.omega, resp1.omega) + + # Regenerate the response using omega as a list of two elements + resp2 = ctrl.frequency_response(sys, [resp0.omega[0], resp0.omega[-1]]) + np.testing.assert_equal(resp0.omega, resp2.omega) + assert resp2.omega.size > 100 + + # Make sure that generating response using array does the right thing + resp3 = ctrl.frequency_response( + sys, np.array([resp0.omega[0], resp0.omega[-1]])) + np.testing.assert_equal(resp3.omega, [resp0.omega[0], resp0.omega[-1]]) diff --git a/control/tests/interconnect_test.py b/control/tests/interconnect_test.py index 285e9d096..604488ca5 100644 --- a/control/tests/interconnect_test.py +++ b/control/tests/interconnect_test.py @@ -16,6 +16,7 @@ import numpy as np import scipy as sp +import math import control as ct @@ -659,3 +660,48 @@ def test_interconnect_rewrite(): outputs=['y', 'z']) assert icsys.input_labels == ['u[0]', 'u[1]', 'w[0]', 'w[1]'] + + +def test_interconnect_params(): + # Create a nominally unstable system + sys1 = ct.nlsys( + lambda t, x, u, params: params['a'] * x[0] + u[0], + states=1, inputs='u', outputs='y', params={'a': 1}) + + # Simple system for serial interconnection + sys2 = ct.nlsys( + None, lambda t, x, u, params: u[0], + inputs='r', outputs='u') + + # Create a series interconnection + sys = ct.interconnect([sys1, sys2], inputs='r', outputs='y') + + # Make sure we can call the update function + sys.updfcn(0, [0], [0], {}) + + # Make sure the serial interconnection is unstable to start + assert sys.linearize([0], [0]).poles()[0].real == 1 + + # Change the parameter and make sure it takes + assert sys.linearize([0], [0], params={'a': -1}).poles()[0].real == -1 + + # Now try running a simulation + timepts = np.linspace(0, 10) + resp = ct.input_output_response(sys, timepts, 0, params={'a': -1}) + assert resp.states[0, -1].item() < 2 * math.exp(-10) + + +# Bug identified in issue #1015 +def test_parallel_interconnect(): + sys1 = ct.rss(2, 1, 1, name='S1') + sys2 = ct.rss(2, 1, 1, name='S2') + + sys_bd = sys1 + sys2 + sys_ic = ct.interconnect( + [sys1, sys2], + inplist=[['S1.u[0]', 'S2.u[0]']], + outlist=[['S1.y[0]', 'S2.y[0]']]) + np.testing.assert_allclose(sys_bd.A, sys_ic.A) + np.testing.assert_allclose(sys_bd.B, sys_ic.B) + np.testing.assert_allclose(sys_bd.C, sys_ic.C) + np.testing.assert_allclose(sys_bd.D, sys_ic.D) diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index f3693cf00..dd30ea71e 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -77,7 +77,7 @@ def test_tf2io(self, tsys): # Create a transfer function from the state space system linsys = tsys.siso_linsys tfsys = ct.ss2tf(linsys) - with pytest.warns(DeprecationWarning, match="use tf2ss"): + with pytest.warns(FutureWarning, match="use tf2ss"): iosys = ct.tf2io(tfsys) # Verify correctness via simulation @@ -90,13 +90,13 @@ def test_tf2io(self, tsys): # Make sure that non-proper transfer functions generate an error tfsys = ct.tf('s') with pytest.raises(ValueError): - with pytest.warns(DeprecationWarning, match="use tf2ss"): + with pytest.warns(FutureWarning, match="use tf2ss"): iosys=ct.tf2io(tfsys) def test_ss2io(self, tsys): # Create an input/output system from the linear system linsys = tsys.siso_linsys - with pytest.warns(DeprecationWarning, match="use ss"): + with pytest.warns(FutureWarning, match="use ss"): iosys = ct.ss2io(linsys) np.testing.assert_allclose(linsys.A, iosys.A) np.testing.assert_allclose(linsys.B, iosys.B) @@ -104,7 +104,7 @@ def test_ss2io(self, tsys): np.testing.assert_allclose(linsys.D, iosys.D) # Try adding names to things - with pytest.warns(DeprecationWarning, match="use ss"): + with pytest.warns(FutureWarning, match="use ss"): iosys_named = ct.ss2io(linsys, inputs='u', outputs='y', states=['x1', 'x2'], name='iosys_named') assert iosys_named.find_input('u') == 0 @@ -231,11 +231,20 @@ def test_linearize(self, tsys, kincar): linearized.C, [[1, 0, 0], [0, 1, 0]]) np.testing.assert_array_almost_equal(linearized.D, np.zeros((2,2))) + # Pass fewer than the required elements + padded = iosys.linearize([0, 0], np.array([0])) + assert padded.nstates == linearized.nstates + assert padded.ninputs == linearized.ninputs + + # Check for warning if last element before padding is nonzero + with pytest.warns(UserWarning, match="x0 too short; padding"): + padded = iosys.linearize([0, 1], np.array([0])) + @pytest.mark.usefixtures("editsdefaults") def test_linearize_named_signals(self, kincar): # Full form of the call - linearized = kincar.linearize([0, 0, 0], [0, 0], copy_names=True, - name='linearized') + linearized = kincar.linearize( + [0, 0, 0], [0, 0], copy_names=True, name='linearized') assert linearized.name == 'linearized' assert linearized.find_input('v') == 0 assert linearized.find_input('phi') == 1 @@ -256,8 +265,8 @@ def test_linearize_named_signals(self, kincar): assert lin_nocopy.find_state('x') is None # if signal names are provided, they should override those of kincar - linearized_newnames = kincar.linearize([0, 0, 0], [0, 0], - name='linearized', + linearized_newnames = kincar.linearize( + [0, 0, 0], [0, 0], name='linearized', copy_names=True, inputs=['v2', 'phi2'], outputs=['x2','y2']) assert linearized_newnames.name == 'linearized' assert linearized_newnames.find_input('v2') == 0 @@ -269,6 +278,11 @@ def test_linearize_named_signals(self, kincar): assert linearized_newnames.find_output('x') is None assert linearized_newnames.find_output('y') is None + # if system name is provided but copy_names is false, override name + linearized_newsysname = kincar.linearize( + [0, 0, 0], [0, 0], name='newname', copy_names=False) + assert linearized_newsysname.name == 'newname' + # Test legacy version as well with pytest.warns(UserWarning, match="NumPy matrix class no longer"): ct.use_legacy_defaults('0.8.4') @@ -1417,7 +1431,7 @@ def test_operand_badtype(self, C, op): def test_neg_badsize(self): # Create a system of unspecified size sys = ct.NonlinearIOSystem(lambda t, x, u, params: -x) - with pytest.raises(ValueError, match="Can't determine"): + with pytest.raises(ValueError, match="Can't determine number"): -sys def test_bad_signal_list(self): @@ -1881,7 +1895,7 @@ def test_input_output_broadcasting(): np.testing.assert_equal(resp_cov0.states, resp_init.states) # Specify only some of the initial conditions - with pytest.warns(UserWarning, match="initial state too short; padding"): + with pytest.warns(UserWarning, match="X0 too short; padding"): resp_short = ct.input_output_response(sys, T, [U[0], [0, 1]], [X0, 1]) # Make sure that inconsistent settings don't work @@ -1928,7 +1942,7 @@ def test_nonuniform_timepts(nstates, noutputs, ninputs): def test_ss_nonlinear(): """Test ss() for creating nonlinear systems""" - with pytest.warns(DeprecationWarning, match="use nlsys()"): + with pytest.warns(FutureWarning, match="use nlsys()"): secord = ct.ss(secord_update, secord_output, inputs='u', outputs='y', states = ['x1', 'x2'], name='secord') assert secord.name == 'secord' @@ -1949,12 +1963,12 @@ def test_ss_nonlinear(): np.testing.assert_almost_equal(ss_response.outputs, io_response.outputs) # Make sure that optional keywords are allowed - with pytest.warns(DeprecationWarning, match="use nlsys()"): + with pytest.warns(FutureWarning, match="use nlsys()"): secord = ct.ss(secord_update, secord_output, dt=True) assert ct.isdtime(secord) # Make sure that state space keywords are flagged - with pytest.warns(DeprecationWarning, match="use nlsys()"): + with pytest.warns(FutureWarning, match="use nlsys()"): with pytest.raises(TypeError, match="unrecognized keyword"): ct.ss(secord_update, remove_useless_states=True) @@ -2072,6 +2086,7 @@ def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect): 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) @@ -2082,3 +2097,74 @@ def test_iosys_sample(): dsys = ct.sample_system(csys, 0.1) assert isinstance(dsys, ct.StateSpace) assert dsys.dt == 0.1 + + +# Make sure that we can determine system sizes automatically +def test_find_size(): + # Create a nonlinear system with no size information + sys = ct.nlsys( + lambda t, x, u, params: -x + u, + lambda t, x, u, params: x[:1]) + + # Run a simulation with size set by parameters + timepts = np.linspace(0, 1) + resp = ct.input_output_response(sys, timepts, [0, 1], X0=[0, 0]) + assert resp.states.shape[0] == 2 + assert resp.inputs.shape[0] == 2 + assert resp.outputs.shape[0] == 1 + + # + # Make sure we get warnings if things are inconsistent + # + + # Define a system of fixed size + sys = ct.nlsys( + lambda t, x, u, params: -x + u, + lambda t, x, u, params: x[:1], + inputs=2, states=2) + + with pytest.raises(ValueError, match="inconsistent .* size of X0"): + resp = ct.input_output_response(sys, timepts, [0, 1], X0=[0, 0, 1]) + + with pytest.raises(ValueError, match=".*U.* Wrong shape"): + resp = ct.input_output_response(sys, timepts, [0, 1, 2], X0=[0, 0]) + + with pytest.raises(RuntimeError, match="inconsistent size of outputs"): + sys = ct.nlsys( + lambda t, x, u, params: -x + u, + lambda t, x, u, params: x[:1], + inputs=2, states=2, outputs=2) + resp = ct.input_output_response(sys, timepts, [0, 1], X0=[0, 0]) + + +def test_update_names(): + sys = ct.rss(['x1', 'x2'], 2, 2) + sys.update_names( + name='new', states=2, inputs=['u1', 'u2'], + outputs=2, output_prefix='yy') + assert sys.name == 'new' + assert sys.ninputs == 2 + assert sys.input_labels == ['u1', 'u2'] + assert sys.ninputs == 2 + assert sys.output_labels == ['yy[0]', 'yy[1]'] + assert sys.state_labels == ['x[0]', 'x[1]'] + + # Generate some error conditions + with pytest.raises(ValueError, match="number of inputs does not match"): + sys.update_names(inputs=3) + + with pytest.raises(ValueError, match="number of outputs does not match"): + sys.update_names(outputs=3) + + with pytest.raises(ValueError, match="number of states does not match"): + sys.update_names(states=3) + + with pytest.raises(ValueError, match="number of states does not match"): + siso = ct.tf([1], [1, 2, 1]) + ct.tf(siso).update_names(states=2) + + with pytest.raises(TypeError, match="unrecognized keywords"): + sys.update_names(dt=1) + + with pytest.raises(TypeError, match=".* takes 1 positional argument"): + sys.update_names(5) diff --git a/control/tests/kwargs_test.py b/control/tests/kwargs_test.py index 8180ff418..020910e73 100644 --- a/control/tests/kwargs_test.py +++ b/control/tests/kwargs_test.py @@ -11,23 +11,25 @@ # is a unit test that checks for unrecognized keywords. import inspect -import pytest import warnings + import matplotlib.pyplot as plt +import pytest import control import control.flatsys - +import control.tests.descfcn_test as descfcn_test # List of all of the test modules where kwarg unit tests are defined import control.tests.flatsys_test as flatsys_test import control.tests.frd_test as frd_test +import control.tests.freqplot_test as freqplot_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.stochsys_test as stochsys_test -import control.tests.trdata_test as trdata_test import control.tests.timeplot_test as timeplot_test -import control.tests.descfcn_test as descfcn_test +import control.tests.trdata_test as trdata_test + @pytest.mark.parametrize("module, prefix", [ (control, ""), (control.flatsys, "flatsys."), @@ -53,8 +55,9 @@ def test_kwarg_search(module, prefix): # Get the signature for the function sig = inspect.signature(obj) - # Skip anything that is inherited - if inspect.isclass(module) and obj.__name__ not in module.__dict__: + # Skip anything that is inherited or hidden + if inspect.isclass(module) and obj.__name__ not in module.__dict__ \ + or obj.__name__.startswith('_'): continue # See if there is a variable keyword argument @@ -91,20 +94,25 @@ def test_kwarg_search(module, prefix): @pytest.mark.parametrize( "function, nsssys, ntfsys, moreargs, kwargs", - [(control.dlqe, 1, 0, ([[1]], [[1]]), {}), + [(control.append, 2, 0, (), {}), + (control.dlqe, 1, 0, ([[1]], [[1]]), {}), (control.dlqr, 1, 0, ([[1, 0], [0, 1]], [[1]]), {}), (control.drss, 0, 0, (2, 1, 1), {}), + (control.feedback, 2, 0, (), {}), (control.flatsys.flatsys, 1, 0, (), {}), (control.input_output_response, 1, 0, ([0, 1, 2], [1, 1, 1]), {}), (control.lqe, 1, 0, ([[1]], [[1]]), {}), (control.lqr, 1, 0, ([[1, 0], [0, 1]], [[1]]), {}), (control.linearize, 1, 0, (0, 0), {}), + (control.negate, 1, 0, (), {}), (control.nlsys, 0, 0, (lambda t, x, u, params: np.array([0]),), {}), + (control.parallel, 2, 0, (), {}), (control.pzmap, 1, 0, (), {}), (control.rlocus, 0, 1, (), {}), (control.root_locus, 0, 1, (), {}), (control.root_locus_plot, 0, 1, (), {}), (control.rss, 0, 0, (2, 1, 1), {}), + (control.series, 2, 0, (), {}), (control.set_defaults, 0, 0, ('control',), {'default_dt': True}), (control.ss, 0, 0, (0, 0, 0, 0), {'dt': 1}), (control.ss2io, 1, 0, (), {}), @@ -121,6 +129,7 @@ def test_kwarg_search(module, prefix): (control.LTI, 0, 0, (), {'inputs': 1, 'outputs': 1, 'states': 1}), (control.flatsys.LinearFlatSystem, 1, 0, (), {}), + (control.InputOutputSystem.update_names, 1, 0, (), {}), (control.NonlinearIOSystem.linearize, 1, 0, (0, 0), {}), (control.StateSpace.sample, 1, 0, (0.1,), {}), (control.StateSpace, 0, 0, @@ -193,9 +202,9 @@ def test_matplotlib_kwargs(function, nsysargs, moreargs, kwargs, mplcleanup): def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): # Create a system for testing if mimo: - response = data_fcn(control.rss(4, 2, 2)) + response = data_fcn(control.rss(4, 2, 2, strictly_proper=True)) else: - response = data_fcn(control.rss(4, 1, 1)) + response = data_fcn(control.rss(4, 1, 1, strictly_proper=True)) # Make sure that calling the data function with unknown keyword errs with pytest.raises( @@ -231,6 +240,7 @@ def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): # kwarg_unittest = { + 'append': test_unrecognized_kwargs, 'bode': test_response_plot_kwargs, 'bode_plot': test_response_plot_kwargs, 'create_estimator_iosystem': stochsys_test.test_estimator_errors, @@ -241,7 +251,9 @@ def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): 'dlqe': test_unrecognized_kwargs, 'dlqr': test_unrecognized_kwargs, 'drss': test_unrecognized_kwargs, + 'feedback': test_unrecognized_kwargs, 'flatsys.flatsys': test_unrecognized_kwargs, + 'frd': frd_test.TestFRD.test_unrecognized_keyword, 'gangof4': test_matplotlib_kwargs, 'gangof4_plot': test_matplotlib_kwargs, 'input_output_response': test_unrecognized_kwargs, @@ -250,6 +262,7 @@ def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): 'linearize': test_unrecognized_kwargs, 'lqe': test_unrecognized_kwargs, 'lqr': test_unrecognized_kwargs, + 'negate': test_unrecognized_kwargs, 'nichols_plot': test_matplotlib_kwargs, 'nichols': test_matplotlib_kwargs, 'nlsys': test_unrecognized_kwargs, @@ -257,18 +270,21 @@ def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): 'nyquist_response': test_response_plot_kwargs, 'nyquist_plot': test_matplotlib_kwargs, 'phase_plane_plot': test_matplotlib_kwargs, + 'parallel': test_unrecognized_kwargs, 'pole_zero_plot': test_unrecognized_kwargs, 'pzmap': test_unrecognized_kwargs, 'rlocus': test_unrecognized_kwargs, 'root_locus': test_unrecognized_kwargs, 'root_locus_plot': test_unrecognized_kwargs, 'rss': test_unrecognized_kwargs, + 'series': test_unrecognized_kwargs, 'set_defaults': test_unrecognized_kwargs, 'singular_values_plot': test_matplotlib_kwargs, 'ss': test_unrecognized_kwargs, 'ss2io': test_unrecognized_kwargs, 'ss2tf': test_unrecognized_kwargs, 'summing_junction': interconnect_test.test_interconnect_exceptions, + 'suptitle': freqplot_test.test_suptitle, 'tf': test_unrecognized_kwargs, 'tf2io' : test_unrecognized_kwargs, 'tf2ss' : test_unrecognized_kwargs, @@ -283,12 +299,15 @@ def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): 'optimal.create_mpc_iosystem': optimal_test.test_mpc_iosystem_rename, 'optimal.solve_ocp': optimal_test.test_ocp_argument_errors, 'optimal.solve_oep': optimal_test.test_oep_argument_errors, + 'ControlPlot.set_plot_title': freqplot_test.test_suptitle, 'FrequencyResponseData.__init__': frd_test.TestFRD.test_unrecognized_keyword, 'FrequencyResponseData.plot': test_response_plot_kwargs, + 'FrequencyResponseList.plot': freqplot_test.test_freqresplist_unknown_kw, 'DescribingFunctionResponse.plot': descfcn_test.test_describing_function_exceptions, 'InputOutputSystem.__init__': test_unrecognized_kwargs, + 'InputOutputSystem.update_names': test_unrecognized_kwargs, 'LTI.__init__': test_unrecognized_kwargs, 'flatsys.LinearFlatSystem.__init__': test_unrecognized_kwargs, 'NonlinearIOSystem.linearize': test_unrecognized_kwargs, @@ -305,6 +324,7 @@ def test_response_plot_kwargs(data_fcn, plot_fcn, mimo): 'StateSpace.sample': test_unrecognized_kwargs, 'TimeResponseData.__call__': trdata_test.test_response_copy, 'TimeResponseData.plot': timeplot_test.test_errors, + 'TimeResponseList.plot': timeplot_test.test_errors, 'TransferFunction.__init__': test_unrecognized_kwargs, 'TransferFunction.sample': test_unrecognized_kwargs, 'optimal.OptimalControlProblem.__init__': diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index 2ba3d5df8..26ff16774 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -83,6 +83,7 @@ class tsystems: @pytest.mark.usefixtures("fixedseed") +@pytest.mark.filterwarnings("ignore::FutureWarning") class TestMatlab: """Test matlab style functions""" @@ -173,6 +174,7 @@ def testPZmap(self, siso, subsys, mplcleanup): # pzmap(siso.ss1); not implemented # pzmap(siso.ss2); not implemented pzmap(getattr(siso, subsys)) + # TODO: check to make sure a plot got generated pzmap(getattr(siso, subsys), plot=False) def testStep(self, siso): @@ -404,6 +406,7 @@ def testDcgain_mimo(self, mimo): def testBode(self, siso, mplcleanup): """Call bode()""" + # TODO: make sure plots are generated bode(siso.ss1) bode(siso.tf1) bode(siso.tf2) diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py index 49c2afd58..616ef5f09 100644 --- a/control/tests/modelsimp_test.py +++ b/control/tests/modelsimp_test.py @@ -6,11 +6,12 @@ import numpy as np import pytest - -from control import StateSpace, forced_response, tf, rss, c2d -from control.exception import ControlMIMONotImplemented +from control import StateSpace, TimeResponseData, c2d, forced_response, \ + impulse_response, step_response, rss, tf +from control.exception import ControlArgument, ControlDimension +from control.modelsimp import balred, eigensys_realization, hsvd, markov, \ + modred from control.tests.conftest import slycotonly -from control.modelsimp import balred, hsvd, markov, modred class TestModelsimp: @@ -33,36 +34,149 @@ def testHSVD(self): assert not isinstance(hsv, np.matrix) def testMarkovSignature(self): - U = np.array([[1., 1., 1., 1., 1.]]) + U = np.array([[1., 1., 1., 1., 1., 1., 1.]]) Y = U + response = TimeResponseData(time=np.arange(U.shape[-1]), + outputs=Y, + output_labels='y', + inputs=U, + input_labels='u', + ) + + # setup m = 3 - H = markov(Y, U, m, transpose=False) - Htrue = np.array([[1., 0., 0.]]) - np.testing.assert_array_almost_equal(H, Htrue) + Htrue = np.array([1., 0., 0.]) + Htrue_l = np.array([1., 0., 0., 0., 0., 0., 0.]) + + # test not enough input arguments + with pytest.raises(ControlArgument): + H = markov(Y) + with pytest.raises(ControlArgument): + H = markov() + + # too many positional arguments + with pytest.raises(ControlArgument): + H = markov(Y,U,m,1) + with pytest.raises(ControlArgument): + H = markov(response,m,1) + + # too many positional arguments + with pytest.raises(ControlDimension): + U2 = np.hstack([U,U]) + H = markov(Y,U2,m) + + # not enough data + with pytest.warns(Warning): + H = markov(Y,U,8) + + # Basic Usage, m=l + H = markov(Y, U) + np.testing.assert_array_almost_equal(H, Htrue_l) - # Make sure that transposed data also works - H = markov(np.transpose(Y), np.transpose(U), m, transpose=True) - np.testing.assert_array_almost_equal(H, np.transpose(Htrue)) + H = markov(response) + np.testing.assert_array_almost_equal(H, Htrue_l) - # Generate Markov parameters without any arguments + # Basic Usage, m H = markov(Y, U, m) np.testing.assert_array_almost_equal(H, Htrue) + H = markov(response, m) + np.testing.assert_array_almost_equal(H, Htrue) + + H = markov(Y, U, m=m) + np.testing.assert_array_almost_equal(H, Htrue) + + H = markov(response, m=m) + np.testing.assert_array_almost_equal(H, Htrue) + + response.transpose=False + H = markov(response, m=m) + np.testing.assert_array_almost_equal(H, Htrue) + + # Make sure that transposed data also works, siso + HT = markov(Y.T, U.T, m, transpose=True) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + + response.transpose = True + HT = markov(response, m) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + response.transpose=False + + # Test example from docstring + # TODO: There is a problem here, last markov parameter does not fit + # the approximation error could be to big + Htrue = np.array([0, 1., -0.5]) T = np.linspace(0, 10, 100) U = np.ones((1, 100)) T, Y = forced_response(tf([1], [1, 0.5], True), T, U) - H = markov(Y, U, 3, transpose=False) + H = markov(Y, U, 4, dt=True) + np.testing.assert_array_almost_equal(H[:3], Htrue[:3]) + + response = forced_response(tf([1], [1, 0.5], True), T, U) + H = markov(response, 4, dt=True) + np.testing.assert_array_almost_equal(H[:3], Htrue[:3]) # Test example from issue #395 inp = np.array([1, 2]) outp = np.array([2, 4]) mrk = markov(outp, inp, 1, transpose=False) - # Make sure MIMO generates an error - U = np.ones((2, 100)) # 2 inputs (Y unchanged, with 1 output) - with pytest.raises(ControlMIMONotImplemented): - markov(Y, U, m) + # Test mimo example + # Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. + # Figure 6.5 / Example 6.7 + m1, k1, c1 = 1., 4., 1. + m2, k2, c2 = 2., 2., 1. + k3, c3 = 6., 2. + + A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] + ]) + B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) + C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) + D = np.zeros((2,2)) + + sys = StateSpace(A, B, C, D) + dt = 0.25 + sysd = sys.sample(dt, method='zoh') + + T = np.arange(0,100,dt) + U = np.random.randn(sysd.B.shape[-1], len(T)) + response = forced_response(sysd, U=U) + Y = response.outputs + + m = 100 + _, Htrue = impulse_response(sysd, T=dt*(m-1)) + + + # test array_like + H = markov(Y, U, m, dt=dt) + np.testing.assert_array_almost_equal(H, Htrue) + + # test array_like, truncate + H = markov(Y, U, m, dt=dt, truncate=True) + np.testing.assert_array_almost_equal(H, Htrue) + + # test array_like, transpose + HT = markov(Y.T, U.T, m, dt=dt, transpose=True) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + + # test response data + H = markov(response, m, dt=dt) + np.testing.assert_array_almost_equal(H, Htrue) + + # test response data + H = markov(response, m, dt=dt, truncate=True) + np.testing.assert_array_almost_equal(H, Htrue) + + # test response data, transpose + response.transpose = True + HT = markov(response, m, dt=dt) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + # Make sure markov() returns the right answer @pytest.mark.parametrize("k, m, n", @@ -98,18 +212,113 @@ def testMarkovResults(self, k, m, n): Mtrue = np.hstack([Hd.D] + [ Hd.C @ np.linalg.matrix_power(Hd.A, i) @ Hd.B for i in range(m-1)]) + + Mtrue = np.squeeze(Mtrue) # Generate input/output data T = np.array(range(n)) * Ts U = np.cos(T) + np.sin(T/np.pi) - _, Y = forced_response(Hd, T, U, squeeze=True) - Mcomp = markov(Y, U, m) + + ir_true = impulse_response(Hd,T) + Mtrue_scaled = ir_true[1][:m] # Compare to results from markov() # experimentally determined probability to get non matching results # with rtot=1e-6 and atol=1e-8 due to numerical errors # for k=5, m=n=10: 0.015 % + T, Y = forced_response(Hd, T, U, squeeze=True) + Mcomp = markov(Y, U, m, dt=True) + Mcomp_scaled = markov(Y, U, m, dt=Ts) + + np.testing.assert_allclose(Mtrue, Mcomp, rtol=1e-6, atol=1e-8) + np.testing.assert_allclose(Mtrue_scaled, Mcomp_scaled, rtol=1e-6, atol=1e-8) + + response = forced_response(Hd, T, U, squeeze=True) + Mcomp = markov(response, m, dt=True) + Mcomp_scaled = markov(response, m, dt=Ts) + np.testing.assert_allclose(Mtrue, Mcomp, rtol=1e-6, atol=1e-8) + np.testing.assert_allclose(Mtrue_scaled, Mcomp_scaled, rtol=1e-6, atol=1e-8) + + + def testERASignature(self): + + # test siso + # Katayama, Subspace Methods for System Identification + # Example 6.1, Fibonacci sequence + H_true = np.array([0.,1.,1.,2.,3.,5.,8.,13.,21.,34.]) + + # A realization of fibonacci impulse response + A = np.array([[0., 1.],[1., 1.,]]) + B = np.array([[1.],[1.,]]) + C = np.array([[1., 0.,]]) + D = np.array([[0.,]]) + + T = np.arange(0,10,1) + sysd_true = StateSpace(A,B,C,D,True) + ir_true = impulse_response(sysd_true,T=T) + + # test TimeResponseData + sysd_est, _ = eigensys_realization(ir_true,r=2) + ir_est = impulse_response(sysd_est, T=T) + _, H_est = ir_est + + np.testing.assert_allclose(H_true, H_est, rtol=1e-6, atol=1e-8) + + # test ndarray + _, YY_true = ir_true + sysd_est, _ = eigensys_realization(YY_true,r=2) + ir_est = impulse_response(sysd_est, T=T) + _, H_est = ir_est + + np.testing.assert_allclose(H_true, H_est, rtol=1e-6, atol=1e-8) + + # test mimo + # Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. + # Figure 6.5 / Example 6.7 + # m q_dd + c q_d + k q = f + m1, k1, c1 = 1., 4., 1. + m2, k2, c2 = 2., 2., 1. + k3, c3 = 6., 2. + + A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] + ]) + B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) + C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) + D = np.zeros((2,2)) + + sys = StateSpace(A, B, C, D) + + dt = 0.1 + T = np.arange(0,10,dt) + sysd_true = sys.sample(dt, method='zoh') + ir_true = impulse_response(sysd_true, T=T) + + # test TimeResponseData + sysd_est, _ = eigensys_realization(ir_true,r=4,dt=dt) + + step_true = step_response(sysd_true) + step_est = step_response(sysd_est) + + np.testing.assert_allclose(step_true.outputs, + step_est.outputs, + rtol=1e-6, atol=1e-8) + + # test ndarray + _, YY_true = ir_true + sysd_est, _ = eigensys_realization(YY_true,r=4,dt=dt) + + step_true = step_response(sysd_true, T=T) + step_est = step_response(sysd_est, T=T) + + np.testing.assert_allclose(step_true.outputs, + step_est.outputs, + rtol=1e-6, atol=1e-8) + def testModredMatchDC(self): #balanced realization computed in matlab for the transfer function: diff --git a/control/tests/nlsys_test.py b/control/tests/nlsys_test.py index 1c2976c56..7f649e0cc 100644 --- a/control/tests/nlsys_test.py +++ b/control/tests/nlsys_test.py @@ -9,6 +9,7 @@ import pytest import numpy as np +import math import control as ct # Basic test of nlsys() @@ -45,6 +46,7 @@ def kincar_output(t, x, u, params): ]) def test_lti_nlsys_response(nin, nout, input, output): sys_ss = ct.rss(4, nin, nout, strictly_proper=True) + sys_ss.A = np.diag([-1, -2, -3, -4]) # avoid random numerical errors sys_nl = ct.nlsys( lambda t, x, u, params: sys_ss.A @ x + sys_ss.B @ u, lambda t, x, u, params: sys_ss.C @ x + sys_ss.D @ u, @@ -92,3 +94,63 @@ def test_nlsys_impulse(): # Impulse_response (not implemented) with pytest.raises(ValueError, match="system must be LTI"): resp_nl = ct.impulse_response(sys_nl, timepts) + + +# Test nonlinear systems that are missing inputs or outputs +def test_nlsys_empty_io(): + + # No inputs + sys_nl = ct.nlsys( + lambda t, x, u, params: -x, lambda t, x, u, params: x[0:2], + name="no inputs", states=3, inputs=0, outputs=2) + P = sys_nl.linearize(np.zeros(sys_nl.nstates), None) + assert P.A.shape == (3, 3) + assert P.B.shape == (3, 0) + assert P.C.shape == (2, 3) + assert P.D.shape == (2, 0) + + # Check that we can compute dynamics and outputs + x = np.array([1, 2, 3]) + np.testing.assert_equal(sys_nl.dynamics(0, x, None, {}), -x) + np.testing.assert_equal(P.dynamics(0, x, None), -x) + np.testing.assert_equal(sys_nl.output(0, x, None, {}), x[0:2]) + np.testing.assert_equal(P.output(0, x, None), x[0:2]) + + # Make sure initial response runs OK + resp = ct.initial_response(sys_nl, np.linspace(0, 1), x) + np.testing.assert_allclose( + resp.states[:, -1], x * math.exp(-1), atol=1e-3, rtol=1e-3) + + resp = ct.initial_response(P, np.linspace(0, 1), x) + np.testing.assert_allclose(resp.states[:, -1], x * math.exp(-1)) + + # No outputs + sys_nl = ct.nlsys( + lambda t, x, u, params: -x + np.array([1, 1, 1]) * u[0], None, + name="no outputs", states=3, inputs=1, outputs=0) + P = sys_nl.linearize(np.zeros(sys_nl.nstates), 0) + assert P.A.shape == (3, 3) + assert P.B.shape == (3, 1) + assert P.C.shape == (0, 3) + assert P.D.shape == (0, 1) + + # Check that we can compute dynamics + x = np.array([1, 2, 3]) + np.testing.assert_equal(sys_nl.dynamics(0, x, 1, {}), -x + 1) + np.testing.assert_equal(P.dynamics(0, x, 1), -x + 1) + + # Make sure initial response runs OK + resp = ct.initial_response(sys_nl, np.linspace(0, 1), x) + np.testing.assert_allclose( + resp.states[:, -1], x * math.exp(-1), atol=1e-3, rtol=1e-3) + + resp = ct.initial_response(P, np.linspace(0, 1), x) + np.testing.assert_allclose(resp.states[:, -1], x * math.exp(-1)) + + # Make sure forced response runs OK + resp = ct.forced_response(sys_nl, np.linspace(0, 1), 1) + np.testing.assert_allclose( + resp.states[:, -1], 1 - math.exp(-1), atol=1e-3, rtol=1e-3) + + resp = ct.forced_response(P, np.linspace(0, 1), 1) + np.testing.assert_allclose(resp.states[:, -1], 1 - math.exp(-1)) diff --git a/control/tests/nyquist_test.py b/control/tests/nyquist_test.py index a687ee61b..0d6907b64 100644 --- a/control/tests/nyquist_test.py +++ b/control/tests/nyquist_test.py @@ -132,18 +132,17 @@ def test_nyquist_basic(): # Nyquist plot with poles on imaginary axis, omega specified # (can miss encirclements due to the imaginary poles at +/- 1j) sys = ct.tf([1], [1, 3, 2]) * ct.tf([1], [1, 0, 1]) - with pytest.warns(UserWarning, match="does not match") as records: + with warnings.catch_warnings(record=True) as records: count = ct.nyquist_response(sys, np.linspace(1e-3, 1e1, 1000)) - if len(records) == 0: - assert _Z(sys) == count + _P(sys) - - # Nyquist plot with poles on imaginary axis, omega specified, with contour - sys = ct.tf([1], [1, 3, 2]) * ct.tf([1], [1, 0, 1]) - with pytest.warns(UserWarning, match="does not match") as records: - count, contour = ct.nyquist_response( - sys, np.linspace(1e-3, 1e1, 1000), return_contour=True) - if len(records) == 0: - assert _Z(sys) == count + _P(sys) + if len(records) == 0: + # No warnings (it happens) => make sure count is correct + assert _Z(sys) == count + _P(sys) + elif len(records) == 1: + # Expected case: make sure warning is the right one + assert issubclass(records[0].category, UserWarning) + assert "encirclements does not match" in str(records[0].message) + else: + pytest.fail("multiple warnings in nyquist_response (?)") # Nyquist plot with poles on imaginary axis, return contour sys = ct.tf([1], [1, 3, 2]) * ct.tf([1], [1, 0, 1]) @@ -162,38 +161,40 @@ def test_nyquist_fbs_examples(): """Run through various examples from FBS2e to compare plots""" plt.figure() - plt.title("Figure 10.4: L(s) = 1.4 e^{-s}/(s+1)^2") sys = ct.tf([1.4], [1, 2, 1]) * ct.tf(*ct.pade(1, 4)) response = ct.nyquist_response(sys) - response.plot() + cplt = response.plot() + cplt.set_plot_title("Figure 10.4: L(s) = 1.4 e^{-s}/(s+1)^2") assert _Z(sys) == response.count + _P(sys) plt.figure() - plt.title("Figure 10.4: L(s) = 1/(s + a)^2 with a = 0.6") sys = 1/(s + 0.6)**3 response = ct.nyquist_response(sys) - response.plot() + cplt = response.plot() + cplt.set_plot_title("Figure 10.4: L(s) = 1/(s + a)^2 with a = 0.6") assert _Z(sys) == response.count + _P(sys) plt.figure() - plt.title("Figure 10.6: L(s) = 1/(s (s+1)^2) - pole at the origin") sys = 1/(s * (s+1)**2) response = ct.nyquist_response(sys) - response.plot() + cplt = response.plot() + cplt.set_plot_title( + "Figure 10.6: L(s) = 1/(s (s+1)^2) - pole at the origin") assert _Z(sys) == response.count + _P(sys) plt.figure() - plt.title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2)") sys = 3 * (s+6)**2 / (s * (s+1)**2) response = ct.nyquist_response(sys) - response.plot() + cplt = response.plot() + cplt.set_plot_title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2)") assert _Z(sys) == response.count + _P(sys) plt.figure() - plt.title("Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2) [zoom]") with pytest.warns(UserWarning, match="encirclements does not match"): response = ct.nyquist_response(sys, omega_limits=[1.5, 1e3]) - response.plot() + cplt = response.plot() + cplt.set_plot_title( + "Figure 10.10: L(s) = 3 (s+6)^2 / (s (s+1)^2) [zoom]") # Frequency limits for zoom give incorrect encirclement count # assert _Z(sys) == response.count + _P(sys) assert response.count == -1 @@ -208,12 +209,28 @@ def test_nyquist_fbs_examples(): def test_nyquist_arrows(arrows): sys = ct.tf([1.4], [1, 2, 1]) * ct.tf(*ct.pade(1, 4)) plt.figure(); - plt.title("L(s) = 1.4 e^{-s}/(s+1)^2 / arrows = %s" % arrows) response = ct.nyquist_response(sys) - response.plot(arrows=arrows) + cplt = response.plot(arrows=arrows) + cplt.set_plot_title("L(s) = 1.4 e^{-s}/(s+1)^2 / arrows = %s" % arrows) assert _Z(sys) == response.count + _P(sys) +def test_sensitivity_circles(): + A = np.array([ + [-3.56355873, -1.22980795, -1.5626527 , -0.4626829], + [-8.52361371, -3.60331459, -3.71574266, -0.43839201], + [-2.50458726, -0.72361335, -1.77795489, -0.4038419], + [-0.281183 , 0.23391825, 0.19096003, -0.9771515]]) + B = np.array([[-0.], [-1.42827213], [ 0.76806551], [-1.07987454]]) + C = np.array([[-0., 0.35557249, 0.35941791, -0.]]) + D = np.array([[0]]) + sys1 = ct.ss(A, B, C, D) + sys2 = ct.ss(A, B, C, D, dt=0.1) + plt.figure() + ct.nyquist_plot(sys1, unit_circle=True, mt_circles=[0.9,1,1.1,1.2], ms_circles=[0.9,1,1.1,1.2]) + ct.nyquist_plot(sys2, unit_circle=True, mt_circles=[0.9,1,1.1,1.2], ms_circles=[0.9,1,1.1,1.2]) + + def test_nyquist_encirclements(): # Example 14.14: effect of friction in a cart-pendulum system s = ct.tf('s') @@ -221,14 +238,14 @@ def test_nyquist_encirclements(): plt.figure(); response = ct.nyquist_response(sys) - response.plot() - plt.title("Stable system; encirclements = %d" % response.count) + cplt = response.plot() + cplt.set_plot_title("Stable system; encirclements = %d" % response.count) assert _Z(sys) == response.count + _P(sys) plt.figure(); response = ct.nyquist_response(sys * 3) - response.plot() - plt.title("Unstable system; encirclements = %d" %response.count) + cplt = response.plot() + cplt.set_plot_title("Unstable system; encirclements = %d" %response.count) assert _Z(sys * 3) == response.count + _P(sys * 3) # System with pole at the origin @@ -236,8 +253,9 @@ def test_nyquist_encirclements(): plt.figure(); response = ct.nyquist_response(sys) - response.plot() - plt.title("Pole at the origin; encirclements = %d" %response.count) + cplt = response.plot() + cplt.set_plot_title( + "Pole at the origin; encirclements = %d" %response.count) assert _Z(sys) == response.count + _P(sys) # Non-integer number of encirclements @@ -250,8 +268,9 @@ def test_nyquist_encirclements(): # strip out matrix warnings response = ct.nyquist_response( sys, omega_limits=[0.5, 1e3], encirclement_threshold=0.2) - response.plot() - plt.title("Non-integer number of encirclements [%g]" %response.count) + cplt = response.plot() + cplt.set_plot_title( + "Non-integer number of encirclements [%g]" %response.count) @pytest.fixture @@ -265,8 +284,8 @@ def indentsys(): def test_nyquist_indent_default(indentsys): plt.figure(); response = ct.nyquist_response(indentsys) - response.plot() - plt.title("Pole at origin; indent_radius=default") + cplt = response.plot() + cplt.set_plot_title("Pole at origin; indent_radius=default") assert _Z(indentsys) == response.count + _P(indentsys) @@ -276,7 +295,7 @@ def test_nyquist_indent_dont(indentsys): with pytest.warns() as record: count, contour = ct.nyquist_response( indentsys, omega=[0, 0.2, 0.3, 0.4], indent_radius=.1007, - plot=False, return_contour=True) + return_contour=True) np.testing.assert_allclose(contour[0], .1007+0.j) # second value of omega_vector is larger than indent_radius: not indented assert np.all(contour.real[2:] == 0.) @@ -292,20 +311,30 @@ def test_nyquist_indent_do(indentsys): response = ct.nyquist_response( indentsys, indent_radius=0.01, return_contour=True) count, contour = response - response.plot() - plt.title("Pole at origin; indent_radius=0.01; encirclements = %d" % count) + cplt = response.plot() + cplt.set_plot_title( + "Pole at origin; indent_radius=0.01; encirclements = %d" % count) assert _Z(indentsys) == count + _P(indentsys) # indent radius is smaller than the start of the default omega vector # check that a quarter circle around the pole at origin has been added. np.testing.assert_allclose(contour[:50].real**2 + contour[:50].imag**2, 0.01**2) + # Make sure that the command also works if called directly as _plot() + plt.figure() + with pytest.warns(FutureWarning, match=".* use nyquist_response()"): + count, contour = ct.nyquist_plot( + indentsys, indent_radius=0.01, return_contour=True) + assert _Z(indentsys) == count + _P(indentsys) + np.testing.assert_allclose( + contour[:50].real**2 + contour[:50].imag**2, 0.01**2) + def test_nyquist_indent_left(indentsys): plt.figure(); response = ct.nyquist_response(indentsys, indent_direction='left') - response.plot() - plt.title( + cplt = response.plot() + cplt.set_plot_title( "Pole at origin; indent_direction='left'; encirclements = %d" % response.count) assert _Z(indentsys) == response.count + _P(indentsys, indent='left') @@ -318,15 +347,15 @@ def test_nyquist_indent_im(): # Imaginary poles with standard indentation plt.figure(); response = ct.nyquist_response(sys) - response.plot() - plt.title("Imaginary poles; encirclements = %d" % response.count) + cplt = response.plot() + cplt.set_plot_title("Imaginary poles; encirclements = %d" % response.count) assert _Z(sys) == response.count + _P(sys) # Imaginary poles with indentation to the left plt.figure(); response = ct.nyquist_response(sys, indent_direction='left') - response.plot(label_freq=300) - plt.title( + cplt = response.plot(label_freq=300) + cplt.set_plot_title( "Imaginary poles; indent_direction='left'; encirclements = %d" % response.count) assert _Z(sys) == response.count + _P(sys, indent='left') @@ -336,8 +365,8 @@ def test_nyquist_indent_im(): with pytest.warns(UserWarning, match="encirclements does not match"): response = ct.nyquist_response( sys, np.linspace(0, 1e3, 1000), indent_direction='none') - response.plot() - plt.title( + cplt = response.plot() + cplt.set_plot_title( "Imaginary poles; indent_direction='none'; encirclements = %d" % response.count) assert _Z(sys) == response.count + _P(sys) @@ -375,17 +404,17 @@ def test_linestyle_checks(): sys = ct.tf([100], [1, 1, 1]) # Set the line styles - lines = ct.nyquist_plot( + cplt = ct.nyquist_plot( sys, primary_style=[':', ':'], mirror_style=[':', ':']) - assert all([line.get_linestyle() == ':' for line in lines[0]]) + assert all([line.get_linestyle() == ':' for line in cplt.lines[0]]) # Set the line colors - lines = ct.nyquist_plot(sys, color='g') - assert all([line.get_color() == 'g' for line in lines[0]]) + cplt = ct.nyquist_plot(sys, color='g') + assert all([line.get_color() == 'g' for line in cplt.lines[0]]) # Turn off the mirror image - lines = ct.nyquist_plot(sys, mirror_style=False) - assert lines[0][2:] == [None, None] + cplt = ct.nyquist_plot(sys, mirror_style=False) + assert cplt.lines[0][2:] == [None, None] with pytest.raises(ValueError, match="invalid 'primary_style'"): ct.nyquist_plot(sys, primary_style=False) @@ -410,9 +439,11 @@ def test_nyquist_legacy(): response = ct.nyquist_plot(sys) def test_discrete_nyquist(): + # TODO: add tests to make sure plots make sense + # Make sure we can handle discrete time systems with negative poles sys = ct.tf(1, [1, -0.1], dt=1) * ct.tf(1, [1, 0.1], dt=1) - ct.nyquist_response(sys, plot=False) + ct.nyquist_response(sys) # system with a pole at the origin sys = ct.zpk([1,], [.3, 0], 1, dt=True) @@ -429,6 +460,63 @@ def test_discrete_nyquist(): ct.nyquist_response(sys) +def test_freqresp_omega_limits(): + sys = ct.rss(4, 1, 1) + + # Generate a standard frequency response (no limits specified) + resp0 = ct.nyquist_response(sys) + assert resp0.contour.size > 2 + + # Regenerate the response using omega_limits + resp1 = ct.nyquist_response( + sys, omega_limits=[resp0.contour[1].imag, resp0.contour[-1].imag]) + assert resp1.contour.size > 2 + assert np.isclose(resp1.contour[0], resp0.contour[1]) + assert np.isclose(resp1.contour[-1], resp0.contour[-1]) + + # Regenerate the response using omega as a list of two elements + resp2 = ct.nyquist_response( + sys, [resp0.contour[1].imag, resp0.contour[-1].imag]) + np.testing.assert_equal(resp1.contour, resp2.contour) + + # Make sure that generating response using array does the right thing + resp3 = ct.nyquist_response( + sys, np.array([resp0.contour[1].imag, resp0.contour[-1].imag])) + np.testing.assert_equal( + resp3.contour, + np.array([resp0.contour[1], resp0.contour[-1]])) + + +def test_nyquist_frd(): + sys = ct.rss(4, 1, 1) + sys1 = ct.frd(sys, np.logspace(-1, 1, 10), name='sys1') + sys2 = ct.frd(sys, np.logspace(-2, 2, 10), name='sys2') + sys3 = ct.frd(sys, np.logspace(-2, 2, 10), smooth=True, name='sys3') + + # Turn off warnings about number of encirclements + warnings.filterwarnings( + 'ignore', message="number of encirclements was a non-integer value", + category=UserWarning) + + # OK to specify frequency with FRD sys if frequencies match + nyqresp = ct.nyquist_response(sys1, np.logspace(-1, 1, 10)) + np.testing.assert_allclose(nyqresp.contour, np.logspace(-1, 1, 10) * 1j) + + # If a fixed FRD omega is used, generate an error on mismatch + with pytest.raises(ValueError, match="not all frequencies .* in .* list"): + nyqresp = ct.nyquist_response(sys2, np.logspace(-1, 1, 10)) + + # OK to specify frequency with FRD sys if interpolating FRD is used + nyqresp = ct.nyquist_response(sys3, np.logspace(-1, 1, 12)) + np.testing.assert_allclose(nyqresp.contour, np.logspace(-1, 1, 12) * 1j) + + # Computing Nyquist response w/ different frequencies OK if given as a list + nyqresp = ct.nyquist_response([sys1, sys2]) + cplt = nyqresp.plot() + + warnings.resetwarnings() + + if __name__ == "__main__": # # Interactive mode: generate plots for manual viewing @@ -452,6 +540,9 @@ def test_discrete_nyquist(): test_nyquist_arrows(3) test_nyquist_arrows([0.1, 0.5, 0.9]) + print("Test sensitivity circles") + test_sensitivity_circles() + print("Stability checks") test_nyquist_encirclements() @@ -472,19 +563,26 @@ def test_discrete_nyquist(): print("Unusual Nyquist plot") sys = ct.tf([1], [1, 3, 2]) * ct.tf([1], [1, 0, 1]) plt.figure() - plt.title("Poles: %s" % - np.array2string(sys.poles(), precision=2, separator=',')) response = ct.nyquist_response(sys) - response.plot() + cplt = response.plot() + cplt.set_plot_title("Poles: %s" % + np.array2string(sys.poles(), precision=2, separator=',')) assert _Z(sys) == response.count + _P(sys) print("Discrete time systems") sys = ct.c2d(sys, 0.01) plt.figure() - plt.title("Discrete-time; poles: %s" % - np.array2string(sys.poles(), precision=2, separator=',')) response = ct.nyquist_response(sys) - response.plot() - - + cplt = response.plot() + cplt.set_plot_title("Discrete-time; poles: %s" % + np.array2string(sys.poles(), precision=2, separator=',')) + print("Frequency response data (FRD) systems") + sys = ct.tf( + (0.02 * s**3 - 0.1 * s) / (s**4 + s**3 + s**2 + 0.25 * s + 0.04), + name='tf') + sys1 = ct.frd(sys, np.logspace(-1, 1, 15), name='frd1') + sys2 = ct.frd(sys, np.logspace(-2, 2, 20), name='frd2') + plt.figure() + cplt = ct.nyquist_plot([sys, sys1, sys2]) + cplt.set_plot_title("Mixed FRD, tf data") diff --git a/control/tests/phaseplot_test.py b/control/tests/phaseplot_test.py index a01ab2aea..5e7a31651 100644 --- a/control/tests/phaseplot_test.py +++ b/control/tests/phaseplot_test.py @@ -9,14 +9,17 @@ the figures so that you can check them visually. """ +import warnings +from math import pi import matplotlib.pyplot as plt import numpy as np -from numpy import pi import pytest -from control import phase_plot + import control as ct import control.phaseplot as pp +from control import phase_plot +from control.tests.conftest import mplcleanup # Legacy tests @@ -114,6 +117,7 @@ def oscillator_ode(self, x, t, m=1., b=1, k=1, extra=None): [ct.phaseplot.separatrices, [5], {'params': {}, 'gridspec': [5, 5]}], [ct.phaseplot.separatrices, [5], {'color': ('r', 'g')}], ]) +@pytest.mark.usefixtures('mplcleanup') def test_helper_functions(func, args, kwargs): # Test with system sys = ct.nlsys( @@ -126,6 +130,7 @@ def test_helper_functions(func, args, kwargs): out = func(rhsfcn, [-1, 1, -1, 1], *args, **kwargs) +@pytest.mark.usefixtures('mplcleanup') def test_system_types(): # Sample dynamical systems - inverted pendulum def invpend_ode(t, x, m=0, l=0, b=0, g=0): @@ -133,13 +138,14 @@ def invpend_ode(t, x, m=0, l=0, b=0, g=0): # Use callable form, with parameters (if not correct, will get /0 error) ct.phase_plane_plot( - invpend_ode, [-5, 5, 2, 2], params={'args': (1, 1, 0.2, 1)}) + invpend_ode, [-5, 5, -2, 2], params={'args': (1, 1, 0.2, 1)}) # Linear I/O system ct.phase_plane_plot( ct.ss([[0, 1], [-1, -1]], [[0], [1]], [[1, 0]], 0)) +@pytest.mark.usefixtures('mplcleanup') def test_phaseplane_errors(): with pytest.raises(ValueError, match="invalid grid specification"): ct.phase_plane_plot(ct.rss(2, 1, 1), gridspec='bad') @@ -156,9 +162,25 @@ def invpend_ode(t, x, m=0, l=0, b=0, g=0): ct.phase_plane_plot( invpend_ode, [-5, 5, 2, 2], params={'stuff': (1, 1, 0.2, 1)}) - + # Warning messages for invalid solutions: nonlinear spring mass system + sys = ct.nlsys( + lambda t, x, u, params: np.array( + [x[1], -0.25 * (x[0] - 0.01 * x[0]**3) - 0.1 * x[1]]), + states=2, inputs=0) + with pytest.warns(UserWarning, match=r"X0=array\(.*\), solve_ivp failed"): + ct.phase_plane_plot( + sys, [-12, 12, -10, 10], 15, gridspec=[2, 9], + plot_separatrices=False) + + # Turn warnings off + with warnings.catch_warnings(): + warnings.simplefilter("error") + ct.phase_plane_plot( + sys, [-12, 12, -10, 10], 15, gridspec=[2, 9], + plot_separatrices=False, suppress_warnings=True) +@pytest.mark.usefixtures('mplcleanup') def test_basic_phase_plots(savefigs=False): sys = ct.nlsys( lambda t, x, u, params: np.array([[0, 1], [-1, -1]]) @ x, diff --git a/control/tests/pzmap_test.py b/control/tests/pzmap_test.py index ce8adf6e7..438732b84 100644 --- a/control/tests/pzmap_test.py +++ b/control/tests/pzmap_test.py @@ -16,7 +16,7 @@ from control import TransferFunction, config, pzmap -@pytest.mark.filterwarnings("ignore:.*return values.*:DeprecationWarning") +@pytest.mark.filterwarnings("ignore:.*return value.*:FutureWarning") @pytest.mark.parametrize("kwargs", [pytest.param(dict(), id="default"), pytest.param(dict(plot=False), id="plot=False"), @@ -53,7 +53,8 @@ def test_pzmap(kwargs, setdefaults, dt, editsdefaults, mplcleanup): if kwargs.get('plot', None) is None: pzkwargs['plot'] = True # use to get legacy return values - P, Z = pzmap(T, **pzkwargs) + with pytest.warns(FutureWarning, match="return value .* is deprecated"): + P, Z = pzmap(T, **pzkwargs) np.testing.assert_allclose(P, Pref, rtol=1e-3) np.testing.assert_allclose(Z, Zref, rtol=1e-3) @@ -96,7 +97,7 @@ def test_polezerodata(): # Legacy return format for plot in [True, False]: - with pytest.warns(DeprecationWarning, match=".* values .* deprecated"): + with pytest.warns(FutureWarning, match=".* value .* deprecated"): poles, zeros = ct.pole_zero_plot(pzdata, plot=False) np.testing.assert_equal(poles, sys.poles()) np.testing.assert_equal(zeros, sys.zeros()) @@ -119,7 +120,7 @@ def test_pzmap_raises(): def test_pzmap_limits(): sys = ct.tf([1, 2], [1, 2, 3]) - out = ct.pole_zero_plot(sys, xlim=[-1, 1], ylim=[-1, 1]) - ax = ct.get_plot_axes(out)[0, 0] + cplt = ct.pole_zero_plot(sys, xlim=[-1, 1], ylim=[-1, 1]) + ax = cplt.axes[0, 0] assert ax.get_xlim() == (-1, 1) assert ax.get_ylim() == (-1, 1) diff --git a/control/tests/rlocus_test.py b/control/tests/rlocus_test.py index 5511f5b82..a62bc742b 100644 --- a/control/tests/rlocus_test.py +++ b/control/tests/rlocus_test.py @@ -45,7 +45,7 @@ def check_cl_poles(self, sys, pole_list, k_list): poles = np.sort(poles) np.testing.assert_array_almost_equal(poles, poles_expected) - @pytest.mark.filterwarnings("ignore:.*return values.*:DeprecationWarning") + @pytest.mark.filterwarnings("ignore:.*return value.*:FutureWarning") def testRootLocus(self, sys): """Basic root locus (no plot)""" klist = [-1, 0, 1] @@ -61,7 +61,7 @@ def testRootLocus(self, sys): np.testing.assert_allclose(klist, k_out) self.check_cl_poles(sys, roots, klist) - @pytest.mark.filterwarnings("ignore:.*return values.*:DeprecationWarning") + @pytest.mark.filterwarnings("ignore:.*return value.*:FutureWarning") def test_without_gains(self, sys): roots, kvect = root_locus(sys, plot=False) self.check_cl_poles(sys, roots, kvect) @@ -95,7 +95,7 @@ def test_root_locus_plot_grid(self, sys, grid, method): if grid == 'empty': assert n_gridlines == 0 assert not isinstance(ax, AA.Axes) - elif grid is False or method == 'pzmap' and grid is None: + elif grid is False: assert n_gridlines == 2 if sys.isctime() else 3 assert not isinstance(ax, AA.Axes) elif sys.isdtime(strict=True): @@ -109,7 +109,7 @@ def test_root_locus_plot_grid(self, sys, grid, method): # TODO: check validity of grid - @pytest.mark.filterwarnings("ignore:.*return values.*:DeprecationWarning") + @pytest.mark.filterwarnings("ignore:.*return value.*:FutureWarning") def test_root_locus_neg_false_gain_nonproper(self): """ Non proper TranferFunction with negative gain: Not implemented""" with pytest.raises(ValueError, match="with equal order"): @@ -147,7 +147,7 @@ def test_root_locus_zoom(self): assert_array_almost_equal(zoom_x, zoom_x_valid) assert_array_almost_equal(zoom_y, zoom_y_valid) - @pytest.mark.filterwarnings("ignore:.*return values.*:DeprecationWarning") + @pytest.mark.filterwarnings("ignore:.*return value.*:FutureWarning") @pytest.mark.timeout(2) def test_rlocus_default_wn(self): """Check that default wn calculation works properly""" @@ -174,13 +174,24 @@ def test_rlocus_default_wn(self): "sys, grid, xlim, ylim, interactive", [ (ct.tf([1], [1, 2, 1]), None, None, None, False), ]) +@pytest.mark.usefixtures("mplcleanup") def test_root_locus_plots(sys, grid, xlim, ylim, interactive): ct.root_locus_map(sys).plot( grid=grid, xlim=xlim, ylim=ylim, interactive=interactive) # TODO: add tests to make sure everything "looks" OK +# Test deprecated keywords +@pytest.mark.parametrize("keyword", ["kvect", "k"]) +@pytest.mark.usefixtures("mplcleanup") +def test_root_locus_legacy(keyword): + sys = ct.rss(2, 1, 1) + with pytest.warns(FutureWarning, match=f"'{keyword}' is deprecated"): + ct.root_locus_plot(sys, **{keyword: [0, 1, 2]}) + + # Generate plots used in documentation +@pytest.mark.usefixtures("mplcleanup") def test_root_locus_documentation(savefigs=False): plt.figure() sys = ct.tf([1, 2], [1, 2, 3], name='SISO transfer function') @@ -196,8 +207,8 @@ def test_root_locus_documentation(savefigs=False): # TODO: generate event in order to generate real title plt.figure() - out = ct.root_locus_map(sys).plot(initial_gain=3.506) - ax = ct.get_plot_axes(out)[0, 0] + cplt = ct.root_locus_map(sys).plot(initial_gain=3.506) + ax = cplt.axes[0, 0] freqplot_rcParams = ct.config._get_param('freqplot', 'rcParams') with plt.rc_context(freqplot_rcParams): ax.set_title( @@ -270,6 +281,9 @@ def test_root_locus_documentation(savefigs=False): plt.figure() test_root_locus_plots( sys, grid=grid, xlim=xlim, ylim=ylim, interactive=interactive) + ct.suptitle( + f"sys={sys.name}, {grid=}, {xlim=}, {ylim=}, {interactive=}", + frame='figure') # Run tests that generate plots for the documentation test_root_locus_documentation(savefigs=True) diff --git a/control/tests/robust_test.py b/control/tests/robust_test.py index 146ae9e41..dde2423be 100644 --- a/control/tests/robust_test.py +++ b/control/tests/robust_test.py @@ -48,6 +48,7 @@ def testH2syn(self): np.testing.assert_array_almost_equal(k.D, [[0]]) +@pytest.mark.filterwarnings("ignore:connect:FutureWarning") class TestAugw: # tolerance for system equality @@ -324,6 +325,7 @@ def testErrors(self): augw(g1by1, w3=g2by2) +@pytest.mark.filterwarnings("ignore:connect:FutureWarning") class TestMixsyn: """Test control.robust.mixsyn""" diff --git a/control/tests/sisotool_test.py b/control/tests/sisotool_test.py index 325b9c180..1fc744daa 100644 --- a/control/tests/sisotool_test.py +++ b/control/tests/sisotool_test.py @@ -153,6 +153,7 @@ def test_sisotool_initial_gain(self, tsys): with pytest.warns(FutureWarning): sisotool(tsys, kvect=1.2) + @pytest.mark.filterwarnings("ignore:connect:FutureWarning") def test_sisotool_mimo(self, sys222, sys221): # a 2x2 should not raise an error: sisotool(sys222) @@ -196,6 +197,7 @@ def test_pid_designer_1(self, plant, gain, sign, input_signal, Kp0, Ki0, Kd0, de {'input_signal':'r', 'Kp0':0.01, 'derivative_in_feedback_path':True}, {'input_signal':'d', 'Kp0':0.01, 'derivative_in_feedback_path':True}, {'input_signal':'r', 'Kd0':0.01, 'derivative_in_feedback_path':True}]) + @pytest.mark.filterwarnings("ignore:connect:FutureWarning") def test_pid_designer_2(self, plant, kwargs): rootlocus_pid_designer(plant, **kwargs) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 4a0472de7..2d96ad225 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -892,7 +892,7 @@ 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.warns(DeprecationWarning, match="'type' is deprecated"): + with pytest.warns(FutureWarning, match="'type' is deprecated"): ctrl, clsys = ct.create_statefbk_iosystem(sys, K, type='nonlinear') with pytest.raises(ControlArgument, match="duplicate keywords"): diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 59f441456..2829d6988 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -392,7 +392,7 @@ def test_freq_resp(self): np.testing.assert_almost_equal(omega, true_omega) # Deprecated version of the call (should return warning) - with pytest.warns(DeprecationWarning, match="will be removed"): + with pytest.warns(FutureWarning, match="will be removed"): mag, phase, omega = sys.freqresp(true_omega) np.testing.assert_almost_equal(mag, true_mag) @@ -463,28 +463,53 @@ def test_append_tf(self): np.testing.assert_array_almost_equal(sys3c.A[:3, 3:], np.zeros((3, 2))) np.testing.assert_array_almost_equal(sys3c.A[3:, :3], np.zeros((2, 3))) - def test_array_access_ss(self): - + def test_array_access_ss_failure(self): sys1 = StateSpace( [[1., 2.], [3., 4.]], [[5., 6.], [6., 8.]], [[9., 10.], [11., 12.]], [[13., 14.], [15., 16.]], 1, inputs=['u0', 'u1'], outputs=['y0', 'y1']) + with pytest.raises(IOError): + sys1[0] + + @pytest.mark.parametrize("outdx, inpdx", + [(0, 1), + (slice(0, 1, 1), 1), + (0, slice(1, 2, 1)), + (slice(0, 1, 1), slice(1, 2, 1)), + (slice(None, None, -1), 1), + (0, slice(None, None, -1)), + (slice(None, 2, None), 1), + (slice(None, None, 1), slice(None, None, 2)), + (0, slice(1, 2, 1)), + (slice(0, 1, 1), slice(1, 2, 1))]) + def test_array_access_ss(self, outdx, inpdx): + sys1 = StateSpace( + [[1., 2.], [3., 4.]], + [[5., 6.], [7., 8.]], + [[9., 10.], [11., 12.]], + [[13., 14.], [15., 16.]], 1, + inputs=['u0', 'u1'], outputs=['y0', 'y1']) - sys1_01 = sys1[0, 1] + sys1_01 = sys1[outdx, inpdx] + + # Convert int to slice to ensure that numpy doesn't drop the dimension + if isinstance(outdx, int): outdx = slice(outdx, outdx+1, 1) + if isinstance(inpdx, int): inpdx = slice(inpdx, inpdx+1, 1) + np.testing.assert_array_almost_equal(sys1_01.A, sys1.A) np.testing.assert_array_almost_equal(sys1_01.B, - sys1.B[:, 1:2]) + sys1.B[:, inpdx]) np.testing.assert_array_almost_equal(sys1_01.C, - sys1.C[0:1, :]) + sys1.C[outdx, :]) np.testing.assert_array_almost_equal(sys1_01.D, - sys1.D[0, 1]) + sys1.D[outdx, inpdx]) assert sys1.dt == sys1_01.dt - assert sys1_01.input_labels == ['u1'] - assert sys1_01.output_labels == ['y0'] + assert sys1_01.input_labels == sys1.input_labels[inpdx] + assert sys1_01.output_labels == sys1.output_labels[outdx] assert sys1_01.name == sys1.name + "$indexed" def test_dc_gain_cont(self): diff --git a/control/tests/timebase_test.py b/control/tests/timebase_test.py index a391d2fe7..79b1492d7 100644 --- a/control/tests/timebase_test.py +++ b/control/tests/timebase_test.py @@ -3,18 +3,36 @@ import numpy as np import control as ct +# Utility function to convert state space system to nlsys +def ss2io(sys): + return ct.nlsys( + sys.updfcn, sys.outfcn, states=sys.nstates, + inputs=sys.ninputs, outputs=sys.noutputs, dt=sys.dt) + @pytest.mark.parametrize( "dt1, dt2, dt3", [ (0, 0, 0), (0, 0.1, ValueError), (0, None, 0), + (0, 'float', 0), + (0, 'array', 0), + (None, 'array', None), + (None, 'array', None), (0, True, ValueError), (0.1, 0, ValueError), (0.1, 0.1, 0.1), (0.1, None, 0.1), (0.1, True, 0.1), + (0.1, 'array', 0.1), + (0.1, 'float', 0.1), (None, 0, 0), + ('float', 0, 0), + ('array', 0, 0), + ('float', None, None), + ('array', None, None), (None, 0.1, 0.1), + ('array', 0.1, 0.1), + ('float', 0.1, 0.1), (None, None, None), (None, True, True), (True, 0, ValueError), @@ -25,16 +43,28 @@ (0.2, 0.1, ValueError), ]) @pytest.mark.parametrize("op", [ct.series, ct.parallel, ct.feedback]) -@pytest.mark.parametrize("type", [ct.StateSpace, ct.ss, ct.tf]) +@pytest.mark.parametrize("type", [ct.StateSpace, ct.ss, ct.tf, ss2io]) def test_composition(dt1, dt2, dt3, op, type): - # Define the system A, B, C, D = [[1, 1], [0, 1]], [[0], [1]], [[1, 0]], 0 - sys1 = ct.StateSpace(A, B, C, D, dt1) - sys2 = ct.StateSpace(A, B, C, D, dt2) + Karray = np.array([[1]]) + kfloat = 1 - # Convert to the desired form - sys1 = type(sys1) - sys2 = type(sys2) + # Define the system + if isinstance(dt1, (int, float)) or dt1 is None: + sys1 = ct.StateSpace(A, B, C, D, dt1) + sys1 = type(sys1) + elif dt1 == 'array': + sys1 = Karray + elif dt1 == 'float': + sys1 = kfloat + + if isinstance(dt2, (int, float)) or dt2 is None: + sys2 = ct.StateSpace(A, B, C, D, dt2) + sys2 = type(sys2) + elif dt2 == 'array': + sys2 = Karray + elif dt2 == 'float': + sys2 = kfloat if inspect.isclass(dt3) and issubclass(dt3, Exception): with pytest.raises(dt3, match="incompatible timebases"): diff --git a/control/tests/timeplot_test.py b/control/tests/timeplot_test.py index 7cdde5c54..9525c7e02 100644 --- a/control/tests/timeplot_test.py +++ b/control/tests/timeplot_test.py @@ -1,13 +1,14 @@ # timeplot_test.py - test out time response plots # RMM, 23 Jun 2023 -import pytest -import control as ct import matplotlib as mpl import matplotlib.pyplot as plt import numpy as np +import pytest + +import control as ct +from control.tests.conftest import mplcleanup, slycotonly -from control.tests.conftest import slycotonly # Detailed test of (almost) all functionality # @@ -67,6 +68,7 @@ True, True, False, False, False, False), ]) +@pytest.mark.usefixtures('mplcleanup') def test_response_plots( fcn, sys, pltinp, pltout, cmbsig, cmbtrc, trpose, secsys, clear=True): @@ -122,22 +124,22 @@ def test_response_plots( pltinp is False or response.ninputs == 0 or pltinp is None and response.plot_inputs is False): with pytest.raises(ValueError, match=".* no data to plot"): - out = response.plot(**kwargs) + cplt = response.plot(**kwargs) return None elif not pltout and pltinp == 'overlay': with pytest.raises(ValueError, match="can't overlay inputs"): - out = response.plot(**kwargs) + cplt = response.plot(**kwargs) return None elif pltinp in [True, 'overlay'] and response.ninputs == 0: with pytest.raises(ValueError, match=".* but no inputs"): - out = response.plot(**kwargs) + cplt = response.plot(**kwargs) return None - out = response.plot(**kwargs) + cplt = response.plot(**kwargs) # Make sure all of the outputs are of the right type nlines_plotted = 0 - for ax_lines in np.nditer(out, flags=["refs_ok"]): + for ax_lines in np.nditer(cplt.lines, flags=["refs_ok"]): for line in ax_lines.item(): assert isinstance(line, mpl.lines.Line2D) nlines_plotted += 1 @@ -178,7 +180,7 @@ def test_response_plots( assert len(ax.get_lines()) > 1 # Update the title so we can see what is going on - fig = out[0, 0][0].axes.figure + fig = cplt.figure fig.suptitle( fig._suptitle._text + f" [{sys.noutputs}x{sys.ninputs}, cs={cmbsig}, " @@ -190,47 +192,46 @@ def test_response_plots( plt.clf() +@pytest.mark.usefixtures('mplcleanup') def test_axes_setup(): - get_plot_axes = ct.timeplot.get_plot_axes - sys_2x3 = ct.rss(4, 2, 3) sys_2x3b = ct.rss(4, 2, 3) sys_3x2 = ct.rss(4, 3, 2) sys_3x1 = ct.rss(4, 3, 1) # Two plots of the same size leaves axes unchanged - out1 = ct.step_response(sys_2x3).plot() - out2 = ct.step_response(sys_2x3b).plot() - np.testing.assert_equal(get_plot_axes(out1), get_plot_axes(out2)) + cplt1 = ct.step_response(sys_2x3).plot() + cplt2 = ct.step_response(sys_2x3b).plot() + np.testing.assert_equal(cplt1.axes, cplt2.axes) plt.close() # Two plots of same net size leaves axes unchanged (unfortunately) - out1 = ct.step_response(sys_2x3).plot() - out2 = ct.step_response(sys_3x2).plot() + cplt1 = ct.step_response(sys_2x3).plot() + cplt2 = ct.step_response(sys_3x2).plot() np.testing.assert_equal( - get_plot_axes(out1).reshape(-1), get_plot_axes(out2).reshape(-1)) + cplt1.axes.reshape(-1), cplt2.axes.reshape(-1)) plt.close() # Plots of different shapes generate new plots - out1 = ct.step_response(sys_2x3).plot() - out2 = ct.step_response(sys_3x1).plot() - ax1_list = get_plot_axes(out1).reshape(-1).tolist() - ax2_list = get_plot_axes(out2).reshape(-1).tolist() + cplt1 = ct.step_response(sys_2x3).plot() + cplt2 = ct.step_response(sys_3x1).plot() + ax1_list = cplt1.axes.reshape(-1).tolist() + ax2_list = cplt2.axes.reshape(-1).tolist() for ax in ax1_list: assert ax not in ax2_list plt.close() # Passing a list of axes preserves those axes - out1 = ct.step_response(sys_2x3).plot() - out2 = ct.step_response(sys_3x1).plot() - out3 = ct.step_response(sys_2x3b).plot(ax=get_plot_axes(out1)) - np.testing.assert_equal(get_plot_axes(out1), get_plot_axes(out3)) + cplt1 = ct.step_response(sys_2x3).plot() + cplt2 = ct.step_response(sys_3x1).plot() + cplt3 = ct.step_response(sys_2x3b).plot(ax=cplt1.axes) + np.testing.assert_equal(cplt1.axes, cplt3.axes) plt.close() # Sending an axes array of the wrong size raises exception with pytest.raises(ValueError, match="not the right shape"): - out = ct.step_response(sys_2x3).plot() - ct.step_response(sys_3x1).plot(ax=get_plot_axes(out)) + cplt = ct.step_response(sys_2x3).plot() + ct.step_response(sys_3x1).plot(ax=cplt.axes) sys_2x3 = ct.rss(4, 2, 3) sys_2x3b = ct.rss(4, 2, 3) sys_3x2 = ct.rss(4, 3, 2) @@ -238,6 +239,7 @@ def test_axes_setup(): @slycotonly +@pytest.mark.usefixtures('mplcleanup') def test_legend_map(): sys_mimo = ct.tf2ss( [[[1], [0.1]], [[0.2], [1]]], @@ -250,11 +252,12 @@ def test_legend_map(): title='MIMO step response with custom legend placement') +@pytest.mark.usefixtures('mplcleanup') def test_combine_time_responses(): sys_mimo = ct.rss(4, 2, 2) timepts = np.linspace(0, 10, 100) - # Combine two response with ntrace = 0 + # Combine two responses with ntrace = 0 U = np.vstack([np.sin(timepts), np.cos(2*timepts)]) resp1 = ct.input_output_response(sys_mimo, timepts, U) @@ -289,6 +292,7 @@ def test_combine_time_responses(): combresp4 = ct.combine_time_responses( [resp1, resp2, resp3], trace_labels=labels) assert combresp4.trace_labels == labels + assert combresp4.trace_types == [None, None, 'step', 'step'] # Automatically generated trace label names and types resp5 = ct.step_response(sys_mimo, timepts) @@ -298,7 +302,13 @@ def test_combine_time_responses(): combresp5 = ct.combine_time_responses([resp1, resp5]) assert combresp5.trace_labels == [resp1.title] + \ ["test, trace 0", "test, trace 1"] - assert combresp4.trace_types == [None, None, 'step', 'step'] + assert combresp5.trace_types == [None, None, None] + + # ntraces = 0 with trace_types != None + # https://github.com/python-control/python-control/issues/1025 + resp6 = ct.forced_response(sys_mimo, timepts, U) + combresp6 = ct.combine_time_responses([resp1, resp6]) + assert combresp6.trace_types == [None, 'forced'] with pytest.raises(ValueError, match="must have the same number"): resp = ct.step_response(ct.rss(4, 2, 3), timepts) @@ -313,26 +323,76 @@ def test_combine_time_responses(): combresp6 = ct.combine_time_responses([resp1, resp]) +@pytest.mark.parametrize("resp_fcn", [ + ct.step_response, ct.initial_response, ct.impulse_response, + ct.forced_response, ct.input_output_response]) +@pytest.mark.usefixtures('mplcleanup') +def test_list_responses(resp_fcn): + sys1 = ct.rss(2, 2, 2, strictly_proper=True) + sys2 = ct.rss(2, 2, 2, strictly_proper=True) + + # Figure out the expected shape of the system + match resp_fcn: + case ct.step_response | ct.impulse_response: + shape = (2, 2) + kwargs = {} + case ct.initial_response: + shape = (2, 1) + kwargs = {} + case ct.forced_response | ct.input_output_response: + shape = (4, 1) # outputs and inputs both plotted + T = np.linspace(0, 10) + U = [np.sin(T), np.cos(T)] + kwargs = {'T': T, 'U': U} + + resp1 = resp_fcn(sys1, **kwargs) + resp2 = resp_fcn(sys2, **kwargs) + + # Sequential plotting results in colors rotating + plt.figure() + cplt1 = resp1.plot() + cplt2 = resp2.plot() + assert cplt1.shape == shape # legacy access (OK here) + assert cplt2.shape == shape # legacy access (OK here) + for row in range(2): # just look at the outputs + for col in range(shape[1]): + assert cplt1.lines[row, col][0].get_color() == 'tab:blue' + assert cplt2.lines[row, col][0].get_color() == 'tab:orange' + + plt.figure() + resp_combined = resp_fcn([sys1, sys2], **kwargs) + assert isinstance(resp_combined, ct.timeresp.TimeResponseList) + assert resp_combined[0].time[-1] == max(resp1.time[-1], resp2.time[-1]) + assert resp_combined[1].time[-1] == max(resp1.time[-1], resp2.time[-1]) + cplt = resp_combined.plot() + assert cplt.lines.shape == shape + for row in range(2): # just look at the outputs + for col in range(shape[1]): + assert cplt.lines[row, col][0].get_color() == 'tab:blue' + assert cplt.lines[row, col][1].get_color() == 'tab:orange' + + @slycotonly +@pytest.mark.usefixtures('mplcleanup') def test_linestyles(): # Check to make sure we can change line styles sys_mimo = ct.tf2ss( [[[1], [0.1]], [[0.2], [1]]], [[[1, 0.6, 1], [1, 1, 1]], [[1, 0.4, 1], [1, 2, 1]]], name="MIMO") - out = ct.step_response(sys_mimo).plot('k--', plot_inputs=True) - for ax in np.nditer(out, flags=["refs_ok"]): + cplt = ct.step_response(sys_mimo).plot('k--', plot_inputs=True) + for ax in np.nditer(cplt.lines, flags=["refs_ok"]): for line in ax.item(): assert line.get_color() == 'k' assert line.get_linestyle() == '--' - out = ct.step_response(sys_mimo).plot( + cplt = ct.step_response(sys_mimo).plot( plot_inputs='overlay', overlay_signals=True, overlay_traces=True, output_props=[{'color': c} for c in ['blue', 'orange']], input_props=[{'color': c} for c in ['red', 'green']], trace_props=[{'linestyle': s} for s in ['-', '--']]) - assert out.shape == (1, 1) - lines = out[0, 0] + assert cplt.lines.shape == (1, 1) + lines = cplt.lines[0, 0] assert lines[0].get_color() == 'blue' and lines[0].get_linestyle() == '-' assert lines[1].get_color() == 'orange' and lines[1].get_linestyle() == '-' assert lines[2].get_color() == 'red' and lines[2].get_linestyle() == '-' @@ -343,32 +403,114 @@ def test_linestyles(): assert lines[7].get_color() == 'green' and lines[7].get_linestyle() == '--' -def test_rcParams(): - sys = ct.rss(2, 2, 2) - - # Create new set of rcParams - my_rcParams = { - 'axes.labelsize': 10, - 'axes.titlesize': 10, - 'figure.titlesize': 12, - 'legend.fontsize': 10, - 'xtick.labelsize': 10, - 'ytick.labelsize': 10, - } - - # Generate a figure with the new rcParams - out = ct.step_response(sys).plot(rcParams=my_rcParams) - ax = out[0, 0][0].axes - fig = ax.figure - - # Check to make sure new settings were used - assert ax.xaxis.get_label().get_fontsize() == 10 - assert ax.yaxis.get_label().get_fontsize() == 10 - assert ax.title.get_fontsize() == 10 - assert ax.xaxis._get_tick_label_size('x') == 10 - assert ax.yaxis._get_tick_label_size('y') == 10 - assert fig._suptitle.get_fontsize() == 12 +@pytest.mark.parametrize("resp_fcn", [ + ct.step_response, ct.initial_response, ct.impulse_response, + ct.forced_response, ct.input_output_response]) +@pytest.mark.usefixtures('editsdefaults', 'mplcleanup') +def test_timeplot_trace_labels(resp_fcn): + plt.close('all') + sys1 = ct.rss(2, 2, 2, strictly_proper=True, name='sys1') + sys2 = ct.rss(2, 2, 2, strictly_proper=True, name='sys2') + + # Figure out the expected shape of the system + match resp_fcn: + case ct.step_response | ct.impulse_response: + shape = (2, 2) + kwargs = {} + case ct.initial_response: + shape = (2, 1) + kwargs = {} + case ct.forced_response | ct.input_output_response: + shape = (4, 1) # outputs and inputs both plotted + T = np.linspace(0, 10) + U = [np.sin(T), np.cos(T)] + kwargs = {'T': T, 'U': U} + + # Use figure frame for suptitle to speed things up + ct.set_defaults('freqplot', title_frame='figure') + + # Make sure default labels are as expected + cplt = resp_fcn([sys1, sys2], **kwargs).plot() + axs = cplt.axes + if axs.ndim == 1: + legend = axs[0].get_legend().get_texts() + else: + legend = axs[0, -1].get_legend().get_texts() + assert legend[0].get_text() == 'sys1' + assert legend[1].get_text() == 'sys2' + plt.close() + + # Override labels all at once + cplt = resp_fcn([sys1, sys2], **kwargs).plot(label=['line1', 'line2']) + axs = cplt.axes + if axs.ndim == 1: + legend = axs[0].get_legend().get_texts() + else: + legend = axs[0, -1].get_legend().get_texts() + assert legend[0].get_text() == 'line1' + assert legend[1].get_text() == 'line2' + plt.close() + + # Override labels one at a time + cplt = resp_fcn(sys1, **kwargs).plot(label='line1') + cplt = resp_fcn(sys2, **kwargs).plot(label='line2') + axs = cplt.axes + if axs.ndim == 1: + legend = axs[0].get_legend().get_texts() + else: + legend = axs[0, -1].get_legend().get_texts() + assert legend[0].get_text() == 'line1' + assert legend[1].get_text() == 'line2' + plt.close() + +@pytest.mark.usefixtures('mplcleanup') +def test_full_label_override(): + sys1 = ct.rss(2, 2, 2, strictly_proper=True, name='sys1') + sys2 = ct.rss(2, 2, 2, strictly_proper=True, name='sys2') + + labels_2d = np.array([ + ["outsys1u1y1", "outsys1u1y2", "outsys1u2y1", "outsys1u2y2", + "outsys2u1y1", "outsys2u1y2", "outsys2u2y1", "outsys2u2y2"], + ["inpsys1u1y1", "inpsys1u1y2", "inpsys1u2y1", "inpsys1u2y2", + "inpsys2u1y1", "inpsys2u1y2", "inpsys2u2y1", "inpsys2u2y2"]]) + + + labels_4d = np.empty((2, 2, 2, 2), dtype=object) + for i, sys in enumerate(['sys1', 'sys2']): + for j, trace in enumerate(['u1', 'u2']): + for k, out in enumerate(['y1', 'y2']): + labels_4d[i, j, k, 0] = "out" + sys + trace + out + labels_4d[i, j, k, 1] = "inp" + sys + trace + out + + # Test 4D labels + cplt = ct.step_response([sys1, sys2]).plot( + overlay_signals=True, overlay_traces=True, plot_inputs=True, + label=labels_4d) + axs = cplt.axes + assert axs.shape == (2, 1) + legend_text = axs[0, 0].get_legend().get_texts() + for i, label in enumerate(labels_2d[0]): + assert legend_text[i].get_text() == label + legend_text = axs[1, 0].get_legend().get_texts() + for i, label in enumerate(labels_2d[1]): + assert legend_text[i].get_text() == label + + # Test 2D labels + cplt = ct.step_response([sys1, sys2]).plot( + overlay_signals=True, overlay_traces=True, plot_inputs=True, + label=labels_2d) + axs = cplt.axes + assert axs.shape == (2, 1) + legend_text = axs[0, 0].get_legend().get_texts() + for i, label in enumerate(labels_2d[0]): + assert legend_text[i].get_text() == label + legend_text = axs[1, 0].get_legend().get_texts() + for i, label in enumerate(labels_2d[1]): + assert legend_text[i].get_text() == label + + +@pytest.mark.usefixtures('mplcleanup') def test_relabel(): sys1 = ct.rss(2, inputs='u', outputs='y') sys2 = ct.rss(1, 1, 1) # uses default i/o labels @@ -377,8 +519,8 @@ def test_relabel(): ct.step_response(sys1).plot() # Generate a new plot, which overwrites labels - out = ct.step_response(sys2).plot() - ax = ct.get_plot_axes(out) + cplt = ct.step_response(sys2).plot() + ax = cplt.axes assert ax[0, 0].get_ylabel() == 'y[0]' # Regenerate the first plot @@ -386,9 +528,9 @@ def test_relabel(): ct.step_response(sys1).plot() # Generate a new plt, without relabeling - out = ct.step_response(sys2).plot(relabel=False) - ax = ct.get_plot_axes(out) - assert ax[0, 0].get_ylabel() == 'y' + with pytest.warns(FutureWarning, match="deprecated"): + cplt = ct.step_response(sys2).plot(relabel=False) + assert cplt.axes[0, 0].get_ylabel() == 'y' def test_errors(): @@ -408,8 +550,47 @@ def test_errors(): for kw in ['input_props', 'output_props', 'trace_props']: propkw = {kw: {'color': 'green'}} with pytest.warns(UserWarning, match="ignored since fmt string"): - out = stepresp.plot('k-', **propkw) - assert out[0, 0][0].get_color() == 'k' + cplt = stepresp.plot('k-', **propkw) + assert cplt.lines[0, 0][0].get_color() == 'k' + + # Make sure TimeResponseLists also work + stepresp = ct.step_response([sys, sys]) + with pytest.raises(AttributeError, + match="(has no property|unexpected keyword)"): + stepresp.plot(unknown=None) + + +def test_legend_customization(): + sys = ct.rss(4, 2, 1, name='sys') + timepts = np.linspace(0, 10) + U = np.sin(timepts) + resp = ct.input_output_response(sys, timepts, U) + + # Generic input/output plot + cplt = resp.plot(overlay_signals=True) + axs = cplt.axes + assert axs[0, 0].get_legend()._loc == 7 # center right + assert len(axs[0, 0].get_legend().get_texts()) == 2 + assert axs[1, 0].get_legend() == None + plt.close() + + # Hide legend + cplt = resp.plot(overlay_signals=True, show_legend=False) + axs = cplt.axes + assert axs[0, 0].get_legend() == None + assert axs[1, 0].get_legend() == None + plt.close() + + # Put legend in both axes + cplt = resp.plot( + overlay_signals=True, legend_map=[['center left'], ['center right']]) + axs = cplt.axes + assert axs[0, 0].get_legend()._loc == 6 # center left + assert len(axs[0, 0].get_legend().get_texts()) == 2 + assert axs[1, 0].get_legend()._loc == 7 # center right + assert len(axs[1, 0].get_legend().get_texts()) == 1 + plt.close() + if __name__ == "__main__": # @@ -503,9 +684,34 @@ def test_errors(): plt.savefig('timeplot-mimo_ioresp-mt_tr.png') plt.figure() - out = ct.step_response(sys_mimo).plot( - plot_inputs='overlay', overlay_signals=True, overlay_traces=True, - output_props=[{'color': c} for c in ['blue', 'orange']], - input_props=[{'color': c} for c in ['red', 'green']], - trace_props=[{'linestyle': s} for s in ['-', '--']]) + cplt = ct.step_response(sys_mimo).plot( + plot_inputs='overlay', overlay_signals=True, overlay_traces=True, + output_props=[{'color': c} for c in ['blue', 'orange']], + input_props=[{'color': c} for c in ['red', 'green']], + trace_props=[{'linestyle': s} for s in ['-', '--']]) plt.savefig('timeplot-mimo_step-linestyle.png') + + sys1 = ct.rss(4, 2, 2) + sys2 = ct.rss(4, 2, 2) + resp_list = ct.step_response([sys1, sys2]) + + fig = plt.figure() + cplt = ct.combine_time_responses( + [ct.step_response(sys1, resp_list[0].time), + ct.step_response(sys2, resp_list[1].time)] + ).plot(overlay_traces=True) + cplt.set_plot_title("[Combine] " + fig._suptitle._text) + + fig = plt.figure() + ct.step_response(sys1).plot() + cplt = ct.step_response(sys2).plot() + cplt.set_plot_title("[Sequential] " + fig._suptitle._text) + + fig = plt.figure() + ct.step_response(sys1).plot(color='b') + cplt = ct.step_response(sys2).plot(color='r') + cplt.set_plot_title("[Seq w/color] " + fig._suptitle._text) + + fig = plt.figure() + cplt = ct.step_response([sys1, sys2]).plot() + cplt.set_plot_title("[List] " + fig._suptitle._text) diff --git a/control/tests/timeresp_test.py b/control/tests/timeresp_test.py index fb21180b3..e2d93be0e 100644 --- a/control/tests/timeresp_test.py +++ b/control/tests/timeresp_test.py @@ -1,6 +1,7 @@ """timeresp_test.py - test time response functions""" from copy import copy +from math import isclose import numpy as np import pytest @@ -8,11 +9,11 @@ import control as ct from control import StateSpace, TransferFunction, c2d, isctime, ss2tf, tf2ss -from control.exception import slycot_check, pandas_check +from control.exception import pandas_check, slycot_check from control.tests.conftest import slycotonly -from control.timeresp import (_default_time_vector, _ideal_tfinal_and_dt, - forced_response, impulse_response, - initial_response, step_info, step_response) +from control.timeresp import _default_time_vector, _ideal_tfinal_and_dt, \ + forced_response, impulse_response, initial_response, step_info, \ + step_response class TSys: @@ -173,15 +174,15 @@ def tsystem(self, request): # System Type 1 - Step response not stationary: G(s)=1/s(s+1) siso_tf_type1 = TSys(TransferFunction(1, [1, 1, 0])) siso_tf_type1.step_info = { - 'RiseTime': np.NaN, - 'SettlingTime': np.NaN, - 'SettlingMin': np.NaN, - 'SettlingMax': np.NaN, - 'Overshoot': np.NaN, - 'Undershoot': np.NaN, - 'Peak': np.Inf, - 'PeakTime': np.Inf, - 'SteadyStateValue': np.NaN} + 'RiseTime': np.nan, + 'SettlingTime': np.nan, + 'SettlingMin': np.nan, + 'SettlingMax': np.nan, + 'Overshoot': np.nan, + 'Undershoot': np.nan, + 'Peak': np.inf, + 'PeakTime': np.inf, + 'SteadyStateValue': np.nan} # SISO under shoot response and positive final value # G(s)=(-s+1)/(s²+s+1) @@ -536,6 +537,32 @@ def test_discrete_time_impulse(self, tsystem): sysdt = sys.sample(dt, 'impulse') np.testing.assert_array_almost_equal(impulse_response(sys, t)[1], impulse_response(sysdt, t)[1]) + + def test_discrete_time_impulse_input(self): + # discrete time impulse input, Only one active input for each trace + A = [[.5, 0.25],[.0, .5]] + B = [[1., 0,],[0., 1.]] + C = [[1., 0.],[0., 1.]] + D = [[0., 0.],[0., 0.]] + dt = True + sysd = ct.ss(A,B,C,D, dt=dt) + response = ct.impulse_response(sysd,T=dt*3) + + Uexpected = np.zeros((2,2,4), dtype=float).astype(object) + Uexpected[0,0,0] = 1./dt + Uexpected[1,1,0] = 1./dt + + np.testing.assert_array_equal(response.inputs,Uexpected) + + dt = 0.5 + sysd = ct.ss(A,B,C,D, dt=dt) + response = ct.impulse_response(sysd,T=dt*3) + + Uexpected = np.zeros((2,2,4), dtype=float).astype(object) + Uexpected[0,0,0] = 1./dt + Uexpected[1,1,0] = 1./dt + + np.testing.assert_array_equal(response.inputs,Uexpected) @pytest.mark.parametrize("tsystem", ["siso_ss1"], indirect=True) def test_impulse_response_warnD(self, tsystem): @@ -1249,3 +1276,45 @@ def test_no_pandas(): # Convert to pandas with pytest.raises(ImportError, match="pandas"): df = resp.to_pandas() + + +# https://github.com/python-control/python-control/issues/1014 +def test_step_info_nonstep(): + # Pass a constant input + timepts = np.linspace(0, 10, endpoint=False) + y_const = np.ones_like(timepts) + + # Constant value of 1 + step_info = ct.step_info(y_const, timepts) + assert step_info['RiseTime'] == 0 + assert step_info['SettlingTime'] == 0 + assert step_info['SettlingMin'] == 1 + assert step_info['SettlingMax'] == 1 + assert step_info['Overshoot'] == 0 + assert step_info['Undershoot'] == 0 + assert step_info['Peak'] == 1 + assert step_info['PeakTime'] == 0 + assert step_info['SteadyStateValue'] == 1 + + # Constant value of -1 + step_info = ct.step_info(-y_const, timepts) + assert step_info['RiseTime'] == 0 + assert step_info['SettlingTime'] == 0 + assert step_info['SettlingMin'] == -1 + assert step_info['SettlingMax'] == -1 + assert step_info['Overshoot'] == 0 + assert step_info['Undershoot'] == 0 + assert step_info['Peak'] == 1 + assert step_info['PeakTime'] == 0 + assert step_info['SteadyStateValue'] == -1 + + # Ramp from -1 to 1 + step_info = ct.step_info(-1 + 2 * timepts/10, timepts) + assert step_info['RiseTime'] == 3.8 + assert step_info['SettlingTime'] == 9.8 + assert isclose(step_info['SettlingMin'], 0.88) + assert isclose(step_info['SettlingMax'], 0.96) + assert step_info['Overshoot'] == 0 + assert step_info['Peak'] == 1 + assert step_info['PeakTime'] == 0 + assert isclose(step_info['SteadyStateValue'], 0.96) diff --git a/control/tests/type_conversion_test.py b/control/tests/type_conversion_test.py index ad8dea911..efd1a66a8 100644 --- a/control/tests/type_conversion_test.py +++ b/control/tests/type_conversion_test.py @@ -57,7 +57,7 @@ def sys_dict(): ('add', 'ios', ['ios', 'ios', 'E', 'ios', 'ios', 'ios']), ('add', 'arr', ['ss', 'tf', 'frd', 'ios', 'arr', 'arr']), ('add', 'flt', ['ss', 'tf', 'frd', 'ios', 'arr', 'flt']), - + # op left ss tf frd ios arr flt ('sub', 'ss', ['ss', 'ss', 'frd', 'ios', 'ss', 'ss' ]), ('sub', 'tf', ['tf', 'tf', 'frd', 'ios', 'tf', 'tf' ]), @@ -65,7 +65,7 @@ def sys_dict(): ('sub', 'ios', ['ios', 'ios', 'E', 'ios', 'ios', 'ios']), ('sub', 'arr', ['ss', 'tf', 'frd', 'ios', 'arr', 'arr']), ('sub', 'flt', ['ss', 'tf', 'frd', 'ios', 'arr', 'flt']), - + # op left ss tf frd ios arr flt ('mul', 'ss', ['ss', 'ss', 'frd', 'ios', 'ss', 'ss' ]), ('mul', 'tf', ['tf', 'tf', 'frd', 'ios', 'tf', 'tf' ]), @@ -73,7 +73,7 @@ def sys_dict(): ('mul', 'ios', ['ios', 'ios', 'E', 'ios', 'ios', 'ios']), ('mul', 'arr', ['ss', 'tf', 'frd', 'ios', 'arr', 'arr']), ('mul', 'flt', ['ss', 'tf', 'frd', 'ios', 'arr', 'flt']), - + # op left ss tf frd ios arr flt ('truediv', 'ss', ['E', 'tf', 'frd', 'E', 'ss', 'ss' ]), ('truediv', 'tf', ['tf', 'tf', 'xrd', 'E', 'tf', 'tf' ]), @@ -88,7 +88,7 @@ def sys_dict(): for rtype, expected in zip(rtype_list, expected_list): # Add this to the list of tests to run test_matrix.append([opname, ltype, rtype, expected]) - + @pytest.mark.parametrize("opname, ltype, rtype, expected", test_matrix) def test_operator_type_conversion(opname, ltype, rtype, expected, sys_dict): op = getattr(operator, opname) @@ -98,7 +98,7 @@ def test_operator_type_conversion(opname, ltype, rtype, expected, sys_dict): # Get rid of warnings for NonlinearIOSystem objects by making a copy if isinstance(leftsys, ct.NonlinearIOSystem) and leftsys == rightsys: rightsys = leftsys.copy() - + # Make sure we get the right result if expected == 'E' or expected[0] == 'x': # Exception expected @@ -107,7 +107,7 @@ def test_operator_type_conversion(opname, ltype, rtype, expected, sys_dict): else: # Operation should work and return the given type result = op(leftsys, rightsys) - + # Print out what we are testing in case something goes wrong assert isinstance(result, type_dict[expected]) @@ -126,7 +126,7 @@ def test_operator_type_conversion(opname, ltype, rtype, expected, sys_dict): # # * For IOS/LTI, convert to IOS. In the case of a linear I/O system (LIO), # this will preserve the linear structure since the LTI system will -# be converted to state space. +# be converted to state space. # # * When combining state space or transfer with linear I/O systems, the # * output should be of type Linear IO system, since that maintains the diff --git a/control/tests/xferfcn_test.py b/control/tests/xferfcn_test.py index cb5b38cba..14a11b669 100644 --- a/control/tests/xferfcn_test.py +++ b/control/tests/xferfcn_test.py @@ -487,7 +487,7 @@ def test_call_mimo(self): def test_freqresp_deprecated(self): sys = TransferFunction([1., 3., 5], [1., 6., 2., -1.]) # Deprecated version of the call (should generate warning) - with pytest.warns(DeprecationWarning): + with pytest.warns(FutureWarning): sys.freqresp(1.) def test_frequency_response_siso(self): diff --git a/control/timeplot.py b/control/timeplot.py index 58f7d8382..1c7efe894 100644 --- a/control/timeplot.py +++ b/control/timeplot.py @@ -8,30 +8,21 @@ # Note: It might eventually make sense to put the functions here # directly into timeresp.py. -import numpy as np +import itertools +from warnings import warn + import matplotlib as mpl import matplotlib.pyplot as plt -from os.path import commonprefix -from warnings import warn +import numpy as np from . import config +from .ctrlplot import ControlPlot, _make_legend_labels,\ + _process_legend_keywords, _update_plot_title -__all__ = ['time_response_plot', 'combine_time_responses', 'get_plot_axes'] - -# Default font dictionary -_timeplot_rcParams = mpl.rcParams.copy() -_timeplot_rcParams.update({ - 'axes.labelsize': 'small', - 'axes.titlesize': 'small', - 'figure.titlesize': 'medium', - 'legend.fontsize': 'x-small', - 'xtick.labelsize': 'small', - 'ytick.labelsize': 'small', -}) +__all__ = ['time_response_plot', 'combine_time_responses'] # Default values for module parameter variables _timeplot_defaults = { - 'timeplot.rcParams': _timeplot_rcParams, 'timeplot.trace_props': [ {'linestyle': s} for s in ['-', '--', ':', '-.']], 'timeplot.output_props': [ @@ -41,14 +32,17 @@ {'color': c} for c in [ 'tab:red', 'tab:purple', 'tab:brown', 'tab:olive', 'tab:cyan']], 'timeplot.time_label': "Time [s]", + 'timeplot.sharex': 'col', + 'timeplot.sharey': False, } + # Plot the input/output response of a system def time_response_plot( data, *fmt, ax=None, plot_inputs=None, plot_outputs=True, transpose=False, overlay_traces=False, overlay_signals=False, - legend_map=None, legend_loc=None, add_initial_zero=True, - trace_labels=None, title=None, relabel=True, **kwargs): + add_initial_zero=True, label=None, trace_labels=None, title=None, + relabel=True, **kwargs): """Plot the time response of an input/output system. This function creates a standard set of plots for the input/output @@ -60,15 +54,6 @@ def time_response_plot( ---------- data : TimeResponseData Data to be plotted. - ax : array of Axes - The matplotlib Axes to draw the figure on. If not specified, the - Axes for the current figure are used or, if there is no current - figure with the correct number and shape of Axes, a new figure is - created. The default shape of the array should be (noutputs + - ninputs, ntraces), but if `overlay_traces` is set to `True` then - only one row is needed and if `overlay_signals` is set to `True` - then only one or two columns are needed (depending on plot_inputs - and plot_outputs). plot_inputs : bool or str, optional Sets how and where to plot the inputs: * False: don't plot the inputs @@ -83,6 +68,14 @@ def time_response_plot( overlay_signals : bool, optional If set to True, combine all input and output signals onto a single plot (for each). + sharex, sharey : str or bool, optional + Determine whether and how x- and y-axis limits are shared between + subplots. Can be set set to 'row' to share across all subplots in + a row, 'col' to set across all subplots in a column, 'all' to share + across all subplots, or `False` to allow independent limits. + Default values are `False` for `sharex' and 'col' for `sharey`, and + can be set using config.defaults['timeplot.sharex'] and + config.defaults['timeplot.sharey']. transpose : bool, optional If transpose is False (default), signals are plotted from top to bottom, starting with outputs (if plotted) and then inputs. @@ -97,35 +90,69 @@ def time_response_plot( Returns ------- - out : array of list of Line2D - Array of Line2D objects for each line in the plot. The shape of - the array matches the subplots shape and the value of the array is a - list of Line2D objects in that subplot. + cplt : :class:`ControlPlot` object + Object containing the data that were plotted: + + * cplt.lines: Array of :class:`matplotlib.lines.Line2D` objects + for each line in the plot. The shape of the array matches the + subplots shape and the value of the array is a list of Line2D + objects in that subplot. + + * cplt.axes: 2D array of :class:`matplotlib.axes.Axes` for the plot. + + * cplt.figure: :class:`matplotlib.figure.Figure` containing the plot. + + * cplt.legend: legend object(s) contained in the plot + + See :class:`ControlPlot` for more detailed information. Other Parameters ---------------- add_initial_zero : bool Add an initial point of zero at the first time point for all inputs with type 'step'. Default is True. + ax : array of matplotlib.axes.Axes, optional + The matplotlib axes to draw the figure on. If not specified, the + axes for the current figure are used or, if there is no current + figure with the correct number and shape of axes, a new figure is + created. The shape of the array must match the shape of the + plotted data. input_props : array of dicts List of line properties to use when plotting combined inputs. The default values are set by config.defaults['timeplot.input_props']. - legend_map : array of str, option - Location of the legend for multi-trace plots. Specifies an array + label : str or array_like of str, optional + If present, replace automatically generated label(s) with the given + label(s). If more than one line is being generated, an array of + labels should be provided with label[trace, :, 0] representing the + output labels and label[trace, :, 1] representing the input labels. + legend_map : array of str, optional + Location of the legend for multi-axes plots. Specifies an array of legend location strings matching the shape of the subplots, with each entry being either None (for no legend) or a legend location string (see :func:`~matplotlib.pyplot.legend`). - legend_loc : str - Location of the legend within the axes for which it appears. This - value is used if legend_map is None. - output_props : array of dicts + legend_loc : int or str, optional + Include a legend in the given location. Default is 'center right', + with no legend for a single response. Use False to suppress legend. + output_props : array of dicts, optional List of line properties to use when plotting combined outputs. The default values are set by config.defaults['timeplot.output_props']. + rcParams : dict + Override the default parameters used for generating plots. + Default is set by config.default['ctrlplot.rcParams']. relabel : bool, optional - By default, existing figures and axes are relabeled when new data - are added. If set to `False`, just plot new data on existing axes. + (deprecated) By default, existing figures and axes are relabeled + when new data are added. If set to `False`, just plot new data on + existing axes. + show_legend : bool, optional + Force legend to be shown if ``True`` or hidden if ``False``. If + ``None``, then show legend when there is more than one line on an + axis or ``legend_loc`` or ``legend_map`` has been specified. time_label : str, optional Label to use for the time axis. + title : str, optional + Set the title of the plot. Defaults to plot type and system name(s). + trace_labels : list of str, optional + Replace the default trace labels with the given labels. trace_props : array of dicts List of line properties to use when plotting combined outputs. The default values are set by config.defaults['timeplot.trace_props']. @@ -150,18 +177,20 @@ def time_response_plot( config.defaults[''timeplot.rcParams']. """ + from .ctrlplot import _process_ax_keyword, _process_line_labels from .iosys import InputOutputSystem from .timeresp import TimeResponseData # # Process keywords and set defaults # - # Set up defaults + ax_user = ax + sharex = config._get_param('timeplot', 'sharex', kwargs, pop=True) + sharey = config._get_param('timeplot', 'sharey', kwargs, pop=True) time_label = config._get_param( 'timeplot', 'time_label', kwargs, _timeplot_defaults, pop=True) - timeplot_rcParams = config._get_param( - 'timeplot', 'rcParams', kwargs, _timeplot_defaults, pop=True) + rcParams = config._get_param('ctrlplot', 'rcParams', kwargs, pop=True) if kwargs.get('input_props', None) and len(fmt) > 0: warn("input_props ignored since fmt string was present") @@ -181,9 +210,6 @@ def time_response_plot( 'timeplot', 'trace_props', kwargs, _timeplot_defaults, pop=True) tprop_len = len(trace_props) - # Set the title for the data - title = data.title if title == None else title - # Determine whether or not to plot the input data (and how) if plot_inputs is None: plot_inputs = data.plot_inputs @@ -275,33 +301,10 @@ def time_response_plot( nrows, ncols = ncols, nrows # See if we can use the current figure axes - fig = plt.gcf() # get current figure (or create new one) - if ax is None and plt.get_fignums(): - ax = fig.get_axes() - if len(ax) == nrows * ncols: - # Assume that the shape is right (no easy way to infer this) - ax = np.array(ax).reshape(nrows, ncols) - elif len(ax) != 0: - # Need to generate a new figure - fig, ax = plt.figure(), None - else: - # Blank figure, just need to recreate axes - ax = None - - # Create new axes, if needed, and customize them - if ax is None: - with plt.rc_context(timeplot_rcParams): - ax_array = fig.subplots(nrows, ncols, sharex=True, squeeze=False) - fig.set_layout_engine('tight') - fig.align_labels() - - else: - # Make sure the axes are the right shape - if ax.shape != (nrows, ncols): - raise ValueError( - "specified axes are not the right shape; " - f"got {ax.shape} but expecting ({nrows}, {ncols})") - ax_array = ax + fig, ax_array = _process_ax_keyword( + ax, (nrows, ncols), rcParams=rcParams, sharex=sharex, sharey=sharey) + legend_loc, legend_map, show_legend = _process_legend_keywords( + kwargs, (nrows, ncols), 'center right') # # Map inputs/outputs and traces to axes @@ -366,6 +369,7 @@ def time_response_plot( out[i, j] = [] # unique list in each element # Utility function for creating line label + # TODO: combine with freqplot version? def _make_line_label(signal_index, signal_labels, trace_index): label = "" # start with an empty label @@ -389,16 +393,38 @@ def _make_line_label(signal_index, signal_labels, trace_index): return label + # + # Store the color offsets with the figure to allow color/style cycling + # + # To allow repeated calls to time_response_plot() to cycle through + # colors, we store an offset in the figure object that we can + # retrieve in a later call, if needed. + # + output_offset = fig._output_offset = getattr(fig, '_output_offset', 0) + input_offset = fig._input_offset = getattr(fig, '_input_offset', 0) + + # + # Plot the lines for the response + # + + # Process labels + line_labels = _process_line_labels( + label, ntraces, max(ninputs, noutputs), 2) + # Go through each trace and each input/output for trace in range(ntraces): # Plot the output for i in range(noutputs): - label = _make_line_label(i, data.output_labels, trace) + if line_labels is None: + label = _make_line_label(i, data.output_labels, trace) + else: + label = line_labels[trace, i, 0] # Set up line properties for this output, trace if len(fmt) == 0: line_props = output_props[ - i % oprop_len if overlay_signals else 0].copy() + (i + output_offset) % oprop_len if overlay_signals + else output_offset].copy() line_props.update( trace_props[trace % tprop_len if overlay_traces else 0]) line_props.update(kwargs) @@ -410,7 +436,10 @@ def _make_line_label(signal_index, signal_labels, trace_index): # Plot the input for i in range(ninputs): - label = _make_line_label(i, data.input_labels, trace) + if line_labels is None: + label = _make_line_label(i, data.input_labels, trace) + else: + label = line_labels[trace, i, 1] if add_initial_zero and data.ntraces > i \ and data.trace_types[i] == 'step': @@ -422,7 +451,8 @@ def _make_line_label(signal_index, signal_labels, trace_index): # Set up line properties for this output, trace if len(fmt) == 0: line_props = input_props[ - i % iprop_len if overlay_signals else 0].copy() + (i + input_offset) % iprop_len if overlay_signals + else input_offset].copy() line_props.update( trace_props[trace % tprop_len if overlay_traces else 0]) line_props.update(kwargs) @@ -432,9 +462,16 @@ def _make_line_label(signal_index, signal_labels, trace_index): out[input_map[i, trace]] += ax_array[input_map[i, trace]].plot( x, y, *fmt, label=label, **line_props) + # Update the offsets so that we start at a new color/style the next time + fig._output_offset = ( + output_offset + (noutputs if overlay_signals else 1)) % oprop_len + fig._input_offset = ( + input_offset + (ninputs if overlay_signals else 1)) % iprop_len + # Stop here if the user wants to control everything if not relabel: - return out + warn("relabel keyword is deprecated", FutureWarning) + return ControlPlot(out, ax_array, fig) # # Label the axes (including trace labels) @@ -506,7 +543,7 @@ def _make_line_label(signal_index, signal_labels, trace_index): else: label = f"Trace {trace}" - with plt.rc_context(timeplot_rcParams): + with plt.rc_context(rcParams): ax_array[0, trace].set_title(label) # Label the outputs @@ -547,10 +584,9 @@ def _make_line_label(signal_index, signal_labels, trace_index): # # Figure out where to put legends - if legend_map is None: + if show_legend != False and legend_map is None: legend_map = np.full(ax_array.shape, None, dtype=object) - if legend_loc == None: - legend_loc = 'center right' + if transpose: if (overlay_signals or plot_inputs == 'overlay') and overlay_traces: # Put a legend in each plot for inputs and outputs @@ -574,6 +610,7 @@ def _make_line_label(signal_index, signal_labels, trace_index): else: # Put legend in the upper right legend_map[0, -1] = legend_loc + else: # regular layout if (overlay_signals or plot_inputs == 'overlay') and overlay_traces: # Put a legend in each plot for inputs and outputs @@ -598,19 +635,24 @@ def _make_line_label(signal_index, signal_labels, trace_index): # Put legend in the upper right legend_map[0, -1] = legend_loc - # Create axis legends - for i in range(nrows): - for j in range(ncols): + if show_legend != False: + # Create axis legends + legend_array = np.full(ax_array.shape, None, dtype=object) + for i, j in itertools.product(range(nrows), range(ncols)): + if legend_map[i, j] is None: + continue ax = ax_array[i, j] - # Get the labels to use labels = [line.get_label() for line in ax.get_lines()] - labels = _make_legend_labels(labels, plot_inputs == 'overlay') + if line_labels is None: + labels = _make_legend_labels(labels, plot_inputs == 'overlay') # Update the labels to remove common strings - if len(labels) > 1 and legend_map[i, j] != None: - with plt.rc_context(timeplot_rcParams): - ax.legend(labels, loc=legend_map[i, j]) - + if show_legend == True or len(labels) > 1: + with plt.rc_context(rcParams): + legend_array[i, j] = ax.legend( + labels, loc=legend_map[i, j]) + else: + legend_array = None # # Update the plot title (= figure suptitle) @@ -622,31 +664,13 @@ def _make_line_label(signal_index, signal_labels, trace_index): # list of systems (e.g., "Step response for sys[1], sys[2]"). # - if fig is not None and title is not None: - # Get the current title, if it exists - old_title = None if fig._suptitle is None else fig._suptitle._text - new_title = title + if ax_user is None and title is None: + title = data.title if title == None else title + _update_plot_title(title, fig, rcParams=rcParams) + elif ax_user is None: + _update_plot_title(title, fig, rcParams=rcParams, use_existing=False) - if old_title is not None: - # Find the common part of the titles - common_prefix = commonprefix([old_title, new_title]) - - # Back up to the last space - last_space = common_prefix.rfind(' ') - if last_space > 0: - common_prefix = common_prefix[:last_space] - common_len = len(common_prefix) - - # Add the new part of the title (usually the system name) - if old_title[common_len:] != new_title[common_len:]: - separator = ',' if len(common_prefix) > 0 else ';' - new_title = old_title + separator + new_title[common_len:] - - # Add the title - with plt.rc_context(timeplot_rcParams): - fig.suptitle(new_title) - - return out + return ControlPlot(out, ax_array, fig, legend=legend_map) def combine_time_responses(response_list, trace_labels=None, title=None): @@ -662,6 +686,9 @@ def combine_time_responses(response_list, trace_labels=None, title=None): trace_labels : list of str, optional List of labels for each trace. If not specified, trace names are taken from the input data or set to None. + title : str, optional + Set the title to use when plotting. Defaults to plot type and + system name(s). Returns ------- @@ -681,6 +708,7 @@ def combine_time_responses(response_list, trace_labels=None, title=None): ntraces = max(1, base.ntraces) # Initial pass through trace list to count things up and do error checks + nstates = base.nstates for response in response_list[1:]: # Make sure the time vector is the same if not np.allclose(base.t, response.t): @@ -688,17 +716,20 @@ def combine_time_responses(response_list, trace_labels=None, title=None): # Make sure the dimensions are all the same if base.ninputs != response.ninputs or \ - base.noutputs != response.noutputs or \ - base.nstates != response.nstates: + base.noutputs != response.noutputs: raise ValueError("all responses must have the same number of " "inputs, outputs, and states") + if nstates != response.nstates: + warn("responses have different state dimensions; dropping states") + nstates = 0 + ntraces += max(1, response.ntraces) # Create data structures for the new time response data object inputs = np.empty((base.ninputs, ntraces, base.t.size)) outputs = np.empty((base.noutputs, ntraces, base.t.size)) - states = np.empty((base.nstates, ntraces, base.t.size)) + states = np.empty((nstates, ntraces, base.t.size)) # See whether we should create labels or not if trace_labels is None: @@ -717,21 +748,24 @@ def combine_time_responses(response_list, trace_labels=None, title=None): # Single trace inputs[:, offset, :] = response.u outputs[:, offset, :] = response.y - states[:, offset, :] = response.x + if nstates: + states[:, offset, :] = response.x offset += 1 # Add on trace label and trace type if generate_trace_labels: trace_labels.append(response.title) trace_types.append( - None if response.trace_types is None else response.types[0]) + None if response.trace_types is None + else response.trace_types[0]) else: # Save the data for i in range(response.ntraces): inputs[:, offset, :] = response.u[:, i, :] outputs[:, offset, :] = response.y[:, i, :] - states[:, offset, :] = response.x[:, i, :] + if nstates: + states[:, offset, :] = response.x[:, i, :] # Save the trace labels if generate_trace_labels: @@ -749,68 +783,10 @@ def combine_time_responses(response_list, trace_labels=None, title=None): trace_types += [None] * response.ntraces return TimeResponseData( - base.t, outputs, states, inputs, issiso=base.issiso, + base.t, outputs, states if nstates else None, inputs, output_labels=base.output_labels, input_labels=base.input_labels, - state_labels=base.state_labels, title=title, transpose=base.transpose, - return_x=base.return_x, squeeze=base.squeeze, sysname=base.sysname, + state_labels=base.state_labels if nstates else None, + title=title, transpose=base.transpose, return_x=base.return_x, + issiso=base.issiso, squeeze=base.squeeze, sysname=base.sysname, trace_labels=trace_labels, trace_types=trace_types, plot_inputs=base.plot_inputs) - - -# Create vectorized function to find axes from lines -def get_plot_axes(line_array): - """Get a list of axes from an array of lines. - - This function can be used to return the set of axes corresponding to - the line array that is returned by `time_response_plot`. This is useful for - generating an axes array that can be passed to subsequent plotting - calls. - - Parameters - ---------- - line_array : array of list of Line2D - A 2D array with elements corresponding to a list of lines appearing - in an axes, matching the return type of a time response data plot. - - Returns - ------- - axes_array : array of list of Axes - A 2D array with elements corresponding to the Axes assocated with - the lines in `line_array`. - - Notes - ----- - Only the first element of each array entry is used to determine the axes. - - """ - _get_axes = np.vectorize(lambda lines: lines[0].axes) - return _get_axes(line_array) - - -# Utility function to make legend labels -def _make_legend_labels(labels, ignore_common=False): - - # Look for a common prefix (up to a space) - common_prefix = commonprefix(labels) - last_space = common_prefix.rfind(', ') - if last_space < 0 or ignore_common: - common_prefix = '' - elif last_space > 0: - common_prefix = common_prefix[:last_space] - prefix_len = len(common_prefix) - - # Look for a common suffice (up to a space) - common_suffix = commonprefix( - [label[::-1] for label in labels])[::-1] - suffix_len = len(common_suffix) - # Only chop things off after a comma or space - while suffix_len > 0 and common_suffix[-suffix_len] != ',': - suffix_len -= 1 - - # Strip the labels of common information - if suffix_len > 0: - labels = [label[prefix_len:-suffix_len] for label in labels] - else: - labels = [label[prefix_len:] for label in labels] - - return labels diff --git a/control/timeresp.py b/control/timeresp.py index 58207e88e..5813c166d 100644 --- a/control/timeresp.py +++ b/control/timeresp.py @@ -71,21 +71,21 @@ """ import warnings +from copy import copy import numpy as np import scipy as sp from numpy import einsum, maximum, minimum from scipy.linalg import eig, eigvals, matrix_balance, norm -from copy import copy from . import config from .exception import pandas_check from .iosys import isctime, isdtime from .timeplot import time_response_plot - __all__ = ['forced_response', 'step_response', 'step_info', - 'initial_response', 'impulse_response', 'TimeResponseData'] + 'initial_response', 'impulse_response', 'TimeResponseData', + 'TimeResponseList'] class TimeResponseData: @@ -111,7 +111,7 @@ class TimeResponseData: :attr:`time`, :attr:`outputs`, :attr:`states`, :attr:`inputs`. When accessing time responses via their properties, squeeze processing is applied so that (by default) single-input, single-output systems will have - the output and input indices supressed. This behavior is set using the + the output and input indices suppressed. This behavior is set using the ``squeeze`` keyword. Attributes @@ -170,6 +170,13 @@ class TimeResponseData: input_labels, output_labels, state_labels : array of str Names for the input, output, and state variables. + success : bool, optional + If ``False``, result may not be valid (see + :func:`~control.input_output_response`). Defaults to ``True``. + + message : str, optional + Informational message if ``success`` is ``False``. + sysname : str, optional Name of the system that created the data. @@ -185,6 +192,9 @@ class TimeResponseData: response. If ntraces is 0 (default) then the data represents a single trace with the trace index surpressed in the data. + title : str, optional + Set the title to use when plotting. + trace_labels : array of string, optional Labels to use for traces (set to sysname it ntraces is 0) @@ -230,7 +240,8 @@ def __init__( output_labels=None, state_labels=None, input_labels=None, title=None, transpose=False, return_x=False, squeeze=None, multi_trace=False, trace_labels=None, trace_types=None, - plot_inputs=True, sysname=None, params=None + plot_inputs=True, sysname=None, params=None, success=True, + message=None ): """Create an input/output time response object. @@ -307,6 +318,13 @@ def __init__( a MIMO system, the ``input`` attribute should then be set to indicate which trace is being specified. Default is ``False``. + success : bool, optional + If ``False``, result may not be valid (see + :func:`~control.input_output_response`). + + message : str, optional + Informational message if ``success`` is ``False``. + """ # # Process and store the basic input/output elements @@ -460,6 +478,10 @@ def __init__( # Store legacy keyword values (only needed for legacy interface) self.return_x = return_x + # Information on the whether the simulation result may be incorrect + self.success = success + self.message = message + def __call__(self, **kwargs): """Change value of processing keywords. @@ -676,6 +698,12 @@ def __len__(self): # Convert to pandas def to_pandas(self): + """Convert response data to pandas data frame. + + Creates a pandas data frame using the input, output, and state + labels for the time response. + + """ if not pandas_check(): raise ImportError("pandas not installed") import pandas @@ -696,8 +724,45 @@ def to_pandas(self): # Plot data def plot(self, *args, **kwargs): + """Plot the time response data objects. + + This method calls :func:`time_response_plot`, passing all arguments + and keywords. + + """ return time_response_plot(self, *args, **kwargs) +# +# Time response data list class +# +# This class is a subclass of list that adds a plot() method, enabling +# direct plotting from routines returning a list of TimeResponseData +# objects. +# + +class TimeResponseList(list): + """This class consist of a list of :class:`TimeResponseData` objects. + It is a subclass of the Python `list` class, with a `plot` method that + plots the individual :class:`TimeResponseData` objects. + + """ + def plot(self, *args, **kwargs): + from .ctrlplot import ControlPlot + + lines = None + label = kwargs.pop('label', [None] * len(self)) + for i, response in enumerate(self): + cplt = TimeResponseData.plot( + response, *args, label=label[i], **kwargs) + if lines is None: + lines = cplt.lines + else: + # Append the lines in the new plot to previous lines + for row in range(cplt.lines.shape[0]): + for col in range(cplt.lines.shape[1]): + lines[row, col] += cplt.lines[row, col] + return ControlPlot(lines, cplt.axes, cplt.figure) + # Process signal labels def _process_labels(labels, signal, length): @@ -860,7 +925,7 @@ def shape_matches(s_legal, s_actual): # Forced response of a linear system -def forced_response(sys, T=None, U=0., X0=0., transpose=False, params=None, +def forced_response(sysdata, T=None, U=0., X0=0., transpose=False, params=None, interpolate=False, return_x=None, squeeze=None): """Compute the output of a linear system given the input. @@ -873,8 +938,8 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, params=None, Parameters ---------- - sys : StateSpace or TransferFunction - LTI system to simulate + sysdata : I/O system or list of I/O systems + I/O system(s) for which forced response is computed. T : array_like, optional for discrete LTI `sys` Time steps at which the input is defined; values must be evenly spaced. @@ -929,9 +994,10 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, params=None, Returns ------- - results : TimeResponseData - Time response represented as a :class:`TimeResponseData` object - containing the following properties: + results : :class:`TimeResponseData` or :class:`TimeResponseList` + Time response represented as a :class:`TimeResponseData` object or + list of :class:`TimeResponseData` objects containing the following + properties: * time (array): Time values of the output. @@ -945,9 +1011,8 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, params=None, * inputs (array): Input(s) to the system, indexed by input and time. - The return value of the system can also be accessed by assigning the - function to a tuple of length 2 (time, output) or of length 3 (time, - output, state) if ``return_x`` is ``True``. + The `plot()` method can be used to create a plot of the time + response(s) (see :func:`time_response_plot` for more information). See Also -------- @@ -968,6 +1033,10 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, params=None, that `forced_response` is specialized (and optimized) for linear systems. + 4. (legacy) The return value of the system can also be accessed by + assigning the function to a tuple of length 2 (time, output) or of + length 3 (time, output, state) if ``return_x`` is ``True``. + Examples -------- >>> G = ct.rss(4) @@ -978,9 +1047,20 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, params=None, :ref:`package-configuration-parameters`. """ + from .nlsys import NonlinearIOSystem, input_output_response from .statesp import StateSpace, _convert_to_statespace from .xferfcn import TransferFunction - from .nlsys import NonlinearIOSystem, input_output_response + + # If passed a list, recursively call individual responses with given T + if isinstance(sysdata, (list, tuple)): + responses = [] + for sys in sysdata: + responses.append(forced_response( + sys, T, U=U, X0=X0, transpose=transpose, params=params, + interpolate=interpolate, return_x=return_x, squeeze=squeeze)) + return TimeResponseList(responses) + else: + sys = sysdata if not isinstance(sys, (StateSpace, TransferFunction)): if isinstance(sys, NonlinearIOSystem): @@ -1262,8 +1342,9 @@ def _process_time_response( return tout, yout -def step_response(sys, T=None, X0=0, input=None, output=None, T_num=None, - transpose=False, return_x=False, squeeze=None, params=None): +def step_response( + sysdata, T=None, X0=0, input=None, output=None, T_num=None, + transpose=False, return_x=False, squeeze=None, params=None): # pylint: disable=W0622 """Compute the step response for a linear system. @@ -1278,8 +1359,8 @@ def step_response(sys, T=None, X0=0, input=None, output=None, T_num=None, Parameters ---------- - sys : StateSpace or TransferFunction - LTI system to simulate + sysdata : I/O system or list of I/O systems + I/O system(s) for which step response is computed. T : array_like or float, optional Time vector, or simulation time duration if a number. If T is not @@ -1332,27 +1413,10 @@ def step_response(sys, T=None, X0=0, input=None, output=None, T_num=None, Returns ------- - results : TimeResponseData - Time response represented as a :class:`TimeResponseData` object - containing the following properties: - - * time (array): Time values of the output. - - * outputs (array): Response of the system. If the system is SISO and - squeeze is not True, the array is 1D (indexed by time). If the - system is not SISO or ``squeeze`` is False, the array is 3D (indexed - by the output, trace, and time). - - * states (array): Time evolution of the state vector, represented as - either a 2D array indexed by state and time (if SISO) or a 3D array - indexed by state, trace, and time. Not affected by ``squeeze``. - - * inputs (array): Input(s) to the system, indexed in the same manner - as ``outputs``. - - The return value of the system can also be accessed by assigning the - function to a tuple of length 2 (time, output) or of length 3 (time, - output, state) if ``return_x`` is ``True``. + results : `TimeResponseData` or `TimeResponseList` + Time response represented as a :class:`TimeResponseData` object or + list of :class:`TimeResponseData` objects. See + :func:`forced_response` for additional information. See Also -------- @@ -1370,16 +1434,28 @@ def step_response(sys, T=None, X0=0, input=None, output=None, T_num=None, """ from .lti import LTI - from .xferfcn import TransferFunction from .statesp import _convert_to_statespace + from .xferfcn import TransferFunction # Create the time and input vectors if T is None or np.asarray(T).size == 1: - T = _default_time_vector(sys, N=T_num, tfinal=T, is_step=True) + T = _default_time_vector(sysdata, N=T_num, tfinal=T, is_step=True) T = np.atleast_1d(T).reshape(-1) if T.ndim != 1 and len(T) < 2: raise ValueError("invalid value of T for this type of system") + # If passed a list, recursively call individual responses with given T + if isinstance(sysdata, (list, tuple)): + responses = [] + for sys in sysdata: + responses.append(step_response( + sys, T, X0=X0, input=input, output=output, T_num=T_num, + transpose=transpose, return_x=return_x, squeeze=squeeze, + params=params)) + return TimeResponseList(responses) + else: + sys = sysdata + # If we are passed a transfer function and X0 is non-zero, warn the user if isinstance(sys, TransferFunction) and np.any(X0 != 0): warnings.warn( @@ -1543,9 +1619,9 @@ def step_info(sysdata, T=None, T_num=None, yfinal=None, params=None, PeakTime: 4.242 SteadyStateValue: -1.0 """ + from .nlsys import NonlinearIOSystem from .statesp import StateSpace from .xferfcn import TransferFunction - from .nlsys import NonlinearIOSystem if isinstance(sysdata, (StateSpace, TransferFunction, NonlinearIOSystem)): T, Yout = step_response(sysdata, T, squeeze=False, params=params) @@ -1590,29 +1666,31 @@ def step_info(sysdata, T=None, T_num=None, yfinal=None, params=None, InfValue = InfValues[i, j] sgnInf = np.sign(InfValue.real) - rise_time: float = np.NaN - settling_time: float = np.NaN - settling_min: float = np.NaN - settling_max: float = np.NaN - peak_value: float = np.Inf - peak_time: float = np.Inf - undershoot: float = np.NaN - overshoot: float = np.NaN - steady_state_value: complex = np.NaN + rise_time: float = np.nan + settling_time: float = np.nan + settling_min: float = np.nan + settling_max: float = np.nan + peak_value: float = np.inf + peak_time: float = np.inf + undershoot: float = np.nan + overshoot: float = np.nan + steady_state_value: complex = np.nan if not np.isnan(InfValue) and not np.isinf(InfValue): # RiseTime - tr_lower_index = np.where( + tr_lower_index = np.nonzero( sgnInf * (yout - RiseTimeLimits[0] * InfValue) >= 0 )[0][0] - tr_upper_index = np.where( + tr_upper_index = np.nonzero( sgnInf * (yout - RiseTimeLimits[1] * InfValue) >= 0 )[0][0] rise_time = T[tr_upper_index] - T[tr_lower_index] # SettlingTime - settled = np.where( - np.abs(yout/InfValue-1) >= SettlingTimeThreshold)[0][-1]+1 + outside_threshold = np.nonzero( + np.abs(yout/InfValue - 1) >= SettlingTimeThreshold)[0] + settled = 0 if outside_threshold.size == 0 \ + else outside_threshold[-1] + 1 # MIMO systems can have unsettled channels without infinite # InfValue if settled < len(T): @@ -1663,8 +1741,9 @@ def step_info(sysdata, T=None, T_num=None, yfinal=None, params=None, return ret[0][0] if retsiso else ret -def initial_response(sys, T=None, X0=0, output=None, T_num=None, params=None, - transpose=False, return_x=False, squeeze=None): +def initial_response( + sysdata, T=None, X0=0, output=None, T_num=None, params=None, + transpose=False, return_x=False, squeeze=None): # pylint: disable=W0622 """Compute the initial condition response for a linear system. @@ -1677,6 +1756,9 @@ def initial_response(sys, T=None, X0=0, output=None, T_num=None, params=None, Parameters ---------- + sysdata : I/O system or list of I/O systems + I/O system(s) for which initial response is computed. + sys : StateSpace or TransferFunction LTI system to simulate @@ -1719,24 +1801,10 @@ def initial_response(sys, T=None, X0=0, output=None, T_num=None, params=None, Returns ------- - results : TimeResponseData - Time response represented as a :class:`TimeResponseData` object - containing the following properties: - - * time (array): Time values of the output. - - * outputs (array): Response of the system. If the system is SISO and - squeeze is not True, the array is 1D (indexed by time). If the - system is not SISO or ``squeeze`` is False, the array is 2D (indexed - by the output and time). - - * states (array): Time evolution of the state vector, represented as - either a 2D array indexed by state and time (if SISO). Not affected - by ``squeeze``. - - The return value of the system can also be accessed by assigning the - function to a tuple of length 2 (time, output) or of length 3 (time, - output, state) if ``return_x`` is ``True``. + results : `TimeResponseData` or `TimeResponseList` + Time response represented as a :class:`TimeResponseData` object or + list of :class:`TimeResponseData` objects. See + :func:`forced_response` for additional information. See Also -------- @@ -1757,11 +1825,22 @@ def initial_response(sys, T=None, X0=0, output=None, T_num=None, params=None, # Create the time and input vectors if T is None or np.asarray(T).size == 1: - T = _default_time_vector(sys, N=T_num, tfinal=T, is_step=False) + T = _default_time_vector(sysdata, N=T_num, tfinal=T, is_step=False) T = np.atleast_1d(T).reshape(-1) if T.ndim != 1 and len(T) < 2: raise ValueError("invalid value of T for this type of system") + # If passed a list, recursively call individual responses with given T + if isinstance(sysdata, (list, tuple)): + responses = [] + for sys in sysdata: + responses.append(initial_response( + sys, T, X0=X0, output=output, T_num=T_num, transpose=transpose, + return_x=return_x, squeeze=squeeze, params=params)) + return TimeResponseList(responses) + else: + sys = sysdata + # Compute the forced response response = forced_response(sys, T, 0, X0, params=params) @@ -1782,8 +1861,9 @@ def initial_response(sys, T=None, X0=0, output=None, T_num=None, params=None, transpose=transpose, return_x=return_x, squeeze=squeeze) -def impulse_response(sys, T=None, input=None, output=None, T_num=None, - transpose=False, return_x=False, squeeze=None): +def impulse_response( + sysdata, T=None, input=None, output=None, T_num=None, + transpose=False, return_x=False, squeeze=None): # pylint: disable=W0622 """Compute the impulse response for a linear system. @@ -1798,8 +1878,8 @@ def impulse_response(sys, T=None, input=None, output=None, T_num=None, Parameters ---------- - sys : StateSpace, TransferFunction - LTI system to simulate + sysdata : I/O system or list of I/O systems + I/O system(s) for which impluse response is computed. T : array_like or float, optional Time vector, or simulation time duration if a scalar (time vector is @@ -1838,24 +1918,10 @@ def impulse_response(sys, T=None, input=None, output=None, T_num=None, Returns ------- - results : TimeResponseData - Impulse response represented as a :class:`TimeResponseData` object - containing the following properties: - - * time (array): Time values of the output. - - * outputs (array): Response of the system. If the system is SISO and - squeeze is not True, the array is 1D (indexed by time). If the - system is not SISO or ``squeeze`` is False, the array is 3D (indexed - by the output, trace, and time). - - * states (array): Time evolution of the state vector, represented as - either a 2D array indexed by state and time (if SISO) or a 3D array - indexed by state, trace, and time. Not affected by ``squeeze``. - - The return value of the system can also be accessed by assigning the - function to a tuple of length 2 (time, output) or of length 3 (time, - output, state) if ``return_x`` is ``True``. + results : `TimeResponseData` or `TimeResponseList` + Time response represented as a :class:`TimeResponseData` object or + list of :class:`TimeResponseData` objects. See + :func:`forced_response` for additional information. See Also -------- @@ -1866,8 +1932,8 @@ def impulse_response(sys, T=None, input=None, output=None, T_num=None, This function uses the `forced_response` function to compute the time response. For continuous time systems, the initial condition is altered to account for the initial impulse. For discrete-time aystems, the - impulse is sized so that it has unit area. Response for nonlinear - systems is computed using `input_output_response`. + impulse is sized so that it has unit area. The impulse response for + nonlinear systems is not implemented. Examples -------- @@ -1875,20 +1941,31 @@ def impulse_response(sys, T=None, input=None, output=None, T_num=None, >>> T, yout = ct.impulse_response(G) """ - from .statesp import _convert_to_statespace from .lti import LTI - - # Make sure we have an LTI system - if not isinstance(sys, LTI): - raise ValueError("system must be LTI system for impulse response") + from .statesp import _convert_to_statespace # Create the time and input vectors if T is None or np.asarray(T).size == 1: - T = _default_time_vector(sys, N=T_num, tfinal=T, is_step=False) + T = _default_time_vector(sysdata, N=T_num, tfinal=T, is_step=False) T = np.atleast_1d(T).reshape(-1) if T.ndim != 1 and len(T) < 2: raise ValueError("invalid value of T for this type of system") + # If passed a list, recursively call individual responses with given T + if isinstance(sysdata, (list, tuple)): + responses = [] + for sys in sysdata: + responses.append(impulse_response( + sys, T, input=input, output=output, T_num=T_num, + transpose=transpose, return_x=return_x, squeeze=squeeze)) + return TimeResponseList(responses) + else: + sys = sysdata + + # Make sure we have an LTI system + if not isinstance(sys, LTI): + raise ValueError("system must be LTI system for impulse response") + # Convert to state space so that we can simulate if sys.nstates is None: sys = _convert_to_statespace(sys) @@ -1942,7 +2019,7 @@ def impulse_response(sys, T=None, input=None, output=None, T_num=None, yout[:, inpidx, :] = response.y if output is None \ else response.y[output] xout[:, inpidx, :] = response.x - uout[:, inpidx, :] = U[i] + uout[:, inpidx, :] = U if input is None else U[i] # Figure out if the system is SISO or not issiso = sys.issiso() or (input is not None and output is not None) @@ -2130,11 +2207,22 @@ def _ideal_tfinal_and_dt(sys, is_step=True): return tfinal, dt -def _default_time_vector(sys, N=None, tfinal=None, is_step=True): +def _default_time_vector(sysdata, N=None, tfinal=None, is_step=True): """Returns a time vector that has a reasonable number of points. if system is discrete-time, N is ignored """ from .lti import LTI + if isinstance(sysdata, (list, tuple)): + tfinal_max = N_max = 0 + for sys in sysdata: + timevec = _default_time_vector( + sys, N=N, tfinal=tfinal, is_step=is_step) + tfinal_max = max(tfinal_max, timevec[-1]) + N_max = max(N_max, timevec.size) + return np.linspace(0, tfinal_max, N_max, endpoint=True) + else: + sys = sysdata + # For non-LTI system, need tfinal if not isinstance(sys, LTI): if tfinal is None: diff --git a/control/xferfcn.py b/control/xferfcn.py index 099f64258..ee41cbd2b 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -47,6 +47,8 @@ """ +from collections.abc import Iterable + # External function declarations import numpy as np from numpy import angle, array, empty, finfo, ndarray, ones, \ @@ -96,7 +98,7 @@ class TransferFunction(LTI): time, positive number is discrete time with specified sampling time, None indicates unspecified timebase (either continuous or discrete time). - display_format: None, 'poly' or 'zpk' + display_format : None, 'poly' or 'zpk', optional Set the display format used in printing the TransferFunction object. Default behavior is polynomial display and can be changed by changing config.defaults['xferfcn.display_format']. @@ -758,7 +760,12 @@ def __pow__(self, other): return (TransferFunction([1], [1]) / self) * (self**(other + 1)) def __getitem__(self, key): + if not isinstance(key, Iterable) or len(key) != 2: + raise IOError('must provide indices of length 2 for transfer functions') + key1, key2 = key + if not isinstance(key1, (int, slice)) or not isinstance(key2, (int, slice)): + raise TypeError(f"system indices must be integers or slices") # pre-process if isinstance(key1, int): @@ -804,7 +811,7 @@ def __getitem__(self, key): num, den, self.dt, inputs=inputs, outputs=outputs, name=sysname) def freqresp(self, omega): - """(deprecated) Evaluate transfer function at complex frequencies. + """Evaluate transfer function at complex frequencies. .. deprecated::0.9.0 Method has been given the more pythonic name @@ -814,7 +821,7 @@ def freqresp(self, omega): warn("TransferFunction.freqresp(omega) will be removed in a " "future release of python-control; use " "sys.frequency_response(omega), or freqresp(sys, omega) in the " - "MATLAB compatibility module instead", DeprecationWarning) + "MATLAB compatibility module instead", FutureWarning) return self.frequency_response(omega) def poles(self): @@ -1241,7 +1248,7 @@ def dcgain(self, warn_infinite=False): -------- >>> G = ct.tf([1], [1, 4]) >>> G.dcgain() - 0.25 + np.float64(0.25) """ return self._dcgain(warn_infinite) @@ -1571,13 +1578,13 @@ def tf(*args, **kwargs): Parameters ---------- - sys: LTI (StateSpace or TransferFunction) + sys : LTI (StateSpace or TransferFunction) A linear system - num: array_like, or list of list of array_like + num : array_like, or list of list of array_like Polynomial coefficients of the numerator - den: array_like, or list of list of array_like + den : array_like, or list of list of array_like Polynomial coefficients of the denominator - display_format: None, 'poly' or 'zpk' + display_format : None, 'poly' or 'zpk' Set the display format used in printing the TransferFunction object. Default behavior is polynomial display and can be changed by changing config.defaults['xferfcn.display_format'].. @@ -1670,7 +1677,7 @@ def tf(*args, **kwargs): raise ValueError("Needs 1 or 2 arguments; received %i." % len(args)) -def zpk(zeros, poles, gain, *args, **kwargs): +def zpk(zeros, poles, gain, dt=None, **kwargs): """zpk(zeros, poles, gain[, dt]) Create a transfer function from zeros, poles, gain. @@ -1687,7 +1694,7 @@ def zpk(zeros, poles, gain, *args, **kwargs): zeros : array_like Array containing the location of zeros. poles : array_like - Array containing the location of zeros. + Array containing the location of poles. gain : float System gain dt : None, True or float, optional @@ -1704,7 +1711,7 @@ def zpk(zeros, poles, gain, *args, **kwargs): name : string, optional System name (used for specifying signals). If unspecified, a generic name is generated with a unique integer id. - display_format: None, 'poly' or 'zpk' + display_format : None, 'poly' or 'zpk', optional Set the display format used in printing the TransferFunction object. Default behavior is polynomial display and can be changed by changing config.defaults['xferfcn.display_format']. @@ -1725,7 +1732,7 @@ def zpk(zeros, poles, gain, *args, **kwargs): """ num, den = zpk2tf(zeros, poles, gain) - return TransferFunction(num, den, *args, **kwargs) + return TransferFunction(num, den, dt=dt, **kwargs) def ss2tf(*args, **kwargs): @@ -1748,16 +1755,18 @@ def ss2tf(*args, **kwargs): Parameters ---------- - sys: StateSpace + sys : StateSpace A linear system - A: array_like or string + A : array_like or string System matrix - B: array_like or string + B : array_like or string Control matrix - C: array_like or string + C : array_like or string Output matrix - D: array_like or string + D : array_like or string Feedthrough matrix + **kwargs : keyword arguments + Additional arguments passed to :func:`tf` (e.g., signal names) Returns ------- @@ -1832,7 +1841,7 @@ def tfdata(sys): Parameters ---------- - sys: LTI (StateSpace, or TransferFunction) + sys : LTI (StateSpace, or TransferFunction) LTI system whose data will be returned Returns @@ -1852,7 +1861,7 @@ def _clean_part(data): Parameters ---------- - data: numerator or denominator of a transfer function. + data : numerator or denominator of a transfer function. Returns ------- diff --git a/doc/Makefile b/doc/Makefile index dfd34f4f1..f1a54c3cc 100644 --- a/doc/Makefile +++ b/doc/Makefile @@ -17,7 +17,7 @@ help: # Rules to create figures FIGS = classes.pdf timeplot-mimo_step-default.png \ freqplot-siso_bode-default.png rlocus-siso_ctime-default.png \ - phaseplot-dampedosc-default.png + phaseplot-dampedosc-default.png ctrlplot-servomech.png classes.pdf: classes.fig fig2dev -Lpdf $< $@ @@ -33,6 +33,9 @@ rlocus-siso_ctime-default.png: ../control/tests/rlocus_test.py phaseplot-dampedosc-default.png: ../control/tests/phaseplot_test.py PYTHONPATH=.. python $< +ctrlplot-servomech.png: ../control/tests/ctrlplot_test.py + PYTHONPATH=.. python $< + # Catch-all target: route all unknown targets to Sphinx using the new # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). html pdf clean doctest: Makefile $(FIGS) diff --git a/doc/conf.py b/doc/conf.py index 7a45ba3f9..75981d630 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -103,9 +103,9 @@ # This config value contains the locations and names of other projects that # should be linked to in this documentation. intersphinx_mapping = \ - {'scipy': ('https://docs.scipy.org/doc/scipy/reference', None), + {'scipy': ('https://docs.scipy.org/doc/scipy', None), 'numpy': ('https://numpy.org/doc/stable', None), - 'matplotlib': ('https://matplotlib.org/', None), + 'matplotlib': ('https://matplotlib.org/stable/', None), } # If this is True, todo and todolist produce output, else they produce nothing. diff --git a/doc/control.rst b/doc/control.rst index 1b1b74069..1544f93d0 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -73,7 +73,6 @@ Time domain simulation input_output_response phase_plot step_response - TimeResponseData Control system analysis ======================= @@ -124,6 +123,7 @@ Control system synthesis lqr mixsyn place + place_varga rootlocus_pid_designer Model simplification tools @@ -131,11 +131,11 @@ Model simplification tools .. autosummary:: :toctree: generated/ - minreal - balred - hsvd - modred - era + minimal_realization + balanced_reduction + hankel_singular_values + model_reduction + eigensys_realization markov Nonlinear system support diff --git a/doc/conventions.rst b/doc/conventions.rst index 2844fd47a..21f3ab82b 100644 --- a/doc/conventions.rst +++ b/doc/conventions.rst @@ -61,20 +61,21 @@ Transfer functions can be manipulated using standard arithmetic operations as well as the :func:`feedback`, :func:`parallel`, and :func:`series` function. A full list of functions can be found in :ref:`function-ref`. -FRD (frequency response data) systems +Frequency response data (FRD) systems ------------------------------------- The :class:`FrequencyResponseData` (FRD) class is used to represent systems in frequency response data form. The main data members are `omega` and `fresp`, where `omega` is a 1D array with the frequency points of the response, and `fresp` is a 3D array, with -the first dimension corresponding to the output index of the FRD, the second -dimension corresponding to the input index, and the 3rd dimension +the first dimension corresponding to the output index of the system, the +second dimension corresponding to the input index, and the 3rd dimension corresponding to the frequency points in omega. -FRD systems have a somewhat more limited set of functions that are -available, although all of the standard algebraic manipulations can be -performed. +FRD systems can be created with the :func:`~control.frd` factory function. +Frequency response data systems have a somewhat more limited set of +functions that are available, although all of the standard algebraic +manipulations can be performed. The FRD class is also used as the return type for the :func:`frequency_response` function (and the equivalent method for the @@ -153,6 +154,17 @@ The :func:`forced_response` system is the most general and allows by the zero initial state response to be simulated as well as the response from a non-zero initial condition. +For linear time invariant (LTI) systems, the :func:`impulse_response`, +:func:`initial_response`, and :func:`step_response` functions will +automatically compute the time vector based on the poles and zeros of +the system. If a list of systems is passed, a common time vector will be +computed and a list of responses will be returned in the form of a +:class:`TimeResponseList` object. The :func:`forced_response` function can +also take a list of systems, to which a single common input is applied. +The :class:`TimeResponseList` object has a `plot()` method that will plot +each of the responses in turn, using a sequence of different colors with +appropriate titles and legends. + In addition the :func:`input_output_response` function, which handles simulation of nonlinear systems and interconnected systems, can be used. For an LTI system, results are generally more accurate using diff --git a/doc/ctrlplot-pole_zero_subplots.png b/doc/ctrlplot-pole_zero_subplots.png new file mode 100644 index 000000000..bc6158971 Binary files /dev/null and b/doc/ctrlplot-pole_zero_subplots.png differ diff --git a/doc/ctrlplot-servomech.png b/doc/ctrlplot-servomech.png new file mode 100644 index 000000000..ce02c2638 Binary files /dev/null and b/doc/ctrlplot-servomech.png differ diff --git a/doc/era_msd.py b/doc/era_msd.py new file mode 120000 index 000000000..0cf6a5282 --- /dev/null +++ b/doc/era_msd.py @@ -0,0 +1 @@ +../examples/era_msd.py \ No newline at end of file diff --git a/doc/era_msd.rst b/doc/era_msd.rst new file mode 100644 index 000000000..de702406e --- /dev/null +++ b/doc/era_msd.rst @@ -0,0 +1,15 @@ +ERA example, mass spring damper system +-------------------------------------- + +Code +.... +.. literalinclude:: era_msd.py + :language: python + :linenos: + + +Notes +..... + +1. The environment variable `PYCONTROL_TEST_EXAMPLES` is used for +testing to turn off plotting of the outputs.0 \ No newline at end of file diff --git a/doc/examples.rst b/doc/examples.rst index 21364157e..5c253e0a9 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -7,7 +7,7 @@ Examples The source code for the examples below are available in the `examples/` subdirectory of the source code distribution. They can also be accessed online -via the [python-control GitHub repository](https://github.com/python-control/python-control/tree/master/examples). +via the `python-control GitHub repository `_. Python scripts @@ -35,6 +35,8 @@ other sources. kincar-flatsys mrac_siso_mit mrac_siso_lyapunov + markov + era_msd Jupyter notebooks ================= @@ -60,3 +62,19 @@ online sources. simulating_discrete_nonlinear steering stochresp + +Google Colab Notebooks +====================== + +A collection of Jupyter notebooks are available on `Google Colab +`_, where they can be executed +through a web browser: + +* `Caltech CDS 110 Google Colab notebooks + `_: + Jupyter notebooks created by Richard Murray for CDS 110 (Analysis + and Design of Feedback Systems) at Caltech. + +Note: in order to execute the Jupyter notebooks in this collection, +you will need a Google account that has access to the Google +Colaboratory application. diff --git a/doc/freqplot-gangof4.png b/doc/freqplot-gangof4.png index 538284a0f..f911e7207 100644 Binary files a/doc/freqplot-gangof4.png and b/doc/freqplot-gangof4.png differ diff --git a/doc/freqplot-mimo_bode-default.png b/doc/freqplot-mimo_bode-default.png index 995203336..88a45071b 100644 Binary files a/doc/freqplot-mimo_bode-default.png and b/doc/freqplot-mimo_bode-default.png differ diff --git a/doc/freqplot-mimo_bode-magonly.png b/doc/freqplot-mimo_bode-magonly.png index 106620b95..7fd5538ed 100644 Binary files a/doc/freqplot-mimo_bode-magonly.png and b/doc/freqplot-mimo_bode-magonly.png differ diff --git a/doc/freqplot-mimo_svplot-default.png b/doc/freqplot-mimo_svplot-default.png index d64330e25..f546992cd 100644 Binary files a/doc/freqplot-mimo_svplot-default.png and b/doc/freqplot-mimo_svplot-default.png differ diff --git a/doc/freqplot-nyquist-custom.png b/doc/freqplot-nyquist-custom.png new file mode 100644 index 000000000..eff33135b Binary files /dev/null and b/doc/freqplot-nyquist-custom.png differ diff --git a/doc/freqplot-nyquist-default.png b/doc/freqplot-nyquist-default.png new file mode 100644 index 000000000..ce5215493 Binary files /dev/null and b/doc/freqplot-nyquist-default.png differ diff --git a/doc/freqplot-siso_bode-default.png b/doc/freqplot-siso_bode-default.png index 924de66f4..f4e3fc67e 100644 Binary files a/doc/freqplot-siso_bode-default.png and b/doc/freqplot-siso_bode-default.png differ diff --git a/doc/freqplot-siso_bode-omega.png b/doc/freqplot-siso_bode-omega.png new file mode 100644 index 000000000..7763d51bb Binary files /dev/null and b/doc/freqplot-siso_bode-omega.png differ diff --git a/doc/freqplot-siso_nichols-default.png b/doc/freqplot-siso_nichols-default.png index 687afdd51..d8eab3feb 100644 Binary files a/doc/freqplot-siso_nichols-default.png and b/doc/freqplot-siso_nichols-default.png differ diff --git a/doc/iosys.rst b/doc/iosys.rst index c0c2cca31..9008a0e56 100644 --- a/doc/iosys.rst +++ b/doc/iosys.rst @@ -25,11 +25,11 @@ a :class:`~control.StateSpace` linear system. Use the ss_sys = ct.linearize(io_sys, xeq, ueq) Input/output systems are automatically created for state space LTI systems -when using the :func:`ss` function. Nonlinear input/output systems can be -created using the :func:`~control.nlsys` function, which requires -the definition of an update function (for the right hand side of the -differential or different equation) and an output function (computes the -outputs from the state):: +when using the :func:`~control.ss` function. Nonlinear input/output +systems can be created using the :func:`~control.nlsys` function, which +requires the definition of an update function (for the right hand side of +the differential or different equation) and an output function (computes +the outputs from the state):: io_sys = ct.nlsys(updfcn, outfcn, inputs=M, outputs=P, states=N) @@ -191,7 +191,86 @@ Additional features =================== The I/O systems module has a number of other features that can be used to -simplify the creation of interconnected input/output systems. +simplify the creation and use of interconnected input/output systems. + +Vector elements processing +-------------------------- + +Several I/O system commands perform processing of vector elements +(such as initial states or input vectors) and broadcast these to the +proper shape. + +For static elements, such as the initial state in a simulation or the +nominal state and input for a linearization), the following processing +is done: + +* Scalars are automatically converted to a vector of the appropriate + size consisting of the scalar value. This is commonly used when + specifying the origin ('0') or a step input ('1'). + +* Lists of values are concatenated into a single vector. This is + often used when you have an interconnected system and you need to + specify the initial condition or input value for each subsystem + (e.g., [X1eq, X2eq, ...]). + +* Vector elements are zero padded to the required length. If you + specify only a portion of the values for states or inputs, the + remaining values are taken as zero. (If the final element in the + given vector is non-zero, a warning is issued.) + +Similar processing is done for input time series, used for the +:func:`~control.input_output_response` and +:func:`~control.forced_response` commands, with the following +additional feature: + +* Time series elements are broadcast to match the number of time points + specified. If a list of time series and static elements are given (as a + list), static elements are broadcast to the proper number of time points, + and the overall list of elements concatenated to provide the full input + vector. + +As an example, suppose we have an interconnected system consisting of three +subsystems, a controlled process, an estimator, and a (static) controller:: + + proc = ct.nlsys(..., + states=2, inputs=['u1', 'u2', 'd'], outputs='y') + estim = ct.nlsys(..., + states=2, inputs='y', outputs=['xhat[0]', 'xhat[1]') + ctrl = ct.nlsys(..., + states=0, inputs=['r', 'xhat[0]', 'xhat[1]'], outputs=['u1', 'u2']) + + clsys = ct.interconnect( + [proc, estim, ctrl], inputs=['r', 'd'], outputs=['y', 'u1', 'u2']) + +To linearize the system around the origin, we can utilize the scalar +processing feature of vector elements:: + + P = proc.linearize(0, 0) + +In this command, the states and the inputs are broadcast to the size of the +state and input vectors, respectively. + +If we want to linearize the closed loop system around a process state +``x0`` (with two elements) and an estimator state ``0`` (for both states), +we can use the list processing feature:: + + H = clsys.linearize([x0, 0], 0) + +Note that this also utilizes the zero-padding functionality, since the +second argument in the list ``[x0, 0]`` is a scalar and so the vector +``[x0, 0]`` only has three elements instead of the required four. + +To run an input/output simulation with a sinusoidal signal for the first +input, a constant for the second input, and no external disturbance, we can +use the list processing feature combined with time series broadcasting:: + + timepts = np.linspace(0, 10) + u1 = np.sin(timepts) + u2 = 1 + resp = ct.input_output_response(clsys, timepts, [u1, u2, 0]) + +In this command, the second and third arguments will be broadcast to match +the number of time points. Summing junction ---------------- @@ -355,8 +434,8 @@ of an individual system are used in a given specification:: inplist=['sum.r', 'P.v'], outlist=['P', 'C.u'] ) -And finally, since we have named the signals throughout the system in -a consistent way, we could let :func:`ct.interconnect` do all of the +And finally, since we have named the signals throughout the system in a +consistent way, we could let :func:`~control.interconnect` do all of the work:: clsys5 = ct.interconnect( diff --git a/doc/markov.py b/doc/markov.py new file mode 120000 index 000000000..471188252 --- /dev/null +++ b/doc/markov.py @@ -0,0 +1 @@ +../examples/markov.py \ No newline at end of file diff --git a/doc/markov.rst b/doc/markov.rst new file mode 100644 index 000000000..36e0fd8e5 --- /dev/null +++ b/doc/markov.rst @@ -0,0 +1,15 @@ +Estimation of Makrov parameters +------------------------------- + +Code +.... +.. literalinclude:: markov.py + :language: python + :linenos: + + +Notes +..... + +1. The environment variable `PYCONTROL_TEST_EXAMPLES` is used for +testing to turn off plotting of the outputs.0 \ No newline at end of file diff --git a/doc/optimal.rst b/doc/optimal.rst index 4df8d4861..bc37607e1 100644 --- a/doc/optimal.rst +++ b/doc/optimal.rst @@ -221,7 +221,7 @@ state and/or input, either along the trajectory and at the terminal time. The optimal control module operates by converting the optimal control problem into a standard optimization problem that can be solved by :func:`scipy.optimize.minimize`. The optimal control problem can be solved -by using the :func:`~control.obc.solve_ocp` function:: +by using the :func:`~control.optimal.solve_ocp` function:: res = obc.solve_ocp(sys, timepts, X0, cost, constraints) @@ -467,6 +467,10 @@ formulations. Module classes and functions ============================ + +The following classes and functions are defined in the +``optimal`` module: + .. autosummary:: :toctree: generated/ :template: custom-class-template.rst diff --git a/doc/phaseplot-dampedosc-default.png b/doc/phaseplot-dampedosc-default.png index da4e24e35..64ca7bd36 100644 Binary files a/doc/phaseplot-dampedosc-default.png and b/doc/phaseplot-dampedosc-default.png differ diff --git a/doc/phaseplot-invpend-meshgrid.png b/doc/phaseplot-invpend-meshgrid.png index 040b45558..eba45c153 100644 Binary files a/doc/phaseplot-invpend-meshgrid.png and b/doc/phaseplot-invpend-meshgrid.png differ diff --git a/doc/phaseplot-oscillator-helpers.png b/doc/phaseplot-oscillator-helpers.png index 0b5ebf43f..beceb948b 100644 Binary files a/doc/phaseplot-oscillator-helpers.png and b/doc/phaseplot-oscillator-helpers.png differ diff --git a/doc/phaseplots.py b/doc/phaseplots.py deleted file mode 120000 index 4b0575c0f..000000000 --- a/doc/phaseplots.py +++ /dev/null @@ -1 +0,0 @@ -../examples/phaseplots.py \ No newline at end of file diff --git a/doc/plotting.rst b/doc/plotting.rst index 8eb548a85..367e2c349 100644 --- a/doc/plotting.rst +++ b/doc/plotting.rst @@ -1,5 +1,7 @@ .. _plotting-module: +.. currentmodule:: control + ************* Plotting data ************* @@ -24,11 +26,12 @@ resulting in the following standard pattern:: response = ct.nyquist_response([sys1, sys2]) count = ct.response.count # number of encirclements of -1 - lines = ct.nyquist_plot(response) # Nyquist plot + cplt = ct.nyquist_plot(response) # Nyquist plot -The returned value `lines` provides access to the individual lines in the -generated plot, allowing various aspects of the plot to be modified to suit -specific needs. +Plotting commands return a :class:`~control.ControlPlot` object that +provides access to the individual lines in the generated plot using +`cplt.lines`, allowing various aspects of the plot to be modified to +suit specific needs. The plotting function is also available via the `plot()` method of the analysis object, allowing the following type of calls:: @@ -107,13 +110,13 @@ each other). The following plot shows the use of `plot_inputs='overlay'` as well as the ability to reposition the legends using the `legend_map` keyword:: - timepts = np.linspace(0, 10, 100) - U = np.vstack([np.sin(timepts), np.cos(2*timepts)]) - ct.input_output_response(sys_mimo, timepts, U).plot( - plot_inputs='overlay', - legend_map=np.array([['lower right'], ['lower right']]), - title="I/O response for 2x2 MIMO system " + - "[plot_inputs='overlay', legend_map]") + timepts = np.linspace(0, 10, 100) + U = np.vstack([np.sin(timepts), np.cos(2*timepts)]) + ct.input_output_response(sys_mimo, timepts, U).plot( + plot_inputs='overlay', + legend_map=np.array([['lower right'], ['lower right']]), + title="I/O response for 2x2 MIMO system " + + "[plot_inputs='overlay', legend_map]") .. image:: timeplot-mimo_ioresp-ov_lm.png @@ -122,17 +125,17 @@ instead of plotting the outputs on the top and inputs on the bottom, the inputs are plotted on the left and outputs on the right, as shown in the following figure:: - U1 = np.vstack([np.sin(timepts), np.cos(2*timepts)]) - resp1 = ct.input_output_response(sys_mimo, timepts, U1) + U1 = np.vstack([np.sin(timepts), np.cos(2*timepts)]) + resp1 = ct.input_output_response(sys_mimo, timepts, U1) - U2 = np.vstack([np.cos(2*timepts), np.sin(timepts)]) - resp2 = ct.input_output_response(sys_mimo, timepts, U2) + U2 = np.vstack([np.cos(2*timepts), np.sin(timepts)]) + resp2 = ct.input_output_response(sys_mimo, timepts, U2) - ct.combine_time_responses( - [resp1, resp2], trace_labels=["Scenario #1", "Scenario #2"]).plot( - transpose=True, - title="I/O responses for 2x2 MIMO system, multiple traces " - "[transpose]") + ct.combine_time_responses( + [resp1, resp2], trace_labels=["Scenario #1", "Scenario #2"]).plot( + transpose=True, + title="I/O responses for 2x2 MIMO system, multiple traces " + "[transpose]") .. image:: timeplot-mimo_ioresp-mt_tr.png @@ -146,11 +149,11 @@ Additional customization is possible using the `input_props`, `output_props`, and `trace_props` keywords to set complementary line colors and styles for various signals and traces:: - out = ct.step_response(sys_mimo).plot( - plot_inputs='overlay', overlay_signals=True, overlay_traces=True, - output_props=[{'color': c} for c in ['blue', 'orange']], - input_props=[{'color': c} for c in ['red', 'green']], - trace_props=[{'linestyle': s} for s in ['-', '--']]) + cplt = ct.step_response(sys_mimo).plot( + plot_inputs='overlay', overlay_signals=True, overlay_traces=True, + output_props=[{'color': c} for c in ['blue', 'orange']], + input_props=[{'color': c} for c in ['red', 'green']], + trace_props=[{'linestyle': s} for s in ['-', '--']]) .. image:: timeplot-mimo_step-linestyle.png @@ -196,7 +199,7 @@ overlaying the inputs or outputs:: .. image:: freqplot-mimo_bode-magonly.png -The :func:`~ct.singular_values_response` function can be used to +The :func:`~control.singular_values_response` function can be used to generate Bode plots that show the singular values of a transfer function:: @@ -212,17 +215,69 @@ plot, use `plot_type='nichols'`:: .. image:: freqplot-siso_nichols-default.png -Another response function that can be used to generate Bode plots is -the :func:`~ct.gangof4` function, which computes the four primary +Another response function that can be used to generate Bode plots is the +:func:`~control.gangof4_response` function, which computes the four primary sensitivity functions for a feedback control system in standard form:: - proc = ct.tf([1], [1, 1, 1], name="process") - ctrl = ct.tf([100], [1, 5], name="control") - response = rect.gangof4_response(proc, ctrl) - ct.bode_plot(response) # or response.plot() + proc = ct.tf([1], [1, 1, 1], name="process") + ctrl = ct.tf([100], [1, 5], name="control") + response = rect.gangof4_response(proc, ctrl) + ct.bode_plot(response) # or response.plot() .. image:: freqplot-gangof4.png +Nyquist analysis can be done using the :func:`~control.nyquist_response` +function, which evaluates an LTI system along the Nyquist contour, and +the :func:`~control.nyquist_plot` function, which generates a Nyquist plot:: + + sys = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys') + nyquist_plot(sys) + +.. image:: freqplot-nyquist-default.png + +The :func:`~control.nyquist_response` function can be used to compute +the number of encirclements of the -1 point and can return the Nyquist +contour that was used to generate the Nyquist curve. + +By default, the Nyquist response will generate small semicircles around +poles that are on the imaginary axis. In addition, portions of the Nyquist +curve that are far from the origin are scaled to a maximum value, while the +line style is changed to reflect the scaling, and it is possible to offset +the scaled portions to separate out the portions of the Nyquist curve at +:math:`\infty`. A number of keyword parameters for both are available for +:func:`~control.nyquist_response` and :func:`~control.nyquist_plot` to tune +the computation of the Nyquist curve and the way the data are plotted:: + + sys = ct.tf([1, 0.2], [1, 0, 1]) * ct.tf([1], [1, 0]) + nyqresp = ct.nyquist_response(sys) + nyqresp.plot( + max_curve_magnitude=6, max_curve_offset=1, + arrows=[0, 0.15, 0.3, 0.6, 0.7, 0.925], label='sys') + print("Encirclements =", nyqresp.count) + +.. image:: freqplot-nyquist-custom.png + +All frequency domain plotting functions will automatically compute the +range of frequencies to plot based on the poles and zeros of the frequency +response. Frequency points can be explicitly specified by including an +array of frequencies as a second argument (after the list of systems):: + + sys1 = ct.tf([1], [1, 2, 1], name='sys1') + sys2 = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys2') + omega = np.logspace(-2, 2, 500) + ct.frequency_response([sys1, sys2], omega).plot(initial_phase=0) + +.. image:: freqplot-siso_bode-omega.png + +Alternatively, frequency ranges can be specified by passing a list of the +form ``[wmin, wmax]``, where ``wmin`` and ``wmax`` are the minimum and +maximum frequencies in the (log-spaced) frequency range:: + + response = ct.frequency_response([sys1, sys2], [1e-2, 1e2]) + +The number of (log-spaced) points in the frequency can be specified using +the ``omega_num`` keyword parameter. + Pole/zero data ============== @@ -288,7 +343,7 @@ The default method for generating a phase plane plot is to provide a 2D dynamical system along with a range of coordinates and time limit:: sys = ct.nlsys( - lambda t, x, u, params: np.array([[0, 1], [-1, -1]]) @ x, + lambda t, x, u, params: np.array([[0, 1], [-1, -1]]) @ x, states=['position', 'velocity'], inputs=0, name='damped oscillator') axis_limits = [-1, 1, -1, 1] T = 8 @@ -310,7 +365,7 @@ an inverted pendulum system, which is created using a mesh grid:: m, l, b, g = params['m'], params['l'], params['b'], params['g'] return [x[1], -b/m * x[1] + (g * l / m) * np.sin(x[0]) + u[0]/m] invpend = ct.nlsys(invpend_update, states=2, inputs=1, name='invpend') - + ct.phase_plane_plot( invpend, [-2*pi, 2*pi, -2, 2], 5, gridtype='meshgrid', gridspec=[5, 8], arrows=3, @@ -318,7 +373,7 @@ an inverted pendulum system, which is created using a mesh grid:: params={'m': 1, 'l': 1, 'b': 0.2, 'g': 1}) plt.xlabel(r"$\theta$ [rad]") plt.ylabel(r"$\dot\theta$ [rad/sec]") - + .. image:: phaseplot-invpend-meshgrid.png This figure shows several features of more complex phase plane plots: @@ -341,7 +396,7 @@ are part of the :mod:`~control.phaseplot` (pp) module:: -x[0] + x[1] * (1 - x[0]**2 - x[1]**2)] oscillator = ct.nlsys( oscillator_update, states=2, inputs=0, name='nonlinear oscillator') - + ct.phase_plane_plot(oscillator, [-1.5, 1.5, -1.5, 1.5], 0.9) pp.streamlines( oscillator, np.array([[0, 0]]), 1.5, @@ -355,10 +410,10 @@ are part of the :mod:`~control.phaseplot` (pp) module:: The following helper functions are available: .. autosummary:: - ~control.phaseplot.equilpoints - ~control.phaseplot.separatrices - ~control.phaseplot.streamlines - ~control.phaseplot.vectorfield + phaseplot.equilpoints + phaseplot.separatrices + phaseplot.streamlines + phaseplot.vectorfield The :func:`~control.phase_plane_plot` function calls these helper functions based on the options it is passed. @@ -370,6 +425,198 @@ Instead, the plot is generated directly be a call to the :mod:`~control.phaseplot` helper functions. +Customizing control plots +========================= + +A set of common options are available to customize control plots in +various ways. The following general rules apply: + +* If a plotting function is called multiple times with data that generate + control plots with the same shape for the array of subplots, the new data + will be overlaid with the old data, with a change in color(s) for the + new data (chosen from the standard matplotlib color cycle). If not + overridden, the plot title and legends will be updated to reflect all + data shown on the plot. + +* If a plotting function is called and the shape for the array of subplots + does not match the currently displayed plot, a new figure is created. + Note that only the shape is checked, so if two different types of + plotting commands that generate the same shape of subplots are called + sequentially, the :func:`matplotlib.pyplot.figure` command should be used + to explicitly create a new figure. + +* The ``ax`` keyword argument can be used to direct the plotting function + to use a specific axes or array of axes. The value of the ``ax`` keyword + must have the proper number of axes for the plot (so a plot generating a + 2x2 array of subplots should be given a 2x2 array of axes for the ``ax`` + keyword). + +* The ``color``, ``linestyle``, ``linewidth``, and other matplotlib line + property arguments can be used to override the default line properties. + If these arguments are absent, the default matplotlib line properties are + used and the color cycles through the default matplotlib color cycle. + + The :func:`~control.bode_plot`, :func:`~control.time_response_plot`, + and selected other commands can also accept a matplotlib format + string (e.g., ``'r--'``). The format string must appear as a positional + argument right after the required data argument. + + Note that line property arguments are the same for all lines generated as + part of a single plotting command call, including when multiple responses + are passed as a list to the plotting command. For this reason it is + often easiest to call multiple plot commands in sequence, with each + command setting the line properties for that system/trace. + +* The ``label`` keyword argument can be used to override the line labels + that are used in generating the title and legend. If more than one line + is being plotted in a given call to a plot command, the ``label`` + argument value should be a list of labels, one for each line, in the + order they will appear in the legend. + + For input/output plots (frequency and time responses), the labels that + appear in the legend are of the form ", , , ". The trace name is used only for multi-trace time + plots (for example, step responses for MIMO systems). Common information + present in all traces is removed, so that the labels appearing in the + legend represent the unique characteristics of each line. + + For non-input/output plots (e.g., Nyquist plots, pole/zero plots, root + locus plots), the default labels are the system name. + + If ``label`` is set to ``False``, individual lines are still given + labels, but no legend is generated in the plot. (This can also be + accomplished by setting ``legend_map`` to ``False``). + + Note: the ``label`` keyword argument is not implemented for describing + function plots or phase plane plots, since these plots are primarily + intended to be for a single system. Standard ``matplotlib`` commands can + be used to customize these plots for displaying information for multiple + systems. + +* The ``legend_loc``, ``legend_map`` and ``show_legend`` keyword arguments + can be used to customize the locations for legends. By default, a + minimal number of legends are used such that lines can be uniquely + identified and no legend is generated if there is only one line in the + plot. Setting ``show_legend`` to ``False`` will suppress the legend and + setting it to ``True`` will force the legend to be displayed even if + there is only a single line in each axes. In addition, if the value of + the ``legend_loc`` keyword argument is set to a string or integer, it + will set the position of the legend as described in the + :func:`matplotlib.legend` documentation. Finally, ``legend_map`` can be + set to an array that matches the shape of the subplots, with each item + being a string indicating the location of the legend for that axes (or + ``None`` for no legend). + +* The ``rcParams`` keyword argument can be used to override the default + matplotlib style parameters used when creating a plot. The default + parameters for all control plots are given by the ``ct.rcParams`` + dictionary and have the following values: + + .. list-table:: + :widths: 50 50 + :header-rows: 1 + + * - Key + - Value + * - 'axes.labelsize' + - 'small' + * - 'axes.titlesize' + - 'small' + * - 'figure.titlesize' + - 'medium' + * - 'legend.fontsize' + - 'x-small' + * - 'xtick.labelsize' + - 'small' + * - 'ytick.labelsize' + - 'small' + + Only those values that should be changed from the default need to be + specified in the ``rcParams`` keyword argument. To override the defaults + for all control plots, update the ``ct.rcParams`` dictionary entries. + + The default values for style parameters for control plots can be restored + using :func:`~control.reset_rcParams`. + +* For multi-input, multi-output time and frequency domain plots, the + `sharex` and `sharey` keyword arguments can be used to determine whether + and how axis limits are shared between the individual subplots. Setting + the keyword to 'row' will share the axes limits across all subplots in a + row, 'col' will share across all subplots in a column, 'all' will share + across all subplots in the figure, and `False` will allow independent + limits for each subplot. + + For Bode plots, the `share_magnitude` and `share_phase` keyword arguments + can be used to independently control axis limit sharing for the magnitude + and phase portions of the plot, and `share_frequency` can be used instead + of `sharex`. + +* The ``title`` keyword can be used to override the automatic creation of + the plot title. The default title is a string of the form " plot + for " where is a list of the sys names contained in + the plot (which can be updated if the plotting is called multiple times). + Use ``title=False`` to suppress the title completely. The title can also + be updated using the :func:`~control.ControlPlot.set_plot_title` method + for the returned control plot object. + + The plot title is only generated if ``ax`` is ``None``. + +The following code illustrates the use of some of these customization +features:: + + P = ct.tf([0.02], [1, 0.1, 0.01]) # servomechanism + C1 = ct.tf([1, 1], [1, 0]) # unstable + L1 = P * C1 + C2 = ct.tf([1, 0.05], [1, 0]) # stable + L2 = P * C2 + + plt.rcParams.update(ct.rcParams) + fig = plt.figure(figsize=[7, 4]) + ax_mag = fig.add_subplot(2, 2, 1) + ax_phase = fig.add_subplot(2, 2, 3) + ax_nyquist = fig.add_subplot(1, 2, 2) + + ct.bode_plot( + [L1, L2], ax=[ax_mag, ax_phase], + label=["$L_1$ (unstable)", "$L_2$ (unstable)"], + show_legend=False) + ax_mag.set_title("Bode plot for $L_1$, $L_2$") + ax_mag.tick_params(labelbottom=False) + fig.align_labels() + + ct.nyquist_plot(L1, ax=ax_nyquist, label="$L_1$ (unstable)") + ct.nyquist_plot( + L2, ax=ax_nyquist, label="$L_2$ (stable)", + max_curve_magnitude=22, legend_loc='upper right') + ax_nyquist.set_title("Nyquist plot for $L_1$, $L_2$") + + fig.suptitle("Loop analysis for servomechanism control design") + plt.tight_layout() + +.. image:: ctrlplot-servomech.png + +As this example illustrates, python-control plotting functions and +Matplotlib plotting functions can generally be intermixed. One type of +plot for which this does not currently work is pole/zero plots with a +continuous time omega-damping grid (including root locus diagrams), due to +the way that axes grids are implemented. As a workaround, the +:func:`~control.pole_zero_subplots` command can be used to create an array +of subplots with different grid types, as illustrated in the following +example:: + + ax_array = ct.pole_zero_subplots(2, 1, grid=[True, False]) + sys1 = ct.tf([1, 2], [1, 2, 3], name='sys1') + sys2 = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys2') + ct.root_locus_plot([sys1, sys2], ax=ax_array[0, 0]) + cplt = ct.root_locus_plot([sys1, sys2], ax=ax_array[1, 0]) + cplt.set_plot_title("Root locus plots (w/ specified axes)") + +.. image:: ctrlplot-pole_zero_subplots.png + +Alternatively, turning off the omega-damping grid (using ``grid=False`` or +``grid='empty'``) allows use of Matplotlib layout commands. + + Response and plotting functions =============================== @@ -379,7 +626,7 @@ Response functions Response functions take a system or list of systems and return a response object that can be used to retrieve information about the system (e.g., the number of encirclements for a Nyquist plot) as well as plotting (via the -`plot` method). +``plot`` method). .. autosummary:: :toctree: generated/ @@ -406,11 +653,12 @@ Plotting functions ~control.bode_plot ~control.describing_function_plot ~control.nichols_plot + ~control.nyquist_plot ~control.phase_plane_plot - ~control.phaseplot.equilpoints - ~control.phaseplot.separatrices - ~control.phaseplot.streamlines - ~control.phaseplot.vectorfield + phaseplot.equilpoints + phaseplot.separatrices + phaseplot.streamlines + phaseplot.vectorfield ~control.pole_zero_plot ~control.root_locus_plot ~control.singular_values_plot @@ -419,27 +667,32 @@ Plotting functions Utility functions ----------------- - These additional functions can be used to manipulate response data or -returned values from plotting routines. +carry out other operations in creating control plots. + .. autosummary:: :toctree: generated/ + phaseplot.boxgrid ~control.combine_time_responses - ~control.get_plot_axes + ~control.pole_zero_subplots + ~control.reset_rcParams -Response classes ----------------- +Response and plotting classes +----------------------------- The following classes are used in generating response data. .. autosummary:: :toctree: generated/ + ~control.ControlPlot ~control.DescribingFunctionResponse ~control.FrequencyResponseData + ~control.FrequencyResponseList ~control.NyquistResponseData ~control.PoleZeroData ~control.TimeResponseData + ~control.TimeResponseList diff --git a/doc/pzmap-siso_ctime-default.png b/doc/pzmap-siso_ctime-default.png index 1caa7cadf..fd8b18eef 100644 Binary files a/doc/pzmap-siso_ctime-default.png and b/doc/pzmap-siso_ctime-default.png differ diff --git a/doc/rlocus-siso_ctime-clicked.png b/doc/rlocus-siso_ctime-clicked.png index dff339371..e7d7a1001 100644 Binary files a/doc/rlocus-siso_ctime-clicked.png and b/doc/rlocus-siso_ctime-clicked.png differ diff --git a/doc/rlocus-siso_ctime-default.png b/doc/rlocus-siso_ctime-default.png index 636951ed5..8a1984316 100644 Binary files a/doc/rlocus-siso_ctime-default.png and b/doc/rlocus-siso_ctime-default.png differ diff --git a/doc/rlocus-siso_dtime-default.png b/doc/rlocus-siso_dtime-default.png index 301778729..4ae6f09c6 100644 Binary files a/doc/rlocus-siso_dtime-default.png and b/doc/rlocus-siso_dtime-default.png differ diff --git a/doc/rlocus-siso_multiple-nogrid.png b/doc/rlocus-siso_multiple-nogrid.png index 07ece6505..9976ea81a 100644 Binary files a/doc/rlocus-siso_multiple-nogrid.png and b/doc/rlocus-siso_multiple-nogrid.png differ diff --git a/doc/timeplot-mimo_ioresp-mt_tr.png b/doc/timeplot-mimo_ioresp-mt_tr.png index e4c800086..7ddbe3c49 100644 Binary files a/doc/timeplot-mimo_ioresp-mt_tr.png and b/doc/timeplot-mimo_ioresp-mt_tr.png differ diff --git a/doc/timeplot-mimo_ioresp-ov_lm.png b/doc/timeplot-mimo_ioresp-ov_lm.png index 27dd89159..987a08f34 100644 Binary files a/doc/timeplot-mimo_ioresp-ov_lm.png and b/doc/timeplot-mimo_ioresp-ov_lm.png differ diff --git a/doc/timeplot-mimo_step-default.png b/doc/timeplot-mimo_step-default.png index 877764fbf..5850dcb87 100644 Binary files a/doc/timeplot-mimo_step-default.png and b/doc/timeplot-mimo_step-default.png differ diff --git a/doc/timeplot-mimo_step-linestyle.png b/doc/timeplot-mimo_step-linestyle.png index 9685ea6fa..1db7d2a4b 100644 Binary files a/doc/timeplot-mimo_step-linestyle.png and b/doc/timeplot-mimo_step-linestyle.png differ diff --git a/doc/timeplot-mimo_step-pi_cs.png b/doc/timeplot-mimo_step-pi_cs.png index 6046c8cce..44a630f55 100644 Binary files a/doc/timeplot-mimo_step-pi_cs.png and b/doc/timeplot-mimo_step-pi_cs.png differ diff --git a/examples/.gitignore b/examples/.gitignore new file mode 100644 index 000000000..ad3049346 --- /dev/null +++ b/examples/.gitignore @@ -0,0 +1 @@ +.ipynb-clean diff --git a/examples/Makefile b/examples/Makefile new file mode 100644 index 000000000..554e078ff --- /dev/null +++ b/examples/Makefile @@ -0,0 +1,29 @@ +# Makefile for python-control examples +# RMM, 6 Jul 2024 +# +# This makefile allows cleanup and posting of Jupyter notebooks into +# Google Colab. +# +# Files are copied to Google Colab using rclone. In order to copy files to +# Google Colab, you should edit the GDRIVE variable to use the name of the +# drive you have configured in rclone and the path where you want to place +# the files. The default location is set up for the fbsbook.org@gmail.com +# Google Drive account, currently maintained by Richard Murray. + +NOTEBOOKS = cds110-L*_*.ipynb cds112-L*_*.ipynb +GDRIVE= fbsbook-gdrive:python-control/public/notebooks + +# Clean up notebooks to remove output +clean: .ipynb-clean +.ipynb-clean: $(NOTEBOOKS) + @for i in $?; do \ + echo jupyter nbconvert --clear-output clear-metadata $$i; \ + jupyter nbconvert \ + --ClearMetadataPreprocessor.enabled=True \ + --clear-output $$i; \ + done + touch $@ + +# Post Jupyter notebooks on course website +post: .ipynb-clean + rclone copy . $(GDRIVE) --include /cds110-L\*_\*.ipynb diff --git a/examples/cds110-L1_servomech-python.ipynb b/examples/cds110-L1_servomech-python.ipynb new file mode 100644 index 000000000..a4e479492 --- /dev/null +++ b/examples/cds110-L1_servomech-python.ipynb @@ -0,0 +1,571 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "hairy-humidity", + "metadata": { + "id": "hairy-humidity" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 1

\n", + "

Dynamics and Control of a Servomechanism System using Python-Control

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1GKRYwtbHWSWc21EIYYIZUnbJqUorhY8w)\n", + "\n", + "In this lecture we show how to model an input/output system and design a controller for the system (using eigenvalue placement). This main intent of this lecture is to introduce the Python Control Systems Toolbox ([python-control](https://python-control.org)) and how it can be used to design a control system.\n", + "\n", + "We consider a class of control systems know as *servomechanisms*. Servermechanisms are mechanical systems that use feedback to provide high precision control of position and velocity. Some examples of servomechanisms are shown below:\n", + "\n", + "| | | |\n", + "| -- | -- | -- |\n", + "| Satellite Dish | Disk Drive | Robotics |\n", + "| \"Satellite | \"Disk | \"Disk\n", + "| [YouTube video](https://www.youtube.com/watch?v=HSGfE_sC2hw) | [YouTube video](https://www.youtube.com/watch?v=oQh8KDea6SI) | [YouTube video](https://www.youtube.com/watch?v=hg3TIFIxWCo)\n", + "| | |" + ] + }, + { + "cell_type": "markdown", + "id": "2c284896-bcff-4c06-b80d-d9d6fbc0690f", + "metadata": {}, + "source": [ + "The python-control toolbox can be installed using `pip` over from conda-forge. The code below will import the control toolbox either from your local installation or via pip. We use the prefix `ct` to access control toolbox commands:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "invalid-carnival", + "metadata": {}, + "outputs": [], + "source": [ + "# Import standard packages needed for this exercise\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct" + ] + }, + { + "cell_type": "markdown", + "id": "P7t3Nm4Tre2Z", + "metadata": { + "id": "P7t3Nm4Tre2Z" + }, + "source": [ + "## System dynamics\n", + "\n", + "Consider a simple mechanism consisting of a spring loaded arm that is driven by a motor, as shown below:\n", + "\n", + "
\"servomech-diagram\"
\n", + "\n", + "The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached.\n", + "\n", + "The equations of motion for the system are given by\n", + "\n", + "$$\n", + "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n", + "$$\n", + "\n", + "which can be written in state space form as\n", + "\n", + "$$\n", + "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\theta \\end{bmatrix} =\n", + " \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n", + " + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n", + "$$\n", + "\n", + "The system parameters are given by\n", + "\n", + "$$\n", + "k = 1,\\quad J = 100,\\quad b = 10,\n", + "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01.\n", + "$$\n", + "\n", + "and we assume that time is measured in milliseconds (ms) and distance in centimeters (cm). (The constants here are made up and don't necessarily reflect a real disk drive, though the units and time constants are motivated by computer disk drives.)" + ] + }, + { + "cell_type": "markdown", + "id": "3e476db9", + "metadata": { + "id": "3e476db9" + }, + "source": [ + "The system dynamics can be modeled in python-control using a `NonlinearIOSystem` object, which we create with the `nlsys` function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "27bb3c38", + "metadata": {}, + "outputs": [], + "source": [ + "# Parameter values\n", + "servomech_params = {\n", + " 'J': 100, # Moment of inertia of the motor\n", + " 'b': 10, # Angular damping of the arm\n", + " 'k': 1, # Spring constant\n", + " 'r': 1, # Location of spring contact on arm\n", + " 'l': 2, # Distance to the read head\n", + " 'eps': 0.01, # Magnitude of velocity-dependent perturbation\n", + "}\n", + "\n", + "# State derivative\n", + "def servomech_update(t, x, u, params):\n", + " # Extract the configuration and velocity variables from the state vector\n", + " theta = x[0] # Angular position of the disk drive arm\n", + " thetadot = x[1] # Angular velocity of the disk drive arm\n", + " tau = u[0] # Torque applied at the base of the arm\n", + "\n", + " # Get the parameter values\n", + " J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])\n", + "\n", + " # Compute the angular acceleration\n", + " dthetadot = 1/J * (\n", + " -b * thetadot - k * r * np.sin(theta) + tau)\n", + "\n", + " # Return the state update law\n", + " return np.array([thetadot, dthetadot])\n", + "\n", + "# System output (tip radial position + angular velocity)\n", + "def servomech_output(t, x, u, params):\n", + " l = params['l']\n", + " return np.array([l * x[0], x[1]])\n", + "\n", + "# System dynamics\n", + "servomech = ct.nlsys(\n", + " servomech_update, servomech_output, name='servomech',\n", + " params=servomech_params, states=['theta_', 'thdot_'],\n", + " outputs=['y', 'thdot'], inputs=['tau'])\n", + "\n", + "print(servomech)\n", + "print(\"\\nParams:\", servomech.params)" + ] + }, + { + "cell_type": "markdown", + "id": "competitive-terrain", + "metadata": { + "id": "competitive-terrain" + }, + "source": [ + "### Linearization\n", + "\n", + "To study the open loop dynamics of the system, we compute the linearization of the dynamics about the equilibrium point corresponding to $\\theta_\\text{e} = 15^\\circ$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "senior-carpet", + "metadata": {}, + "outputs": [], + "source": [ + "# Convert the equilibrium angle to radians\n", + "theta_e = (15 / 180) * np.pi\n", + "\n", + "# Compute the input required to hold this position\n", + "u_e = servomech.params['k'] * servomech.params['r'] * np.sin(theta_e)\n", + "print(\"Equilibrium torque = %g\" % u_e)\n", + "\n", + "# Linearize the system about the equilibrium point\n", + "P = servomech.linearize([theta_e, 0], u_e)[0, 0]\n", + "# P.update_names(name='linservo')\n", + "print(\"Linearized dynamics:\\n\", P)" + ] + }, + { + "cell_type": "markdown", + "id": "qGtb17lO4PvM", + "metadata": { + "id": "qGtb17lO4PvM" + }, + "source": [ + "We can check the roots of the characteristic equation for this second order system using the `poles` method (we will learn how this works later in the term):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "Vkji0Y8FT7oq", + "metadata": {}, + "outputs": [], + "source": [ + "# Check the stability of the equilibrium point\n", + "P.poles()" + ] + }, + { + "cell_type": "markdown", + "id": "naH-Nl7V4c2R", + "metadata": { + "id": "naH-Nl7V4c2R" + }, + "source": [ + "Alternatively, we can look at the eigenvalues of the \"dynamics matrix\" for the linearized system (we will learn about this formulation in [Lecture 3](cds110-L3_lti-systems.ipynb)):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "aKxayyiK4NLj", + "metadata": {}, + "outputs": [], + "source": [ + "evals, evecs = np.linalg.eig(P.A)\n", + "print(evals)" + ] + }, + { + "cell_type": "markdown", + "id": "AYQlD5v9GcK4", + "metadata": { + "id": "AYQlD5v9GcK4" + }, + "source": [ + "Both approaches give the same result and we see that the system is stable (negative real part) with an imaginary component (so we can expect some oscillation in the response)." + ] + }, + { + "cell_type": "markdown", + "id": "instant-lancaster", + "metadata": { + "id": "instant-lancaster" + }, + "source": [ + "### Open loop step response\n", + "\n", + "A standard method for understanding the dynamics is to plot output of the system in response to an input that is set to 1 at time $t = 0$ (called the \"step response\").\n", + "\n", + "We use the `step_response` function to plot the step response of the linearized, open-loop system and compute the \"rise time\" and \"settling time\" (we will define these more formally next week)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "african-mauritius", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the step response\n", + "lin_response = ct.step_response(P)\n", + "timepts, output = lin_response.time, lin_response.outputs\n", + "\n", + "# Plot step response (input 0 to output 0)\n", + "plt.plot(timepts, output)\n", + "plt.xlabel(\"Time $t$ [ms]\")\n", + "plt.ylabel(\"Position $y$ [cm]\")\n", + "plt.title(\"Step response for the linearized, open-loop system\")\n", + "\n", + "# Compute and print properties of the step response\n", + "results = ct.step_info(P)\n", + "print(\"Rise time:\", results['RiseTime']) # 10-90% rise time\n", + "print(\"Settling time:\", results['SettlingTime']) # 2% error\n", + "\n", + "# Calculate the rise time start time by hand\n", + "rise_time_start = timepts[np.where(output > 0.1 * output[-1])[0][0]]\n", + "rise_time_stop = rise_time_start + results['RiseTime']\n", + "\n", + "# Add lines for the step response features\n", + "plt.plot([timepts[0], timepts[-1]], [output[-1], output[-1]], 'k--')\n", + "\n", + "plt.plot([rise_time_start, rise_time_start], [0, 2.5], 'k:')\n", + "plt.plot([rise_time_stop, rise_time_stop], [0, 2.5], 'k:')\n", + "plt.arrow(rise_time_start, 0.5, rise_time_stop - rise_time_start, 0)\n", + "plt.text((rise_time_start + rise_time_stop)/2, 0.6, '$T_r$')\n", + "\n", + "plt.plot([0, 0], [0, 2.5], 'k:')\n", + "plt.plot([results['SettlingTime'], results['SettlingTime']], [0, 2.5], 'k:')\n", + "plt.arrow(0, 1.5, results['SettlingTime'], 0)\n", + "plt.text(results['SettlingTime']/2, 1.6, '$T_s$');\n" + ] + }, + { + "cell_type": "markdown", + "id": "DoCK6MWlHaUO", + "metadata": { + "id": "DoCK6MWlHaUO" + }, + "source": [ + "We see that the open loop step response (for the linearized system) is stable, and that the final value is larger than 1 (this value just depends on the parameters in the system)." + ] + }, + { + "cell_type": "markdown", + "id": "nviDlWek9dge", + "metadata": { + "id": "nviDlWek9dge" + }, + "source": [ + "We can also compare the response of the linearized system to the full nonlinear system:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "qwrPhD499jbl", + "metadata": {}, + "outputs": [], + "source": [ + "nl_response = ct.input_output_response(servomech, timepts, U=1)\n", + "\n", + "# Plot step response (input 0 to output 0)\n", + "plt.plot(timepts, output, label=\"linearized\")\n", + "plt.plot(timepts, nl_response.outputs[0], label=\"nonlinear\")\n", + "\n", + "plt.xlabel(\"Time $t$ [ms]\")\n", + "plt.ylabel(\"Position $y$ [cm]\")\n", + "plt.title(\"Step response for the open-loop system\")\n", + "plt.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "7YNmgE2XHmL3", + "metadata": { + "id": "7YNmgE2XHmL3" + }, + "source": [ + "We see that the nonlinear system responds differently. This is because the force exerted by the spring is nonlinear due to the kinematics of the mechanism design." + ] + }, + { + "cell_type": "markdown", + "id": "stuffed-premiere", + "metadata": { + "id": "stuffed-premiere" + }, + "source": [ + "## Feedback control design\n", + "\n", + "We next design a feedback controller for the system that allows the system to track a desired position $y_\\text{d}$ and sets the closed loop eigenvalues of the linearized system to $\\lambda_{1,2} = −10 \\pm 10 i$. We will learn how to do this more formally in later lectures, so if you aren't familiar with these techniques, that's OK.\n", + "\n", + "We make use of full state feedback of the form $u = -K(x - x_\\text{d})$ where $x_\\text{d}$ is the desired state of the system. The python-control `place` command can be used to compute the state feedback gains $K$ that set the closed loop poles at a desired location:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8NK8O6XT7B_a", + "metadata": {}, + "outputs": [], + "source": [ + "# Place the closed loop poles using feedback\n", + "# u = -K (x - xd)\n", + "\n", + "# Find the gains required to place the gains at the desired location\n", + "K = ct.place(P.A, P.B, [-10 + 10*1j, -10 - 10*1j])\n", + "print(f\"{K=}\\n\")\n", + "\n", + "# Implement an I/O system implementing this control law\n", + "def statefbk_output(t, x, u, params):\n", + " l = params.get('l', 2)\n", + " # Create the current and desired state\n", + " x = np.array([u[0] / l, u[1]])\n", + " xd = np.array([u[2] / l, u[3]])\n", + " return -K @ (x - xd)\n", + "\n", + "statefbk = ct.nlsys(\n", + " None, statefbk_output, name='statefbk',\n", + " inputs=['y', 'thdot', 'y_d', 'thdot_d'],\n", + " outputs=['tau']\n", + ")\n", + "print(statefbk)" + ] + }, + { + "cell_type": "markdown", + "id": "v1fb1pJ_zRLk", + "metadata": { + "id": "v1fb1pJ_zRLk" + }, + "source": [ + "Note that this controller has no internal state, but rather is a static input/output function." + ] + }, + { + "cell_type": "markdown", + "id": "ZR8EKtn-H9V7", + "metadata": { + "id": "ZR8EKtn-H9V7" + }, + "source": [ + "We can now connect the controller to the process using the `interconnect` command. Because we have named the signals in a careful way, the `interconnect` command can automatically connect everything together:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "associate-assistant", + "metadata": {}, + "outputs": [], + "source": [ + "clsys = ct.interconnect(\n", + " [servomech, statefbk],\n", + " inputs=['y_d', 'thdot_d'],\n", + " outputs=['y', 'tau']\n", + ")\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "id": "4o5oy_6N51yf", + "metadata": { + "id": "4o5oy_6N51yf" + }, + "source": [ + "To examine the dynamics of the closed loop system, we plot the step response for the closed loop system and compute the rise time, settling time, and steady state error." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "qIEH3Trn53d4", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the step response of the closed loop system\n", + "timepts = np.linspace(0, 1)\n", + "clsys_resp = ct.input_output_response(clsys, timepts, [1, 0])\n", + "\n", + "plt.plot(clsys_resp.time, clsys_resp.outputs[0])\n", + "plt.xlabel(\"Time $t$ [ms]\")\n", + "plt.ylabel(\"Position $y$ [cm]\")\n", + "plt.title(\"Step response for closed loop, state space controller\")\n", + "\n", + "# Compute and print properties of the step response\n", + "results = ct.step_info(clsys_resp.outputs[0], timepts)\n", + "print(\"\")\n", + "print(f\"Rise time: {results['RiseTime']:.2g} ms\")\n", + "print(f\"Settling time: {results['SettlingTime']:.2g} ms\")\n", + "print(f\"Steady state error: {abs(results['SteadyStateValue'] - 1) * 100:.2g}%\")" + ] + }, + { + "cell_type": "markdown", + "id": "K-ZX_SDmN4rF", + "metadata": { + "id": "K-ZX_SDmN4rF" + }, + "source": [ + "Note the change in timescale (100 ms to 1 ms) and also the fact that the system now goes to the reference value ($y = 1$)." + ] + }, + { + "cell_type": "markdown", + "id": "e0176710", + "metadata": { + "id": "e0176710" + }, + "source": [ + "## Frequency response\n", + "\n", + "Another way to measure the performance of the system is to compute its frequency response.\n", + "\n", + "Roughly speaking, we set the input of the system to be of the form $u(t) = \\sin(\\omega t)$ and then look at the output signal $y(t)$. For a *linear* system, we can show that the output signal will have the form\n", + "\n", + "$$\n", + "y(t) = M \\sin(\\omega t + \\phi)\n", + "$$\n", + "\n", + "where the magnitude $M$ and phase $\\phi$ depend on the input frequency.\n", + "\n", + "We can plot the magnitude (also called the \"gain\") and the phase of the system as a function of the frequency $\\omega$ and plot these values on a log-log and log-linear scale (called a *Bode* plot):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a8684cc1", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the linearization of the closed loop system\n", + "G = clsys.linearize([theta_e, 0], [0, 0], name=\"G\")\n", + "\n", + "# Plot the Bode plot (input[0] = yd, outut[0] = y)\n", + "response = ct.frequency_response(G[0, 0])\n", + "cplt = response.plot(title=\"Bode plot for G\", freq_label=\"Frequency [rad/ms]\")" + ] + }, + { + "cell_type": "markdown", + "id": "W_kzSIKGsSka", + "metadata": { + "id": "W_kzSIKGsSka" + }, + "source": [ + "Examination of the frequency response allows us to identify the range of input frequencies over which the control system can accurately track the input ($M(\\omega) \\approx 1$). For this system, we have good tracking up to approximately 10 rad/ms, which corresponds to about 1.6 kHz." + ] + }, + { + "cell_type": "markdown", + "id": "rocky-hobby", + "metadata": { + "id": "rocky-hobby" + }, + "source": [ + "## Trajectory tracking\n", + "\n", + "Another type of analysis we might do is to see how well the system can track a more complicated reference trajectory. For the disk drive example, we might move the system from one point on the disk to a second and then to a third (as we read different portions of the disk).\n", + "\n", + "To explore this, we can create simulations of the full nonlinear system with the linear controllers designed above and plot the response of the system. We do that here for a reference trajectory that has an initial value of 0 cm at $t = 0$, to 1 cm at $t = 0.5$, to 3 cm at $t = 1$, back to 2 cm at $t = 1.5$ ms:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "utility-community", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a reference trajectory to track\n", + "timepts = np.linspace(0, 2.5, 250)\n", + "ref = [\n", + " np.concatenate((\n", + " np.ones(50) * 0,\n", + " np.ones(50) * 1,\n", + " np.ones(50) * 3,\n", + " np.ones(100) * 2,\n", + " )), 0]\n", + "\n", + "# Create the system response and plot the results\n", + "response = ct.input_output_response(clsys, timepts, ref)\n", + "plt.plot(response.time, response.outputs[0])\n", + "\n", + "# Plot the reference trajectory\n", + "plt.plot(timepts, ref[0], 'k--');\n", + "\n", + "# Label the plot\n", + "plt.xlabel(\"Time $t$ [ms]\")\n", + "plt.ylabel(\"Position $y$ [cm]\")\n", + "plt.title(\"Trajectory tracking with full nonlinear dynamics\");" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "074427a3", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L2_invpend-dynamics.ipynb b/examples/cds110-L2_invpend-dynamics.ipynb new file mode 100644 index 000000000..5b1bfc099 --- /dev/null +++ b/examples/cds110-L2_invpend-dynamics.ipynb @@ -0,0 +1,433 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "t0JD8EbaVWg-" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 2

\n", + "

Nonlinear Dynamics (and Control) of an Inverted Pendulum System

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1is083NiFdHcHX8Hq56oh_AO35nQGO4bh)\n", + "\n", + "In this lecture we investigate the nonlinear dynamics of an inverted pendulum system. More information on this example can be found in [FBS2e](https://fbswiki.org/wiki/index.php?title=FBS), Examples 3.3 and 5.4. This lecture demonstrates how to use [python-control](https://python-control.org) to analyze nonlinear systems, including creating phase plane plots.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Import the packages needed for the examples included in this notebook\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "from math import pi\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "P_ZMCccjvHY1" + }, + "source": [ + "## System model" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Msad1ficHjtc" + }, + "source": [ + "We consider an invereted pendulum, which is a simplified version of a balance system:\n", + "\n", + "
\"invpend.diagram\"
\n", + "\n", + "The dynamics for an inverted pendulum system can be written as:\n", + "\n", + "$$\n", + " \\dfrac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\dot\\theta\\end{bmatrix} =\n", + " \\begin{bmatrix}\n", + " \\dot\\theta \\\\\n", + " \\dfrac{m g l}{J_\\text{t}} \\sin \\theta\n", + " - \\dfrac{b}{J_\\text{t}} \\dot\\theta\n", + " + \\dfrac{l}{J_\\text{t}} u \\cos\\theta\n", + " \\end{bmatrix}, \\qquad\n", + " y = \\theta,\n", + "$$\n", + "\n", + "where $m$ and $J_t = J + m l^2$ are the mass and (total) moment of inertia of the system to be balanced, $l$ is the distance from the base to the center of mass of the balanced body, $b$ is the coefficient of rotational friction, and $g$ is the acceleration due to gravity.\n", + "\n", + "We begin by creating a nonlinear model of the system:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "invpend_params = {'m': 1, 'l': 1, 'b': 0.5, 'g': 1}\n", + "def invpend_update(t, x, u, params):\n", + " m, l, b, g = params['m'], params['l'], params['b'], params['g']\n", + " umax = params.get('umax', 1)\n", + " usat = np.clip(u[0], -umax, umax)\n", + " return [x[1], -b/m * x[1] + (g * l / m) * np.sin(x[0] + usat/m)]\n", + "invpend = ct.nlsys(\n", + " invpend_update, states=['theta', 'thdot'],\n", + " inputs=['tau'], outputs=['theta', 'thdot'],\n", + " params=invpend_params, name='invpend')\n", + "print(invpend)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "IAoQAORFvLj1" + }, + "source": [ + "## Open loop dynamics" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vOALp_IwjVxC" + }, + "source": [ + "The open loop dynamics of the system can be visualized using the `phase_plane_plot` command in python-control:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ct.phase_plane_plot(\n", + " invpend, [-2*pi - 1, 2*pi + 1, -2, 2], 8),\n", + "\n", + "# Draw lines at the downward equilibrium angles\n", + "plt.plot([-pi, -pi], [-2, 2], 'k--')\n", + "plt.plot([pi, pi], [-2, 2], 'k--')" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "WZuvqNzeJinm" + }, + "source": [ + "We see that the vertical ($\\theta = 0$) equilibrium point is unstable, but the downward equlibrium points ($\\theta = \\pm \\pi$) are stable.\n", + "\n", + "Note also the *separatrices* for the equilibrium point, which gives insights into the regions of attraction (the red dashed line separates the two regions of attraction)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "2JibDTJBKHIF" + }, + "source": [ + "## Proportional feedback\n", + "\n", + "We now stabilize the system using a simple proportional feedback controller:\n", + "\n", + "$$u = -k_\\text{p} \\theta.$$\n", + "\n", + "This controller can be designed as an input/output system that has no state dynamics, just a mapping from the inputs to the outputs:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the controller\n", + "def propctrl_output(t, x, u, params):\n", + " kp = params.get('kp', 1)\n", + " return -kp * (u[0] - u[1])\n", + "propctrl = ct.nlsys(\n", + " None, propctrl_output, name=\"p_ctrl\",\n", + " inputs=['theta', 'r'], outputs='tau'\n", + ")\n", + "print(propctrl)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "AvU35WoBMFjt" + }, + "source": [ + "Note that the input to the controller is the reference value $r$ (which we will always take to be zero), the measured output $y$, which is the angle $\\theta$ for our system. The output of the controller is the system input $u$, corresponding to the force applied to the wheels.\n", + "\n", + "To connect the controller to the system, we use the [`interconnect`](https://python-control.readthedocs.io/en/latest/generated/control.interconnect.html) function, which will connect all signals that have the same names:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Create the closed loop system\n", + "clsys = ct.interconnect(\n", + " [invpend, propctrl], name='invpend w/ proportional feedback',\n", + " inputs=['r'], outputs=['theta', 'tau'], params={'kp': 1})\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "IIiSaHNuM1u_" + }, + "source": [ + "Note: you will see a warning when you run this command, because the output $\\dot\\theta$ (`thdot`) is not connected to anything. You can ignore this here, but as you get to more complicated examples, you should pay attention to warnings of this sort and make sure they are OK." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can now linearize the closed loop system at different gains and compute the eigenvalues to check for stability:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Solution\n", + "for kp in [0, 1, 10]:\n", + " print(\"kp = \", kp, \"; poles = \", clsys.linearize([0, 0], [0], params={'kp': kp}).poles())" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "iV4u31DsNWP9" + }, + "source": [ + "We see that at $k_\\text{p} = 10$ the eigenvalues (poles) of the closed loop system both have negative real part, and so the system is stabilized." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Jg87a3iZP-Qd" + }, + "source": [ + "### Phase portrait\n", + "\n", + "To study the resulting dynamics, we try plotting a phase plot using the same commands as before, but now for the closed loop system (with appropriate proportional gain):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ct.phase_plane_plot(\n", + " clsys, [-2*pi, 2*pi, -2, 2], 8, params={'kp': 10});" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "jhU2gidqi-ri" + }, + "source": [ + "This plot is not very useful and has several errors. It shows the limitations of the default parameter values for the `phase_plane_plot` command.\n", + "\n", + "Some things to notice in this plot:\n", + "* Not all of the equilibrium points are showing up (there are two unstable equilibrium points that are missing)\n", + "* There is no detail about what is happening near the origin." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Improved phase portrait\n", + "\n", + "To fix these issues, we can do a couple of things:\n", + "* Restrict the range of the plot from $-3\\pi/2$ to $3\\pi/2$, which means that grid used to calculate the equilibrium point is a bit finer.\n", + "* Reset the grid spacing, so that we have more initial conditions around the edge of the plot and a finer search for equilibrium points.\n", + "\n", + "Here's some improved code:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "kp_params = {'kp': 10}\n", + "ct.phase_plane_plot(\n", + " clsys, [-1.5 * pi, 1.5 * pi, -2, 2], 8,\n", + " gridspec=[13, 7], params=kp_params,\n", + " plot_separatrices={'timedata': 5})\n", + "plt.plot([-pi, -pi], [-2, 2], 'k--', [ pi, pi], [-2, 2], 'k--')\n", + "plt.plot([-pi/2, -pi/2], [-2, 2], 'k:', [ pi/2, pi/2], [-2, 2], 'k:');" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Play around with some paramters to see what happens\n", + "fig, axs = plt.subplots(2, 2)\n", + "for i, kp in enumerate([3, 10]):\n", + " for j, umax in enumerate([0.2, 1]):\n", + " ct.phase_plane_plot(\n", + " clsys, [-1.5 * pi, 1.5 * pi, -2, 2], 8,\n", + " gridspec=[13, 7], plot_separatrices={'timedata': 5},\n", + " params={'kp': kp, 'umax': umax}, ax=axs[i, j])\n", + " axs[i, j].set_title(f\"{kp=}, {umax=}\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "dYeVbfG4kU-9" + }, + "source": [ + "## State space controller\n", + "\n", + "For the proportional controller, we have limited control over the dynamics of the closed loop system. For example, we see that the solutions near the origin are highly oscillatory in both the $k_\\text{p} = 3$ and $k_\\text{p} = 10$ cases.\n", + "\n", + "An alternative is to use \"full state feedback\", in which we set\n", + "\n", + "$$\n", + "u = -K (x - x_\\text{d}) = -k_1 (\\theta - \\theta_d) - k_2 (\\dot\\theta - \\dot\\theta_d).\n", + "$$\n", + "\n", + "We will learn more about how to design these controllers later, so if you aren't familiar with the idea of eigenvalue placement, just take this as a bit of \"control theory magic\" for now.\n", + "\n", + "To compute the gains, we make use of the `place` command, applied to the linearized system:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Linearize the system\n", + "P = invpend.linearize([0, 0], [0])\n", + "\n", + "# Place the closed loop eigenvalues (poles) at desired locations\n", + "K = ct.place(P.A, P.B, [-1 + 0.1j, -1 - 0.1j])\n", + "print(f\"{K=}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def statefbk_output(t, x, u, params):\n", + " K = params.get('K', np.array([0, 0]))\n", + " return -K @ (u[0:2] - u[2:])\n", + "statefbk = ct.nlsys(\n", + " None, statefbk_output, name=\"k_ctrl\",\n", + " inputs=['theta', 'thdot', 'theta_d', 'thdot_d'], outputs='tau'\n", + ")\n", + "print(statefbk)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "clsys_sf = ct.interconnect(\n", + " [invpend, statefbk], name='invpend w/ state feedback',\n", + " inputs=['theta_d', 'thdot_d'], outputs=['theta', 'tau'], params={'kp': 1})\n", + "print(clsys_sf)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "aGm3usQIvmqN" + }, + "source": [ + "### Phase portrait" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ct.phase_plane_plot(\n", + " clsys_sf, [-1.5 * pi, 1.5 * pi, -2, 2], 8,\n", + " gridspec=[13, 7], params={'K': K})\n", + "plt.plot([-pi, -pi], [-2, 2], 'k--', [ pi, pi], [-2, 2], 'k--')\n", + "plt.plot([-pi/2, -pi/2], [-2, 2], 'k:', [ pi/2, pi/2], [-2, 2], 'k:')" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "A7UNUtfJwLWQ" + }, + "source": [ + "Note that the closed loop response around the upright equilibrium point is much less oscillatory (consistent with where we placed the closed loop eigenvalues of the system dynamics)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "eVSa1Mvqycov" + }, + "source": [ + "## Things to try\n", + "\n", + "Here are some things to try with the above code:\n", + "* Try changing the locations of the closed loop eigenvalues in the `place` command\n", + "* Try resetting the limits of the control action (`umax`)\n", + "* Try leaving the state space controller fixed but changing the parameters of the system dynamics ($m$, $l$, $b$). Does the controller still stabilize the system?\n", + "* Plot the initial condition response of the system and see how to map time traces to phase plot traces." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/cds110-L3_lti-systems.ipynb b/examples/cds110-L3_lti-systems.ipynb new file mode 100644 index 000000000..652bb1216 --- /dev/null +++ b/examples/cds110-L3_lti-systems.ipynb @@ -0,0 +1,515 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "gQZtf4ZqM8HL" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 3

\n", + "

Python Tools for Analyzing Linear Systems

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/164yYvB86c2EvEcIHpUPNXCroiN9nnTAa)\n", + "\n", + "In this lecture we describe tools in the Python Control Systems Toolbox ([python-control](https://python-control.org)) that can be used to analyze linear systems, including some of the options available to present the information in different ways.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": { + "id": "qMVGK15gNQw2" + }, + "source": [ + "## Coupled mass spring system\n", + "\n", + "Consider the spring mass system below:\n", + "\n", + "
\n", + "\n", + "We wish to analyze the time and frequency response of this system using a variety of python-control functions for linear systems analysis.\n", + "\n", + "### System dynamics\n", + "\n", + "The dynamics of the system can be written as\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + " m \\ddot{q}_1 &= -2 k q_1 - c \\dot{q}_1 + k q_2, \\\\\n", + " m \\ddot{q}_2 &= k q_1 - 2 k q_2 - c \\dot{q}_2 + ku\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "or in state space form:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + " \\dfrac{dx}{dt} &= \\begin{bmatrix}\n", + " 0 & 0 & 1 & 0 \\\\\n", + " 0 & 0 & 0 & 1 \\\\[0.5ex]\n", + " -\\dfrac{2k}{m} & \\dfrac{k}{m} & -\\dfrac{c}{m} & 0 \\\\[0.5ex]\n", + " \\dfrac{k}{m} & -\\dfrac{2k}{m} & 0 & -\\dfrac{c}{m}\n", + " \\end{bmatrix} x\n", + " + \\begin{bmatrix}\n", + " 0 \\\\ 0 \\\\[0.5ex] 0 \\\\[1ex] \\dfrac{k}{m}\n", + " \\end{bmatrix} u.\n", + "\\end{aligned}\n", + "$$\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the parameters for the system\n", + "m, c, k = 1, 0.1, 2\n", + "# Create a linear system\n", + "A = np.array([\n", + " [0, 0, 1, 0],\n", + " [0, 0, 0, 1],\n", + " [-2*k/m, k/m, -c/m, 0],\n", + " [k/m, -2*k/m, 0, -c/m]\n", + "])\n", + "B = np.array([[0], [0], [0], [k/m]])\n", + "C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])\n", + "D = 0\n", + "\n", + "sys = ct.ss(A, B, C, D, outputs=['q1', 'q2'], name=\"coupled spring mass\")\n", + "print(sys)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "kobxJ1yG4v_1" + }, + "source": [ + "Another way to get these same dynamics is to define an input/output system:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "coupled_params = {'m': 1, 'c': 0.1, 'k': 2}\n", + "def coupled_update(t, x, u, params):\n", + " m, c, k = params['m'], params['c'], params['k']\n", + " return np.array([\n", + " x[2], x[3],\n", + " -2*k/m * x[0] + k/m * x[1] - c/m * x[2],\n", + " k/m * x[0] -2*k/m * x[1] - c/m * x[3] + k/m * u[0]\n", + " ])\n", + "def coupled_output(t, x, u, params):\n", + " return x[0:2]\n", + "coupled = ct.nlsys(\n", + " coupled_update, coupled_output, inputs=1, outputs=['q1', 'q2'],\n", + " states=['q1', 'q2', 'q1dot', 'q2dot'], name='coupled (nl)',\n", + " params=coupled_params\n", + ")\n", + "print(coupled.linearize([0, 0, 0, 0], [0]))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "YmH87LEXWo1U" + }, + "source": [ + "### Initial response\n", + "\n", + "The `initial_response` function can be used to compute the response of the system with no input, but starting from a given initial condition. This function returns a response object, which can be used for plotting." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "response = ct.initial_response(sys, X0=[1, 0, 0, 0])\n", + "cplt = response.plot()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Y4aAxYvZRBnD" + }, + "source": [ + "If you want to play around with the way the data are plotted, you can also use the response object to get direct access to the states and outputs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the outputs of the system on the same graph, in different colors\n", + "t = response.time\n", + "x = response.states\n", + "plt.plot(t, x[0], 'b', t, x[1], 'r')\n", + "plt.legend(['$x_1$', '$x_2$'])\n", + "plt.xlim(0, 50)\n", + "plt.ylabel('States')\n", + "plt.xlabel('Time [s]')\n", + "plt.title(\"Initial response from $x_1 = 1$, $x_2 = 0$\");" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Cou0QVnkTou9" + }, + "source": [ + "There are also lots of options available in `initial_response` and `.plot()` for tuning the plots that you get." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "for X0 in [[1, 0, 0, 0], [0, 2, 0, 0], [1, 2, 0, 0], [0, 0, 1, 0], [0, 0, 2, 0]]:\n", + " response = ct.initial_response(sys, T=20, X0=X0)\n", + " response.plot(label=f\"{X0=}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "b3VFPUBKT4bh" + }, + "source": [ + "### Step response\n", + "\n", + "Similar to `initial_response`, you can also generate a step response for a linear system using the `step_response` function, which returns a time response object:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "cplt = ct.step_response(sys).plot()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "iHZR1Q3IcrFT" + }, + "source": [ + "We can analyze the properties of the step response using the `stepinfo` command:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "step_info = ct.step_info(sys)\n", + "print(\"Input 0, output 0 rise time = \",\n", + " step_info[0][0]['RiseTime'], \"seconds\\n\")\n", + "step_info" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "F8KxXwqHWFab" + }, + "source": [ + "Note that by default the inputs are not included in the step response plot (since they are a bit boring), but you can change that:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "stepresp = ct.step_response(sys)\n", + "cplt = stepresp.plot(plot_inputs=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the inputs on top of the outputs\n", + "cplt = stepresp.plot(plot_inputs='overlay')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Look at the \"shape\" of the step response\n", + "print(f\"{stepresp.time.shape=}\")\n", + "print(f\"{stepresp.inputs.shape=}\")\n", + "print(f\"{stepresp.states.shape=}\")\n", + "print(f\"{stepresp.outputs.shape=}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "FDfZkyk1ly0T" + }, + "source": [ + "## Forced response\n", + "\n", + "To compute the response to an input, using the convolution equation, we can use the `forced_response` function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "T = np.linspace(0, 50, 500)\n", + "U1 = np.cos(T)\n", + "U2 = np.sin(3 * T)\n", + "\n", + "resp1 = ct.forced_response(sys, T, U1)\n", + "resp2 = ct.forced_response(sys, T, U2)\n", + "resp3 = ct.forced_response(sys, T, U1 + U2)\n", + "\n", + "# Plot the individual responses\n", + "resp1.sysname = 'U1'; resp1.plot(color='b')\n", + "resp2.sysname = 'U2'; resp2.plot(color='g')\n", + "resp3.sysname = 'U1 + U2'; resp3.plot(color='r');" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Show that the system response is linear\n", + "cplt = resp3.plot()\n", + "cplt.axes[0, 0].plot(resp1.time, resp1.outputs[0] + resp2.outputs[0], 'k--')\n", + "cplt.axes[1, 0].plot(resp1.time, resp1.outputs[1] + resp2.outputs[1], 'k--')\n", + "cplt.axes[2, 0].plot(resp1.time, resp1.inputs[0] + resp2.inputs[0], 'k--');" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Show that the forced response from non-zero initial condition is not linear\n", + "X0 = [1, 0, 0, 0]\n", + "resp1 = ct.forced_response(sys, T, U1, X0=X0)\n", + "resp2 = ct.forced_response(sys, T, U2, X0=X0)\n", + "resp3 = ct.forced_response(sys, T, U1 + U2, X0=X0)\n", + "\n", + "cplt = resp3.plot()\n", + "cplt.axes[0, 0].plot(resp1.time, resp1.outputs[0] + resp2.outputs[0], 'k--')\n", + "cplt.axes[1, 0].plot(resp1.time, resp1.outputs[1] + resp2.outputs[1], 'k--')\n", + "cplt.axes[2, 0].plot(resp1.time, resp1.inputs[0] + resp2.inputs[0], 'k--');" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "mo7hpvPQkKke" + }, + "source": [ + "### Frequency response" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Manual computation of the frequency response\n", + "resp = ct.input_output_response(sys, T, np.sin(1.35 * T))\n", + "\n", + "cplt = resp.plot(\n", + " plot_inputs='overlay', \n", + " legend_map=np.array([['lower left'], ['lower left']]),\n", + " label=[['q1', 'u[0]'], ['q2', None]])" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "muqeLlJJ6s8F" + }, + "source": [ + "The magnitude and phase of the frequency response is controlled by the transfer function,\n", + "\n", + "$$\n", + "G(s) = C (sI - A)^{-1} B + D\n", + "$$\n", + "\n", + "which can be computed using the `ss2tf` function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "try:\n", + " G = ct.ss2tf(sys, name='u to q1, q2')\n", + "except ct.ControlMIMONotImplemented:\n", + " # Create SISO transfer functions, in case we don't have slycot\n", + " G = ct.ss2tf(sys[0, 0], name='u to q1')\n", + "print(G)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Gain and phase for the simulation above\n", + "from math import pi\n", + "val = G(1.35j)\n", + "print(f\"{G(1.35j)=}\")\n", + "print(f\"Gain: {np.absolute(val)}\")\n", + "print(f\"Phase: {np.angle(val)}\", \" (\", np.angle(val) * 180/pi, \"deg)\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Gain and phase at s = 0 (= steady state step response)\n", + "print(f\"{G(0)=}\")\n", + "print(\"Final value of step response:\", stepresp.outputs[0, 0, -1])" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "I9eFoXm92Jgj" + }, + "source": [ + "The frequency response across all frequencies can be computed using the `frequency_response` function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "freqresp = ct.frequency_response(sys)\n", + "cplt = freqresp.plot()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "pylQb07G2cqe" + }, + "source": [ + "By default, frequency responses are plotted using a \"Bode plot\", which plots the log of the magnitude and the (linear) phase against the log of the forcing frequency.\n", + "\n", + "You can also call the Bode plot command directly, and change the way the data are presented:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "cplt = ct.bode_plot(sys, overlay_outputs=True)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "I_LTjP2J6gqx" + }, + "source": [ + "Note the \"dip\" in the frequency response for y[1] at frequency 2 rad/sec, which corresponds to a \"zero\" of the transfer function.\n", + "\n", + "This dip becomes even more pronounced in the case of low damping coefficient $c$:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "cplt = ct.frequency_response(\n", + " coupled.linearize([0, 0, 0, 0], [0], params={'c': 0.01})\n", + ").plot(overlay_outputs=True)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "c7eWm8LCGh01" + }, + "source": [ + "## Additional resources\n", + "* [Code for FBS2e figures](https://fbswiki.org/wiki/index.php/Category:Figures): Python code used to generate figures in FBS2e\n", + "* [Python-control documentation for plotting time responses](https://python-control.readthedocs.io/en/0.10.0/plotting.html#time-response-data)\n", + "* [Python-control documentation for plotting frequency responses](https://python-control.readthedocs.io/en/0.10.0/plotting.html#frequency-response-data)\n", + "* [Python-control examples](https://python-control.readthedocs.io/en/0.10.0/examples.html): lots of Python and Jupyter examples of control system analysis and design\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/cds110-L4a_predprey-statefbk.ipynb b/examples/cds110-L4a_predprey-statefbk.ipynb new file mode 100644 index 000000000..487a4e40b --- /dev/null +++ b/examples/cds110-L4a_predprey-statefbk.ipynb @@ -0,0 +1,411 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "gQZtf4ZqM8HL" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 4a

\n", + "

Dynamics and State Feedback Control of a Predator-Prey Model

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1yMOSRNDDNtm-TJGMXX3NS7F4XybOuch-)\n", + "\n", + "In this lecture we describe the use of state space control concepts to analyze and stabilize the dynamics of a nonlinear model of a predator-prey system.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "qMVGK15gNQw2" + }, + "source": [ + "## Predator-Prey System Model\n", + "\n", + "We consider a predator-prey system, in which a predator species (lynxes) interacts with a prey species (hares):\n", + "\n", + "
\n", + " \"predprey-photo\"\n", + "   \n", + " \"predprey-photo\"\n", + "
\n", + "\n", + "The graph on the right shows the populations of hares and lynxes between 1845 and 1935 in a section of the Canadian Rockies (MacLulich, 1937)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the dynamics for the predator-prey system (no input)\n", + "predprey_params = {'r': 1.6, 'd': 0.56, 'b': 0.6, 'k': 125, 'a': 3.2, 'c': 50}\n", + "def predprey_update(t, x, u, params):\n", + " \"\"\"Predator prey dynamics\"\"\"\n", + " r, d, b, k, a, c = map(params.get, ['r', 'd', 'b', 'k', 'a', 'c'])\n", + " u = np.clip(u, -r, r)\n", + "\n", + " # Dynamics for the system\n", + " dx0 = (r + u[0]) * x[0] * (1 - x[0]/k) - a * x[1] * x[0]/(c + x[0])\n", + " dx1 = b * a * x[1] * x[0] / (c + x[0]) - d * x[1]\n", + "\n", + " return np.array([dx0, dx1])\n", + "\n", + "# Create a nonlinear I/O system\n", + "predprey = ct.nlsys(\n", + " predprey_update, name='predprey', params=predprey_params,\n", + " states=['H', 'L'], inputs='u', outputs=['H', 'L'])" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "YmH87LEXWo1U" + }, + "source": [ + "### Open loop dynamics\n", + "\n", + "The open loop dynamics of the system are oscillatory, with a period similar to the data shown above:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "T = np.linspace(0, 100, 500)\n", + "response = ct.input_output_response(\n", + " predprey, T, 0, [35, 35]\n", + ")\n", + "ct.time_response_plot(response, plot_inputs=False, overlay_signals=True);" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can also visualize the data using a phase plane plot:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a simple phase portrait\n", + "ct.phase_plane_plot(predprey, [0, 120, 0, 100], 1, gridtype='meshgrid');" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We see that the default parameters give a lot of warning messages and the phase portrait does not convey all of the details in some regions of the state space.\n", + "\n", + "We can make sure of some of the functions in the `phaseplot` module to get a better view of the dynamics:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a phase portrait\n", + "ct.phaseplot.equilpoints(predprey, [-5, 126, -5, 100])\n", + "ct.phaseplot.streamlines(\n", + " predprey, np.array([\n", + " [0, 100], [1, 0],\n", + " ]), 10, color='b')\n", + "ct.phaseplot.streamlines(\n", + " predprey, np.array([[124, 1]]), np.linspace(0, 10, 500), color='b')\n", + "ct.phaseplot.streamlines(\n", + " predprey, np.array([[125, 25], [125, 50], [125, 75]]), 3, color='b')\n", + "ct.phaseplot.streamlines(predprey, np.array([2, 8]), 6, color='b')\n", + "ct.phaseplot.streamlines(\n", + " predprey, np.array([[20, 30]]), np.linspace(0, 65, 500),\n", + " gridtype='circlegrid', gridspec=[2, 1], arrows=10, color='r')\n", + "ct.phaseplot.vectorfield(predprey, [5, 125, 5, 100], gridspec=[20, 20])\n", + "\n", + "# Add the limit cycle\n", + "resp1 = ct.initial_response(predprey, np.linspace(0, 100), [20, 75])\n", + "resp2 = ct.initial_response(\n", + " predprey, np.linspace(0, 20, 500), resp1.states[:, -1])\n", + "plt.plot(resp2.states[0], resp2.states[1], color='k');" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "KhjlC1258qff" + }, + "source": [ + "### Find the equilibrium points and check stability\n", + "\n", + "We see that there are three equilibrium points in the system. We can test the stability of the center equilibrium point, which from the phase portrait appears to be unstable." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "xe, ue = ct.find_eqpt(predprey, [20, 30], 0)\n", + "print(f\"{xe=}\")\n", + "print(f\"{ue=}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "sys = predprey.linearize(xe, ue)\n", + "print(sys)\n", + "print(\"Poles: \", sys.poles())" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "sUECx0cz9QpK" + }, + "source": [ + "## Stabilization\n", + "\n", + "Suppose now that we have the ability to modulate the food supply for the hares. We do this by modifying the parameter $r$ in the model (this is the term `u` in the model at the top of the notebook). We can use the `place` command to find a set of gains that stabilize the dynamics around the unstable equilibrium point." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "K = ct.place(sys.A, sys.B, [-0.1, -0.2])\n", + "print(f\"{K=}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Design an eigenvalue placement (EP) controller to stabilize the equilibrium point\n", + "epctrl = ct.nlsys(\n", + " None, lambda t, x, u, params: -K @ (u[0:2] - xe),\n", + " inputs=['H', 'L', 'r'], outputs=['u'],\n", + ")\n", + "predprey_ep = ct.interconnect(\n", + " [predprey, epctrl], inputs=['r'], outputs=['H', 'L', 'u'],\n", + " name='predprey w/ eval placement'\n", + ")\n", + "print(predprey_ep)\n", + "\n", + "# Show the connection table, useful for debugging what is connected to what\n", + "predprey_ep.connection_table()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "xe_ep, ue_ep = ct.find_eqpt(predprey_ep, [20, 30], [0])\n", + "print(f\"{xe_ep=}\")\n", + "print(f\"{ue_ep=}\")\n", + "print(\"Poles: \", predprey_ep.linearize(xe_ep, ue_ep).poles())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a simple phase portrait\n", + "ct.phase_plane_plot(\n", + " predprey_ep, [0, 120, 0, 100], 1,\n", + " plot_separatrices=False,\n", + " gridtype='meshgrid', gridspec=[8, 5]\n", + " );\n", + "ct.phaseplot.streamlines(\n", + " predprey_ep, np.array([xe_ep]), 20, dir='reverse',\n", + " gridtype='circlegrid', gridspec=[4, 11]);" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Simulation from someplace nearby\n", + "T = np.linspace(0, 40)\n", + "response = ct.input_output_response(predprey_ep, T, 0, [35, 35])\n", + "ct.time_response_plot(\n", + " response, plot_inputs=False, overlay_signals=True,\n", + " title=\"I/O response with eval placement, \" +\n", + " f\"r = {predprey.params['r']}\",\n", + " legend_loc='upper right')\n", + "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "zZTBWhlTgSNk" + }, + "source": [ + "## Integral feedback\n", + "\n", + "Another technique that we will learn about later in the class is integral feedback, which can be used to compensate for modeling uncertainty and constant disturbances.\n", + "\n", + "We start by asking what happens if we change the value for the parameter $r$ from its original value of 1.6 to a new value of 1.65 (a change of less than 4%):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Simulate with a change in food for the hares\n", + "T = np.linspace(0, 40)\n", + "response = ct.input_output_response(\n", + " predprey_ep, T, 0, [35, 35], params={'r': 1.65}\n", + ")\n", + "ct.time_response_plot(\n", + " response, plot_inputs=False, overlay_signals=True,\n", + " title=\"I/O response w/ eval placement, \" +\n", + " f\"r = {response.params['r']}\")\n", + "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')\n", + "response.sysname" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We see that the controller no longer stabilizes the equilibrium point (shown with the dashed lines). In particular, the steady state value of the lynx population does to almost twice the original value.\n", + "\n", + "This effect is even worse if we increase $r$ just a bit more (from 1.65 to 1.7)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "T = np.linspace(0, 40)\n", + "response = ct.input_output_response(\n", + " predprey_ep, T, 0, xe, params={'r': 1.7}\n", + ")\n", + "ct.time_response_plot(\n", + " response, plot_inputs=False, overlay_signals=True,\n", + " title=\"I/O response for predprey w/ eval placement, \" +\n", + " f\"r = {response.params['r']}\")\n", + "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')\n", + "response.sysname" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The system dynamics are now oscillatory, indicating that we are no longer stabilizing the desired equilibrium point. This indicates a lack of robustness in our feedback control system.\n", + "\n", + "We can compensate for the change in the parameter $r$ by making use of integral feedback in our controller. We will learn more about integral feedback in later lectures, but for now we demonstrate its ability to compensate for errors in our system model." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Integral feedback\n", + "# Design an eigenvalue placement (EP) controller to stabilize the equilibrium point\n", + "Ki = 0.0001\n", + "pictrl = ct.nlsys(\n", + " lambda t, x, u, params: u[1] - u[2],\n", + " lambda t, x, u, params: -K @ (u[0:2] - xe) - Ki * x[0],\n", + " inputs=['H', 'L', 'r'], outputs=['u'], states=1,\n", + ")\n", + "predprey_pi = ct.interconnect(\n", + " [predprey, pictrl], inputs=['r'], outputs=['H', 'L', 'u'],\n", + " name='predprey_pi'\n", + ")\n", + "print(predprey_pi)\n", + "\n", + "# Simulate with a change in food for the hares\n", + "T = np.linspace(0, 100, 500)\n", + "response = ct.input_output_response(\n", + " predprey_pi, T, xe[1], [25, 25, 0], params={'r': 1.65})\n", + "ct.time_response_plot(\n", + " response, plot_inputs=False, overlay_signals=True,\n", + " title=\"I/O response w/ integral action, \" +\n", + " f\"r = {response.params['r']}\",\n", + " legend_loc='upper right')\n", + "\n", + "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n", + "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We see that the system is once again stable at the desired equilibrium point!" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/cds110-L4b_lqr-tracking.ipynb b/examples/cds110-L4b_lqr-tracking.ipynb new file mode 100644 index 000000000..a4b1a0711 --- /dev/null +++ b/examples/cds110-L4b_lqr-tracking.ipynb @@ -0,0 +1,916 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "EHq8UWSjXSyz" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 4b

\n", + "

LQR Tracking

\n", + "

Richard M. Murray and Natalie Bernat, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1Q6hXokOO_e3-wl6_ghigpxGJRUrGcHp3)\n", + "\n", + "This example uses a linear system to show how to implement LQR based tracking and some of the tradeoffs between feedfoward and feedback. Integral action is also implemented." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "a23d6f89" + }, + "source": [ + "# Part I: Second order linear system\n", + "\n", + "We'll use a simple linear system to illustrate the concepts:\n", + "$$\n", + "\\frac{dx}{dt} =\n", + "\\begin{bmatrix}\n", + "0 & 10 \\\\\n", + "-1 & 0\n", + "\\end{bmatrix}\n", + "x +\n", + "\\begin{bmatrix}\n", + "0 \\\\\n", + "1\n", + "\\end{bmatrix}\n", + "u,\n", + "\\qquad\n", + "y = \\begin{bmatrix} 1 & 1 \\end{bmatrix} x.\n", + "$$\n", + "\n", + "" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define a simple linear system that we want to control\n", + "A = np.array([[0, 10], [-1, 0]])\n", + "B = np.array([[0], [1]])\n", + "C = np.array([[1, 1]])\n", + "sys = ct.ss(A, B, C, 0, name='sys')\n", + "print(sys)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ja1g1MlbieJy" + }, + "source": [ + "## Linear quadratic regulator (LQR) design\n", + "\n", + "We'll design a controller of the form\n", + "\n", + "$$\n", + "u=-Kx+k_rr\n", + "$$\n", + "\n", + "- For the feedback control gain $K$, we'll use linear quadratic regulator theory. We seek to find the control law that minimizes the cost function:\n", + "\n", + " $$\n", + " J(x(\\cdot), u(\\cdot)) = \\int_0^\\infty x^T(\\tau) Q x(\\tau) + u^T(\\tau) R u(\\tau)\\, d\\tau\n", + " $$\n", + "\n", + " The weighting matrices $Q\\succeq 0 \\in \\mathbb{R}^{n \\times n}$ and $R \\succ 0\\in \\mathbb{R}^{m \\times m}$ should be chosen based on the desired performance of the system (tradeoffs in state errors and input magnitudes). See Example 3.5 in [Optimization Based Control (OBC)](https://fbswiki.org/wiki/index.php/Supplement:_Optimization-Based_Control) for a discussion of how to choose these weights. For now, we just choose identity weights for all states and inputs.\n", + "\n", + "- For the feedforward control gain $k_r$, we derive the feedforward gain from an equilibrium point analysis:\n", + " $$\n", + " y_e = C(A-BK)^{-1}Bk_rr\n", + " \\qquad\\implies\\qquad k_r = \\frac{-1}{C(A-BK)^{-1}B}\n", + " $$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Construct an LQR controller for the system\n", + "Q = np.eye(sys.nstates)\n", + "R = np.eye(sys.ninputs)\n", + "K, _, _ = ct.lqr(sys, Q, R)\n", + "print('K: '+str(K))\n", + "\n", + "# Set the feedforward gain to track the reference\n", + "kr = (-1 / (C @ np.linalg.inv(A - B @ K) @ B))\n", + "print('k_r: '+str(kr))" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "99f036ea" + }, + "source": [ + "Now that we have our gains designed, we can simulate the closed loop system:\n", + "$$\n", + "\\frac{dx}{dt} = A_{cl}x + B_{cl} r,\n", + "\\quad A_{cl} = A-BK,\n", + "\\quad B_{cl} = Bk_r\n", + "$$\n", + "Notice that, with a state feedback controller, the new (closed loop) dynamics matrix absorbs the old (open loop) \"input\" $u$, and the new (closed loop) input is our reference signal $r$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Create a closed loop system\n", + "A_cl = A - B @ K\n", + "B_cl = B * kr\n", + "clsys = ct.ss(A_cl, B_cl, C, 0)\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "84422c3f" + }, + "source": [ + "## System simulations\n", + "\n", + "### Baseline controller\n", + "\n", + "To see how the baseline controller performs, we ask it to track a constant reference $r = 2$:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the step response with respect to the reference input\n", + "r = 2\n", + "Tf = 8\n", + "tvec = np.linspace(0, Tf, 100)\n", + "\n", + "U = r * np.ones_like(tvec)\n", + "time, output = ct.input_output_response(clsys, tvec, U)\n", + "plt.plot(time, output)\n", + "plt.plot([time[0], time[-1]], [r, r], '--');\n", + "plt.legend(['y', 'r']);\n", + "plt.ylabel(\"Output\")\n", + "plt.xlabel(\"Time $t$ [sec]\")\n", + "plt.title(\"Baseline controller step response\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ea2d1c59" + }, + "source": [ + "Things to try:\n", + "- set $k_r=0$\n", + "- set $k_r \\neq \\frac{-1}{C(A-BK)^{-1}B}$\n", + "- try different LQR weightings" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "84ee7635" + }, + "source": [ + "### Disturbance rejection\n", + "\n", + "To add an input disturbance to the system, we include a second open loop input:\n", + "$$\n", + "\\frac{dx}{dt} =\n", + "\\begin{bmatrix}\n", + "0 & 10 \\\\\n", + "-1 & 0\n", + "\\end{bmatrix}\n", + "x +\n", + "\\begin{bmatrix}\n", + "0 & 0\\\\\n", + "1 & 1\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "u\\\\\n", + "d\n", + "\\end{bmatrix},\n", + "\\qquad\n", + "y = \\begin{bmatrix} 1 & 1 \\end{bmatrix} x.\n", + "$$\n", + "\n", + "Our closed loop system becomes:\n", + "$$\n", + "\\frac{dx}{dt} =\n", + "\\begin{bmatrix}\n", + "0 & 10 \\\\\n", + "-1-K_{1} & 0-K_{2}\n", + "\\end{bmatrix}\n", + "x +\n", + "\\begin{bmatrix}\n", + "0 & 0\\\\\n", + "k_r & 1\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "r\\\\\n", + "d\n", + "\\end{bmatrix},\n", + "\\qquad\n", + "y = \\begin{bmatrix} 1 & 1 \\end{bmatrix} x.\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Resimulate with a disturbance input\n", + "B_ext = np.hstack([B * kr, B])\n", + "clsys = ct.ss(A - B @ K, B_ext, C, 0)\n", + "\n", + "# Construct the inputs for the augmented system\n", + "delta = 0.5\n", + "U = np.vstack([r * np.ones_like(tvec), delta * np.ones_like(tvec)])\n", + "\n", + "time, output = ct.input_output_response(clsys, tvec, U)\n", + "\n", + "plt.plot(time, output[0])\n", + "plt.plot([time[0], time[-1]], [r, r], '--')\n", + "plt.legend(['y', 'r']);\n", + "plt.ylabel(\"Output\")\n", + "plt.xlabel(\"Time $t$ [sec]\")\n", + "plt.title(\"Baseline controller step response with disturbance\");" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Qis2PP3nd7ua" + }, + "source": [ + "We see that this leads to steady state error, since the feedforward signal didn't include an offset for the disturbance." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "84a9e61c" + }, + "source": [ + "#### Integral feedback\n", + "\n", + "A standard approach to compensate for constant disturbances is to use integral feedback. To do this, we have to keep track of the integral of the error\n", + "\n", + "$$z = \\int_0^\\tau (y - r)\\, d\\tau= \\int_0^\\tau (Cx - r)\\, d\\tau.$$\n", + "\n", + "We do this by creating an augmented system that includes the dynamics of the process ($dx/dt$) along with the dynamics of the integrator state ($dz/dt$):\n", + "\n", + "$$\n", + "\\frac{d}{dt}\\begin{bmatrix}\n", + "x \\\\\n", + "z\n", + "\\end{bmatrix} =\n", + "\\begin{bmatrix}\n", + "A & 0 \\\\\n", + "C & 0\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "x \\\\\n", + "z\n", + "\\end{bmatrix} +\n", + "\\begin{bmatrix}\n", + "B\\\\\n", + "0 \\\\\n", + "\\end{bmatrix}\n", + "u+\n", + "\\begin{bmatrix}\n", + "0\\\\\n", + "-I \\\\\n", + "\\end{bmatrix}\n", + "r,\n", + "\\qquad\n", + "y = \\begin{bmatrix} C \\\\ 0 \\end{bmatrix} \\begin{bmatrix}\n", + "x \\\\\n", + "z\n", + "\\end{bmatrix}.\n", + "$$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define an augmented state space for use with LQR\n", + "A_aug = np.block([[sys.A, np.zeros((sys.nstates, 1))], [C, 0] ])\n", + "B_aug = np.vstack([sys.B, 0])\n", + "print(\"A =\", A_aug, \"\\nB =\", B_aug)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "463d9b85" + }, + "source": [ + "\n", + "Our controller then takes the form:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "u &= - Kx - k_\\text{i} \\int_0^\\tau (y - r)\\, d\\tau+k_rr \\\\\n", + " &= - (Kx + k_\\text{i}z)+k_rr .\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "This results in the closed loop system:\n", + "$$\n", + "\\frac{dx}{dt} =\n", + "\\begin{bmatrix}\n", + "A-BK & -Bk_i \\\\\n", + "C & 0\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "x \\\\\n", + "z\n", + "\\end{bmatrix} +\n", + "\\begin{bmatrix}\n", + "Bk_r\\\\\n", + "-I \\\\\n", + "\\end{bmatrix}\n", + "r,\n", + "\\qquad\n", + "y = \\begin{bmatrix} C \\\\ 0 \\end{bmatrix} \\begin{bmatrix}\n", + "x \\\\\n", + "z\n", + "\\end{bmatrix}.\n", + "$$\n", + "\n", + "Since z is part of the augmented state space, we can generate an LQR controller for the augmented system to find both the usual gain $K$ and the integral gain $k_i$:\n", + "$$\n", + "\\bar{K} = \\begin{bmatrix} K& k_i\\end{bmatrix}\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Create an LQR controller for the augmented system\n", + "K_aug, _, _ = ct.lqr(A_aug, B_aug, np.diag([1, 1, 1]), np.eye(sys.ninputs))\n", + "print('K_aug: '+str(K_aug))\n", + "\n", + "K = K_aug[:, 0:2]\n", + "ki = K_aug[:, 2]\n", + "kr = -1 / (C @ np.linalg.inv(A - B * K) @ B)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "19bb6592" + }, + "source": [ + "\n", + "\n", + "\n", + "Notice that the value of $K$ changed, so we needed to recompute $k_r$ too." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "zHlf8zoHoqvF" + }, + "source": [ + "To run simulations, we return to our system augmented with a disturbance, but we expand the outputs available to the controller:\n", + "\n", + "$$\n", + "\\frac{dx}{dt} =\n", + "\\begin{bmatrix}\n", + "0 & 10 \\\\\n", + "-1 & 0\n", + "\\end{bmatrix}\n", + "x +\n", + "\\begin{bmatrix}\n", + "0 & 0\\\\\n", + "1 & 1\n", + "\\end{bmatrix}\n", + "\\begin{bmatrix}\n", + "u\\\\\n", + "d\n", + "\\end{bmatrix},\n", + "$$\n", + "\n", + "$$\n", + "\\bar{y} = \\begin{bmatrix} 1 & 0 & 1 \\\\ 0 & 1 & 1 \\end{bmatrix}^T x = \\begin{bmatrix} x_1 & x_2 & y \\end{bmatrix} .\n", + "$$\n", + "\n", + "The controller then constructs its internal state $z$ out of $x$ and $r$.\n", + "\n", + "" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Construct a system with disturbance inputs, and full outputs (for the controller)\n", + "A_integral = sys.A\n", + "B_integral = np.hstack([sys.B, sys.B])\n", + "C_integral = [[1, 0], [0, 1], [1, 1]] # outputs for the controller: x1, x2, y\n", + "sys_integral = ct.ss(\n", + " A_integral, B_integral, C_integral, 0,\n", + " inputs=['u', 'd'],\n", + " outputs=['x1', 'x2', 'y']\n", + ")\n", + "print(sys_integral)\n", + "\n", + "# Construct an LQR+integral controller for the system with an internal state z\n", + "A_ctrl = [[0]]\n", + "B_ctrl = [[1, 1, -1]] # z_dot=Cx-r\n", + "C_ctrl = -ki #-ki*z\n", + "D_ctrl = np.hstack([-K, kr]) #-K*x + kr*r\n", + "ctrl_integral=ct.ss(\n", + " A_ctrl, B_ctrl, C_ctrl, D_ctrl, # u = -ki*z - K*x + kr*r\n", + " inputs=['x1', 'x2', 'r'], # system outputs + reference\n", + " outputs=['u'], # controller action\n", + ")\n", + "print(ctrl_integral)\n", + "\n", + "# Create the closed loop system\n", + "clsys_integral = ct.interconnect([sys_integral, ctrl_integral], inputs=['r', 'd'], outputs=['y'])\n", + "print(clsys_integral)\n", + "\n", + "# Resimulate with a disturbance input\n", + "delta = 0.5\n", + "U = np.vstack([r * np.ones_like(tvec), delta * np.ones_like(tvec)])\n", + "time, output, states = ct.input_output_response(clsys_integral, tvec, U, return_x=True)\n", + "plt.plot(time, output[0])\n", + "plt.plot([time[0], time[-1]], [r, r], '--')\n", + "plt.plot(time, states[2])\n", + "plt.legend(['y', 'r', 'z']);\n", + "plt.ylabel(\"Output\")\n", + "plt.xlabel(\"Time $t$ [sec]\")\n", + "plt.title(\"LQR+integral controller step response with disturbance\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "M9nXbITrhYg7" + }, + "source": [ + "Notice that the steady state value of $z=\\int(y-r)$ is not zero, but rather settles to whatever value makes $y-r$ zero!\n", + "" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "f8bfc15c" + }, + "source": [ + "# Part II: PVTOL Linear Quadratic Regulator Example\n", + "\n", + "Natalie Bernat, 26 Apr 2024
\n", + "Richard M. Murray, 25 Jan 2022\n", + "\n", + "This notebook contains an example of LQR control applied to the PVTOL system. It demonstrates how to construct an LQR controller by linearizing the system, and provides an alternate view of the feedforward component of the controller." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "77e2ed47" + }, + "source": [ + "## System description\n", + "\n", + "We use the PVTOL dynamics from [Feedback Systems (FBS2e)](https://fbswiki.org/wiki/index.php/Feedback_Systems:_An_Introduction_for_Scientists_and_Engineers), which can be found in Example 3.12}\n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x, \\\\\n", + " m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - m g - c \\dot y, \\\\\n", + " J \\ddot \\theta &= r F_1.\n", + "\\end{aligned}\n", + "$$\n", + " \n", + "$$\n", + "\\frac{dz}{dt} =\n", + "\\begin{bmatrix}\n", + "z_4 \\\\\n", + "z_5 \\\\\n", + "z_6 \\\\\n", + "-\\frac{c}{m}z_4 \\\\\n", + "-g-\\frac{c}{m}z_5 \\\\\n", + "0\n", + "\\end{bmatrix} +\n", + "\\begin{bmatrix}\n", + "0 \\\\\n", + "0 \\\\\n", + "0 \\\\\n", + "\\frac{F_1}{m}cos\\theta -\\frac{F_2}{m}sin\\theta \\\\\n", + "\\frac{F_1}{m}sin\\theta +\\frac{F_2}{m}cos\\theta \\\\\n", + "-\\frac{r}{J}F_1\n", + "\\end{bmatrix}\n", + "$$\n", + "
\n", + "\n", + "The state space variables for this system are:\n", + "\n", + "$z=(x,y,\\theta, \\dot x,\\dot y,\\dot \\theta), \\quad u=(F_1,F_2)$\n", + "\n", + "Notice that the x and y positions ($z_1$ and $z_2$) do not actually appear in the dynamics-- this makes sense, since the aircraft should hypothetically fly the same way no matter where in the air it is (neglecting effects near the ground)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# PVTOL dynamics\n", + "def pvtol_update(t, x, u, params):\n", + " from math import cos, sin\n", + " \n", + " # Get the parameter values\n", + " m, J, r, g, c = map(params.get, ['m', 'J', 'r', 'g', 'c'])\n", + "\n", + " # Get the inputs and states\n", + " x, y, theta, xdot, ydot, thetadot = x\n", + " F1, F2 = u\n", + "\n", + " # Constrain the inputs\n", + " F2 = np.clip(F2, 0, 1.5 * m * g)\n", + " F1 = np.clip(F1, -0.1 * F2, 0.1 * F2)\n", + "\n", + " # Dynamics\n", + " xddot = (F1 * cos(theta) - F2 * sin(theta) - c * xdot) / m\n", + " yddot = (F1 * sin(theta) + F2 * cos(theta) - m * g - c * ydot) / m\n", + " thddot = (r * F1) / J\n", + "\n", + " return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])\n", + "\n", + "def pvtol_output(t, x, u, params):\n", + " return x\n", + "\n", + "pvtol = ct.nlsys(\n", + " pvtol_update, pvtol_output, name='pvtol',\n", + " states = [f'x{i}' for i in range(6)],\n", + " inputs = ['F1', 'F2'],\n", + " outputs=[f'x{i}' for i in range(6)],\n", + " # outputs = ['x', 'y', 'theta', 'xdot', 'ydot', 'thdot'],\n", + " params = {\n", + " 'm': 4., # mass of aircraft\n", + " 'J': 0.0475, # inertia around pitch axis\n", + " 'r': 0.25, # distance to center of force\n", + " 'g': 9.8, # gravitational constant\n", + " 'c': 0.05, # damping factor (estimated)\n", + " }\n", + ")\n", + "\n", + "print(pvtol)\n", + "print(pvtol.params)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "YZiISLS-qMS_" + }, + "source": [ + "Next, we'll linearize the system around the equilibrium points. As discussed in FBS2e (example 7.9), the linearization around this equilibrium point has the form:\n", + "$$\n", + "A =\n", + "\\begin{bmatrix}\n", + "0 & 0 & 0 & 1 & 0 & 0\\\\\n", + "0 & 0 & 0 & 0 & 1 & 0 \\\\\n", + "0 & 0 & 0 & 0 & 0 & 1 \\\\\n", + "0 & 0 & -g & -c/m & 0 & 0 \\\\\n", + "0 & 0 & 0 & 0 & -c/m & 0 \\\\\n", + "0 & 0 & 0 & 0 & 0 & 0\n", + "\\end{bmatrix}\n", + ", \\quad B=\n", + "\\begin{bmatrix}\n", + "0 & 0 \\\\\n", + "0 & 0 \\\\\n", + "0 & 0 \\\\\n", + "1/m & 0 \\\\\n", + "0 & 1/m \\\\\n", + "r/J & 0\n", + "\\end{bmatrix}\n", + ".\n", + "$$\n", + "(note that here $r$ is a system parameter, not the same as the reference $r$ we've been using elsewhere in this notebook)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "To compute this linearization in python-control, we start by computing the equilibrium point. We do this using the `find_eqpt` function, which can be used to find equilibrium points satisfying varioius conditions. For this system, we wish to find the state $x_\\text{e}$ and input $u_\\text{e}$ that holds the $x, y$ position of the aircraft at the point $(0, 0)$. The `find_eqpt` function performs a numerical optimization to find the values of $x_\\text{e}$ and $u_\\text{e}$ corresponding to an equilibrium point with the desired values for the outputs. We pass the function initial guesses for the state and input as well the values of the output and the indices of the output that we wish to constrain:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Find the equilibrium point corresponding to hover\n", + "xeq, ueq = ct.find_eqpt(pvtol, np.zeros(6), np.zeros(2), y0=np.zeros(6), iy=[0, 1])\n", + "print(f\"{xeq=}, {ueq=}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Using these values, we compute the linearization:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "linsys = pvtol.linearize(xeq, ueq)\n", + "print(linsys)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "7cb8840b" + }, + "source": [ + "## Linear quadratic regulator (LQR) design\n", + "\n", + "Now that we have a linearized model of the system, we can compute a controller using linear quadratic regulator theory. We wish to minimize the following cost function\n", + "\n", + "$$\n", + "J(\\phi(\\cdot), \\nu(\\cdot)) = \\int_0^\\infty \\phi^T(\\tau) Q \\phi(\\tau) + \\nu^T(\\tau) R \\nu(\\tau)\\, d\\tau,\n", + "$$\n", + "\n", + "where we have changed to our linearized coordinates:\n", + "\n", + "$$\\phi=z-z_e, \\quad \\nu = u-u_e$$\n", + "\n", + "Using the standard approach for finding K, we obtain a feedback controller for the system:\n", + "$$\\nu=-K\\phi$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Start with a diagonal weighting\n", + "Q1 = np.diag([1, 1, 1, 1, 1, 1])\n", + "R1 = np.diag([1, 1])\n", + "K, X, E = ct.lqr(linsys, Q1, R1)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "863d07de" + }, + "source": [ + "To create a controller for the system, we have to apply a control signal $u$, so we change back from the relative coordinates to the absolute coordinates:\n", + "\n", + "$$u=u_e - K(z - z_e)$$\n", + "\n", + "Notice that, since $(Kz_e+u_e)$ is completely determined by (user-defined) inputs to the system, this term is a type of feedforward control signal.\n", + "\n", + "To create a controller for the system, we can use the function [`create_statefbk_iosystem()`](https://python-control.readthedocs.io/en/latest/generated/control.create_statefbk_iosystem.html), which creates an I/O system that takes in a desired trajectory $(x_\\text{d}, u_\\text{d})$ and the current state $x$ and generates a control law of the form:\n", + "\n", + "$$\n", + "u = u_\\text{d} - K (x - x_\\text{d})\n", + "$$\n", + "\n", + "Note that this is slightly different than the first equation: here we are using $x_\\text{d}$ instead of $x_\\text{e}$ and $u_\\text{d}$ instead of $u_\\text{e}$. This is because we want our controller to track a desired trajectory $(x_\\text{d}(t), u_\\text{d}(t))$ rather than just stabilize the equilibrium point $(x_\\text{e}, u_\\text{e})$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "control, pvtol_closed = ct.create_statefbk_iosystem(pvtol, K)\n", + "print(control, \"\\n\")\n", + "print(pvtol_closed)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "This command will usually generate a warning saying that python control \"cannot verify system output is system state\". This happens because we specified an output function `pvtol_output` when we created the system model, and python-control does not have a way of checking that the output function returns the entire state (which is needed if we are going to do full-state feedback).\n", + "\n", + "This warning could be avoided by passing the argument `None` for the system output function, in which case python-control returns the full state as the output (and it knows that the full state is being returned as the output)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "bedcb0c0" + }, + "source": [ + "## Closed loop system simulation\n", + "\n", + "For this simple example, we set the target for the system to be a \"step\" input that moves the system 1 meter to the right.\n", + "\n", + "We start by defining a short function to visualize the output using a collection of plots:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function to plot the results in a useful way\n", + "def plot_results(t, x, u, fig=None):\n", + " # Set the size of the figure\n", + " if fig is None:\n", + " fig = plt.figure(figsize=(10, 6))\n", + "\n", + " # Top plot: xy trajectory\n", + " plt.subplot(2, 1, 1)\n", + " lines = plt.plot(x[0], x[1])\n", + " plt.xlabel('x [m]')\n", + " plt.ylabel('y [m]')\n", + " plt.axis('equal')\n", + "\n", + " # Mark starting and ending points\n", + " color = lines[0].get_color()\n", + " plt.plot(x[0, 0], x[1, 0], 'o', color=color, fillstyle='none')\n", + " plt.plot(x[0, -1], x[1, -1], 'o', color=color, fillstyle='full')\n", + "\n", + "\n", + " # Time traces of the state and input\n", + " plt.subplot(2, 4, 5)\n", + " plt.plot(t, x[1])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('y [m]')\n", + "\n", + " plt.subplot(2, 4, 6)\n", + " plt.plot(t, x[2])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('theta [rad]')\n", + "\n", + " plt.subplot(2, 4, 7)\n", + " plt.plot(t, u[0])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('$F_1$ [N]')\n", + "\n", + " plt.subplot(2, 4, 8)\n", + " plt.plot(t, u[1])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('$F_2$ [N]')\n", + " plt.tight_layout()\n", + "\n", + " return fig" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Next we generate a step response and plot the results. Because our closed loop system takes as inputs $x_\\text{d}$ and $u_\\text{d}$, we need to set those variable to values that would correspond to our step input. In this case, we are taking a step in the $x$ coordinate, so we set $x_\\text{d}$ to be $1$ in that coordinate starting at $t = 0$ and continuing for some sufficiently long period of time ($15$ seconds):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a step response by setting xd, ud\n", + "Tf = 15\n", + "T = np.linspace(0, Tf, 100)\n", + "xd = np.outer(np.array([1, 0, 0, 0, 0, 0]), np.ones_like(T))\n", + "ud = np.outer(ueq, np.ones_like(T))\n", + "ref = np.vstack([xd, ud])\n", + "\n", + "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", + "fig = plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "f014e660" + }, + "source": [ + "This controller does a pretty good job. We see in the top plot the $x$, $y$ projection of the trajectory, with the open circle indicating the starting point and the closed circle indicating the final point. The bottom set of plots show the altitude and pitch as functions of time, as well as the input forces. All of the signals look reasonable.\n", + "\n", + "The limitations of the linear controller can be seen if we take a larger step, say 10 meters." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "xd = np.outer(np.array([10, 0, 0, 0, 0, 0]), np.ones_like(T))\n", + "ref = np.vstack([xd, ud])\n", + "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", + "fig = plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "4luxppVpm6Xo" + }, + "source": [ + "We now see that the trajectory looses significant altitude ($> 2.5$ meters). This is because the linear controller sees a large initial error and so it applies very large input forces to correct for the error ($F_1 \\approx -10$ N at $t = 0$. This causes the aircraft to pitch over to a large angle (almost $-60$ degrees) and this causes a large loss in altitude.\n", + "\n", + "We will see in the [Lecture 6](cds110-L6a_kincar-trajgen) how to remedy this problem by making use of feasible trajectory generation." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/cds110-L5_kincar-estimation.ipynb b/examples/cds110-L5_kincar-estimation.ipynb new file mode 100644 index 000000000..6eea0a1f0 --- /dev/null +++ b/examples/cds110-L5_kincar-estimation.ipynb @@ -0,0 +1,815 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "-cop8q3CTs-G" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 5

\n", + "

State Estimation for a Kinematic Car Model

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1TESB0NzWS3XBxJa_hdOXMifICbBEDRz8)\n", + "\n", + "In this lecture, we will show how to construct an observer for a system in the presence of noise and disturbances.\n", + "\n", + "Recall that an observer is a system that takes as input the (noisy) measured output of a system along with the applied input to the system, and produces as estimate $\\hat x$ of the current state:\n", + "\n", + "
\n", + "\n", + "
\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Import the various Python packages that we require\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "from math import pi, sin, cos, tan\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "import control.flatsys as fs" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "c5UGnS73sH4c" + }, + "source": [ + "## White noise\n", + "\n", + "A white noise process $W(t)$ is a signal that has the property that the mean of the signal is 0 and the value of the signal at any point in time $t$ is uncorrelated to the value of the signal at a point in time $s$, but that has a fixed amount of variance. Mathematically, a white noise process $W\n", + "(t) \\in \\mathbb{R}^k$ satisfies\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\mathbb{E}\\{W(t)\\} &= 0, &&\\text{for all $t$} \\\\\n", + "\\mathbb{E}\\{W^\\mathtt{T}(t) W(s)\\} &= Q\\, \\delta(t-s) && \\text{for all $s, t$},\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "where $Q \\in \\mathbb{R}^{k \\times k}$ is the \"intensity\" of the white noise process.\n", + "\n", + "The python-control function `white_noise` can be used to create an instantiation of a white noise process:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Create the time vector that we want to use\n", + "Tf = 5\n", + "T = np.linspace(0, Tf, 1000)\n", + "dt = T[1] - T[0]\n", + "\n", + "# Create a white noise signal\n", + "?ct.white_noise\n", + "Q = np.array([[0.1]])\n", + "W = ct.white_noise(T, Q)\n", + "\n", + "plt.figure(figsize=[5, 3])\n", + "plt.plot(T, W[0])\n", + "plt.xlabel('Time [s]')\n", + "plt.ylabel('$V$');" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "MtAPkkCd14_g" + }, + "source": [ + "To confirm this is a white noise signal, we can compute the correlation function\n", + "\n", + "$$\n", + "\\rho(\\tau) = \\mathbb{E}\\{V^\\mathtt{T}(t) V(t + \\tau)\\} = Q\\, \\delta(\\tau),\n", + "$$\n", + "\n", + "where $\\delta(\\tau)$ is the unit impulse function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Correlation function for the input\n", + "tau, r_W = ct.correlation(T, W)\n", + "\n", + "plt.plot(tau, r_W, 'r-')\n", + "plt.xlabel(r'$\\tau$')\n", + "plt.ylabel(r'$r_W(\\tau)$')\n", + "\n", + "# Compute out the area under the peak\n", + "print(\"Signal covariance: \", Q.item())\n", + "print(\"Area under impulse: \", np.max(W) * dt)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "1eN_MZ94tQ9v" + }, + "source": [ + "## System definition: kinematic car\n", + "\n", + "We make use of a simple model for a vehicle navigating in the plane, known as the \"bicycle model\". The kinematics of this vehicle can be written in terms of the contact point $(x, y)$ and the angle $\\theta$ of the vehicle with respect to the horizontal axis:\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\large\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "The input $v$ represents the velocity of the vehicle and the input $\\delta$ represents the turning rate. The parameter $l$ is the wheelbase." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# System definition\n", + "# Function to compute the RHS of the system dynamics\n", + "def kincar_update(t, x, u, params):\n", + " # Get the parameters for the model\n", + " l = params['wheelbase'] # vehicle wheelbase\n", + " deltamax = params['maxsteer'] # max steering angle (rad)\n", + "\n", + " # Saturate the steering input\n", + " delta = np.clip(u[1], -deltamax, deltamax)\n", + "\n", + " # Return the derivative of the state\n", + " return np.array([\n", + " np.cos(x[2]) * u[0], # xdot = cos(theta) v\n", + " np.sin(x[2]) * u[0], # ydot = sin(theta) v\n", + " (u[0] / l) * np.tan(delta) # thdot = v/l tan(delta)\n", + " ])\n", + "\n", + "kincar_params={'wheelbase': 3, 'maxsteer': 0.5}\n", + "\n", + "# Create nonlinear input/output system\n", + "kincar = ct.nlsys(\n", + " kincar_update, None, name=\"kincar\", params=kincar_params,\n", + " inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n", + " states=('x', 'y', 'theta'))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function to plot lane change manuever\n", + "def plot_lanechange(t, y, u, figure=None, yf=None, label=None):\n", + " # Plot the xy trajectory\n", + " plt.subplot(3, 1, 1, label='xy')\n", + " plt.plot(y[0], y[1], label=label)\n", + " plt.xlabel(\"x [m]\")\n", + " plt.ylabel(\"y [m]\")\n", + " if yf is not None:\n", + " plt.plot(yf[0], yf[1], 'ro')\n", + "\n", + " # Plot x and y as functions of time\n", + " plt.subplot(3, 2, 3, label='x')\n", + " plt.plot(t, y[0])\n", + " plt.ylabel(\"$x$ [m]\")\n", + "\n", + " plt.subplot(3, 2, 4, label='y')\n", + " plt.plot(t, y[1])\n", + " plt.ylabel(\"$y$ [m]\")\n", + "\n", + " # Plot the inputs as a function of time\n", + " plt.subplot(3, 2, 5, label='v')\n", + " plt.plot(t, u[0])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$v$ [m/s]\")\n", + "\n", + " plt.subplot(3, 2, 6, label='delta')\n", + " plt.plot(t, u[1])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$\\\\delta$ [rad]\")\n", + "\n", + " plt.subplot(3, 1, 1)\n", + " plt.title(\"Lane change manuever\")\n", + " if label:\n", + " plt.legend()\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "5F-40uInyvQr" + }, + "source": [ + "We next define a desired trajectory for the vehicle. For simplicity, we use a piecewise linear trajectory and then stabilize the system around that trajectory. We will learn in a later lecture how to do this is in more rigorous way. For now, it is enough to know that this generates a feasible trajectory for the vehicle." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a trajectory for the vehicle\n", + "# Define the endpoints of the trajectory\n", + "x0 = np.array([0., -4., 0.]); u0 = np.array([10., 0.])\n", + "xf = np.array([40., 4., 0.]); uf = np.array([10., 0.])\n", + "Tf = 4\n", + "Ts = Tf / 100\n", + "\n", + "# First 0.6 seconds: drive straight\n", + "T1 = np.linspace(0, 0.6, 15, endpoint=False)\n", + "x1 = np.array([6, -4, 0])\n", + "xd1 = np.array([x0 + (x1 - x0) * (t - T1[0]) / (T1[-1] - T1[0]) for t in T1]).transpose()\n", + "\n", + "# Next 2.8 seconds: change to the other lane\n", + "T2 = np.linspace(0.6, 3.4, 70, endpoint=False)\n", + "x2 = np.array([35, 4, 0])\n", + "xd2 = np.array([x1 + (x2 - x1) * (t - T2[0]) / (T2[-1] - T2[0]) for t in T2]).transpose()\n", + "\n", + "# Final 0.6 seconds: drive straight\n", + "T3 = np.linspace(3.4, Tf, 15, endpoint=False)\n", + "xd3 = np.array([x2 + (xf - x2) * (t - T3[0]) / (T3[-1] - T3[0]) for t in T3]).transpose()\n", + "\n", + "T = np.hstack([T1, T2, T3])\n", + "xr = np.hstack([xd1, xd2, xd3])\n", + "ur = np.array([u0 for t in T]).transpose()\n", + "\n", + "# Now create a simple controller to stabilize the trajectory\n", + "P = kincar.linearize(x0, u0)\n", + "K, _, _ = ct.lqr(\n", + " kincar.linearize(x0, u0),\n", + " np.diag([10, 100, 1]), np.diag([10, 10])\n", + ")\n", + "\n", + "# Construct a closed loop controller for the system\n", + "ctrl, clsys = ct.create_statefbk_iosystem(kincar, K)\n", + "resp = ct.input_output_response(clsys, T, [xr, ur], x0)\n", + "\n", + "xd = resp.states\n", + "ud = resp.outputs[kincar.nstates:]\n", + "\n", + "plot_lanechange(T, xd, ud, label='feasible')\n", + "plot_lanechange(T, xr, ur, label='reference')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Simulation of the open loop trajectory\n", + "sys_resp = ct.input_output_response(kincar, T, ud, xd[:, 0])\n", + "plt.plot(sys_resp.states[0], sys_resp.states[1])\n", + "plt.axis([0, 40, -5, 5])\n", + "plt.xlabel(\"$x$ [m]\")\n", + "plt.ylabel(\"$y$ [m]\")\n", + "plt.gca().set_aspect('equal')" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "7V81jzfZtiRe" + }, + "source": [ + "## State estimation\n", + "\n", + "To illustrate how we can estimate the state of the trajectory, we construct an observer that takes the measured inputs and outputs to the system and computes an estimate of the state, using a estimator with dynamics\n", + "\n", + "$$\n", + "\\dot{\\hat x} = f(\\hat x, u) - L(C \\hat x - y)\n", + "$$\n", + "\n", + "Note that we go ahead and use the nonlinear dynamics for the prediction term, but the linearization for the correction term.\n", + "\n", + "We can determine the estimator gain $L$ via multiple methods:\n", + "* Eigenvalue placement\n", + "* Optimal estimation (Kalman filter)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "Jt_5SUTBuN7-" + }, + "source": [ + "### Eigenvalue placement" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the outputs to use for measurements\n", + "C = np.eye(2, 3)\n", + "\n", + "# Compute the linearization of the nonlinear dynamics\n", + "P = kincar.linearize([0, 0, 0], [10, 0])\n", + "\n", + "# Compute the gains via eigenvalue placement\n", + "L = ct.place(P.A.T, C.T, [-1, -2, -3]).T\n", + "\n", + "# Estimator update law\n", + "def estimator_update(t, xhat, u, params):\n", + " # Extract the inputs to the estimator\n", + " y = u[0:2] # first two system outputs\n", + " u = u[2:4] # inputs that were applied\n", + "\n", + " # Update the state estimate\n", + " xhatdot = kincar.updfcn(t, xhat, u, kincar_params) \\\n", + " - params['L'] @ (C @ xhat - y)\n", + "\n", + " # Return the derivative\n", + " return xhatdot\n", + "\n", + "estimator = ct.nlsys(\n", + " estimator_update, None, name='estimator',\n", + " states=kincar.nstates, params={'L': L},\n", + " inputs= kincar.state_labels[0:2] + kincar.input_labels,\n", + " outputs=[f'xh{i}' for i in range(kincar.nstates)],\n", + ")\n", + "print(estimator)\n", + "print(estimator.params)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Run the estimator from a different initial condition\n", + "estresp = ct.input_output_response(\n", + " estimator, T, [xd[0:2], ud], [0, -3, 0])\n", + "\n", + "fig, axs = plt.subplots(3, 1, figsize=[5, 4])\n", + "\n", + "axs[0].plot(estresp.time, estresp.outputs[0], 'b-', T, xd[0], 'r--')\n", + "axs[0].set_ylabel(\"$x$\")\n", + "axs[0].legend([r\"$\\hat x$\", \"$x$\"])\n", + "\n", + "axs[1].plot(estresp.time, estresp.outputs[1], 'b-', T, xd[1], 'r--')\n", + "axs[1].set_ylabel(\"$y$\")\n", + "\n", + "axs[2].plot(estresp.time, estresp.outputs[2], 'b-', T, xd[2], 'r--')\n", + "axs[2].set_ylabel(r\"$\\theta$\")\n", + "axs[2].set_xlabel(\"Time $t$ [s]\")\n", + "\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "KPkD-wSXt8d0" + }, + "source": [ + "### Kalman filter\n", + "\n", + "An alternative mechanism for creating an estimator is through the use of optimal estimation (Kalman filtering).\n", + "\n", + "Suppose that we have (very) noisy measurements of the system position, and also have disturbances taht are applied to our control signal." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Disturbance and noise covariances\n", + "Qv = np.diag([0.1**2, 0.01**2])\n", + "Qw = np.eye(2) * 0.1**2\n", + "\n", + "u_noisy = ud + ct.white_noise(T, Qv)\n", + "sys_resp = ct.input_output_response(kincar, T, u_noisy, xd[:, 0])\n", + "\n", + "# Create noisy version of the measurements\n", + "y_noisy = sys_resp.outputs[0:2] + ct.white_noise(T, Qw)\n", + "\n", + "plt.plot(y_noisy[0], y_noisy[1], 'k-')\n", + "plt.plot(sys_resp.outputs[0], sys_resp.outputs[1], 'b-')\n", + "plt.axis([0, 40, -5, 5])\n", + "plt.xlabel(\"$x$ [m]\")\n", + "plt.ylabel(\"$y$ [m]\")\n", + "plt.legend(['measured', 'actual'])\n", + "plt.gca().set_aspect('equal')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "A Kalman filter allows us to estimate the optimal state given measurements of the inputs and outputs, as well as knowledge of the covariance of the signals." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the Kalman gains (linear quadratic estimator)\n", + "L_kf, _, _ = ct.lqe(P.A, P.B, C, Qv, Qw)\n", + "\n", + "kfresp = ct.input_output_response(\n", + " estimator, T, [y_noisy, ud], [0, -3, 0],\n", + " params={'L': L_kf})\n", + "\n", + "fig, axs = plt.subplots(3, 1, figsize=[5, 4])\n", + "\n", + "axs[0].plot(T, y_noisy[0], 'k-')\n", + "axs[0].plot(kfresp.time, kfresp.outputs[0], 'b-', T, sys_resp.outputs[0], 'r--')\n", + "axs[0].set_ylabel(\"$x$\")\n", + "axs[0].legend([r\"$\\hat x$\", \"$x$\"])\n", + "\n", + "axs[1].plot(T, y_noisy[1], 'k-')\n", + "axs[1].plot(kfresp.time, kfresp.outputs[1], 'b-', T, sys_resp.outputs[1], 'r--')\n", + "axs[1].set_ylabel(\"$y$\")\n", + "\n", + "axs[2].plot(kfresp.time, kfresp.outputs[2], 'b-', T, sys_resp.outputs[2], 'r--')\n", + "axs[2].set_ylabel(r\"$\\theta$\")\n", + "axs[2].set_xlabel(\"Time $t$ [s]\")\n", + "\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "pMfHmzsW0Dqh" + }, + "source": [ + "We can get a better view of the convergence by plotting the errors:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "fig, axs = plt.subplots(3, 1, figsize=[5, 4])\n", + "\n", + "axs[0].plot(kfresp.time, kfresp.outputs[0] - sys_resp.outputs[0])\n", + "axs[0].plot([T[0], T[-1]], [0, 0], 'k--')\n", + "axs[0].set_ylabel(\"$x$ error\")\n", + "axs[0].set_ylim([-1, 1])\n", + "\n", + "axs[1].plot(kfresp.time, kfresp.outputs[1] - sys_resp.outputs[1])\n", + "axs[1].plot([T[0], T[-1]], [0, 0], 'k--')\n", + "axs[1].set_ylabel(\"$y$ error\")\n", + "axs[1].set_ylim([-1, 1])\n", + "\n", + "axs[2].plot(kfresp.time, kfresp.outputs[2] - sys_resp.outputs[2])\n", + "axs[2].plot([T[0], T[-1]], [0, 0], 'k--')\n", + "axs[2].set_ylabel(r\"$\\theta$ error\")\n", + "axs[2].set_xlabel(\"Time $t$ [s]\")\n", + "axs[2].set_ylim([-0.2, 0.2])\n", + "\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "nccW48C5tns9" + }, + "source": [ + "## Output feedback control\n", + "\n", + "We next construct a controller that makes use of the estimated state. We will attempt to control the longitudinal position using the steering angle as an input, with the velocity set to the desired velocity (no tracking of the longitudinal position)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the linearization of the nonlinear dynamics\n", + "P = kincar.linearize([0, 0, 0], [10, 0])\n", + "\n", + "# Extract out the linearized dynamics from delta to y\n", + "Alat = P.A[1:3, 1:3]\n", + "Blat = P.B[1:3, 1:2]\n", + "Clat = P.C[1:2, 1:3]\n", + "\n", + "sys = ct.ss(Alat, Blat, Clat, 0)\n", + "print(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Construct a state space controller, using LQR\n", + "Qx = np.diag([1, 10])\n", + "Qu = np.diag([1])\n", + "\n", + "K, _, _ = ct.lqr(Alat, Blat, Qx, Qu)\n", + "print(f\"{K=}\")\n", + "\n", + "kf = -1 / (Clat @ np.linalg.inv(Alat - Blat @ K) @ Blat)\n", + "print(f\"{kf=}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "v5oHK9-XMrEv" + }, + "source": [ + "### Direct state space feedback\n", + "\n", + "We start by checking the response of the system assuming that we measure the state directly.\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Construct a controller for the full system\n", + "def ctrl_output(t, x, u, params):\n", + " r_v, r_y = u[0:2]\n", + " x = u[3:5] # y, theta\n", + " return np.vstack([r_v, -K @ x + kf * r_y])\n", + "ctrl = ct.nlsys(\n", + " None, ctrl_output, name='ctrl',\n", + " inputs=['r_v', 'r_y', 'x', 'y', 'theta'],\n", + " outputs=['v', 'delta']\n", + ")\n", + "print(ctrl)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Direct state feedback\n", + "clsys_direct = ct.interconnect(\n", + " [kincar, ctrl],\n", + " inputs=['r_v', 'r_y'],\n", + " outputs=['x', 'y', 'theta', 'v', 'delta'],\n", + ")\n", + "print(clsys_direct)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Run a simulation\n", + "clresp_direct = ct.input_output_response(\n", + " clsys_direct, T, [10, xd[1]], X0=[0, -3, 0])\n", + "\n", + "plt.plot(clresp_direct.outputs[0], clresp_direct.outputs[1])\n", + "plt.plot(xd[0], xd[1], 'r--')\n", + "# plt.plot(clresp.time, clresp.outputs[1])\n", + "plt.xlabel(\"$x$ [m]\")\n", + "plt.ylabel(\"$y$ [m]\")\n", + "plt.gca().set_aspect('equal')" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "J0iS9V8YT4Ox" + }, + "source": [ + "Note the \"lag\" in the $x$ coordinate. This comes from the fact that we did not use feedback to maintain the longitudinal position as a function of time, compared with the desired trajectory. To see this, we can look at the commanded speed ($v$) versus the desired speed:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plot_lanechange(T, xd, ud, label=\"desired\")\n", + "plot_lanechange(T, clresp_direct.outputs[0:2], clresp_direct.outputs[-2:], label=\"actual\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "SDrkfC_LUPDu" + }, + "source": [ + "From this plot we can also see that there is a very large input $\\delta$ applied at $t=0$. This is something we would have to fix if we were to implement this on a physical system (-1 rad $\\approx -60^\\circ$!)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "KS0E2g6aMgC0" + }, + "source": [ + "### Estimator-based control\n", + "\n", + "We now consider the case were we cannot directly measure the state, but instead have to estimate the state from the commanded input and measured output. We can insert the estimator into the system model by reconnecting the inputs and outputs. The `ct.interconnect` function provides the needed flexibility:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "?ct.interconnect" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "rgI9QjBMAy7b" + }, + "source": [ + "We now create the system model that includes the estimator (observer). Here is the system we are trying to construct:\n", + "\n", + "\n", + "\n", + "\n", + "(Be careful with the notation: in the diagram above $y$ is the measured outputs, which for our system are the $x$ and $y$ position of the vehicle, so overusing the symbol $y$.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Connect the system, estimator, and controller\n", + "clsys_estim = ct.interconnect(\n", + " [kincar, estimator, ctrl],\n", + " inplist=['ctrl.r_v', 'ctrl.r_y', 'estimator.x', 'estimator.y'],\n", + " inputs=['r_v', 'r_y', 'noise_x', 'noise_y'],\n", + " outlist=[\n", + " 'kincar.x', 'kincar.y', 'kincar.theta',\n", + " 'estimator.xh0', 'estimator.xh1', 'estimator.xh2',\n", + " 'ctrl.v', 'ctrl.delta'\n", + " ],\n", + " outputs=['x', 'y', 'theta', 'xhat', 'yhat', 'thhat', 'v', 'delta'],\n", + " connections=[\n", + " ['kincar.v', 'ctrl.v'],\n", + " ['kincar.delta', 'ctrl.delta'],\n", + " ['estimator.x', 'kincar.x'],\n", + " ['estimator.y', 'kincar.y'],\n", + " ['estimator.delta', 'ctrl.delta'],\n", + " ['estimator.v', 'ctrl.v'],\n", + " ['ctrl.x', 'estimator.xh0'],\n", + " ['ctrl.y', 'estimator.xh1'],\n", + " ['ctrl.theta', 'estimator.xh2'],\n", + " ],\n", + ")\n", + "print(clsys_estim)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Run a simulation with no noise first\n", + "clresp_nonoise = ct.input_output_response(\n", + " clsys_estim, T, [10, xd[1], 0, 0], X0=[0, -3, 0, 0, -5, 0])\n", + "\n", + "plt.plot(clresp_nonoise.outputs[0], clresp_nonoise.outputs[1])\n", + "plt.plot(xd[0], xd[1], 'r--')\n", + "\n", + "plt.xlabel(\"$x$ [m]\")\n", + "plt.ylabel(\"$y$ [m]\")\n", + "plt.gca().set_aspect('equal')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Add some noise\n", + "Qv = np.diag([0.1**2, 0.01**2])\n", + "Qw = np.eye(2) * 0.1**2\n", + "\n", + "u_noise = ct.white_noise(T, Qv)\n", + "y_noise = ct.white_noise(T, Qw)\n", + "\n", + "# Run a simulation\n", + "clresp_noisy = ct.input_output_response(\n", + " clsys_estim, T, [10, xd[1], y_noise], X0=[0, -3, 0, 0, -5, 0])\n", + "\n", + "plt.plot(clresp_direct.outputs[0], clresp_direct.outputs[1], label='direct')\n", + "plt.plot(clresp_nonoise.outputs[0], clresp_nonoise.outputs[1], label='nonoise')\n", + "plt.plot(clresp_noisy.outputs[0], clresp_noisy.outputs[1], label='noisy')\n", + "plt.legend()\n", + "plt.plot(xd[0], xd[1], 'r--')\n", + "\n", + "plt.xlabel(\"$x$ [m]\")\n", + "plt.ylabel(\"$y$ [m]\")\n", + "plt.gca().set_aspect('equal')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the differences in y to make differences more clear\n", + "plt.plot(\n", + " clresp_nonoise.time, clresp_nonoise.outputs[1] - clresp_direct.outputs[1],\n", + " label='nonoise')\n", + "plt.plot(\n", + " clresp_noisy.time, clresp_noisy.outputs[1] - clresp_direct.outputs[1],\n", + " label='noisy')\n", + "plt.legend()\n", + "plt.plot([clresp_nonoise.time[0], clresp_nonoise.time[-1]], [0, 0], 'r--')\n", + "\n", + "plt.xlabel(\"Time [s]\")\n", + "plt.ylabel(\"$y$ [m]\");" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Show the control inputs as well as the final trajectory\n", + "plot_lanechange(T, xd, ud, label=\"desired\")\n", + "plot_lanechange(T, clresp_noisy.outputs[0:2], clresp_noisy.outputs[-2:], label=\"actual\")" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "ZfxhaU9p_W4w" + }, + "source": [ + "### Things to try\n", + "\n", + "* Wrap a controller around the velocity (or $x$ position) in addition to the lateral ($y$) position\n", + "* Change the amounts of noise in the sensor signal\n", + "* Add disturbances to the dynamics (corresponding to wind, hills, etc)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/cds110-L6a_kincar-trajgen.ipynb b/examples/cds110-L6a_kincar-trajgen.ipynb new file mode 100644 index 000000000..e139272bd --- /dev/null +++ b/examples/cds110-L6a_kincar-trajgen.ipynb @@ -0,0 +1,533 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "edb7e2c6", + "metadata": { + "id": "edb7e2c6" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 6a

\n", + "

Trajectory Generation for a Kinematic Car Model

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1vBFjCU2W6fSavy8loL0JfgZyO6UC46m3)\n", + "\n", + "This notebook contains an example of using (optimal) trajectory generation for a vehicle steering system. It illustrates different methods of setting up optimal control problems and solving them using python-control." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7066eb69", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import time\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "import control.optimal as opt" + ] + }, + { + "cell_type": "markdown", + "id": "4afb09dd", + "metadata": { + "id": "4afb09dd" + }, + "source": [ + "## Vehicle steering dynamics\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\large\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "The vehicle dynamics are given by a simple bicycle model. We take the state of the system as $(x, y, \\theta)$ where $(x, y)$ is the position of the vehicle in the plane and $\\theta$ is the angle of the vehicle with respect to horizontal. The vehicle input is given by $(v, \\delta)$ where $v$ is the forward velocity of the vehicle and $\\delta$ is the angle of the steering wheel. The model includes saturation of the vehicle steering angle." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a6143a8a", + "metadata": {}, + "outputs": [], + "source": [ + "# Code to model vehicle steering dynamics\n", + "\n", + "# Function to compute the RHS of the system dynamics\n", + "def kincar_update(t, x, u, params):\n", + " # Get the parameters for the model\n", + " l = params['wheelbase'] # vehicle wheelbase\n", + " deltamax = params['maxsteer'] # max steering angle (rad)\n", + "\n", + " # Saturate the steering input\n", + " delta = np.clip(u[1], -deltamax, deltamax)\n", + "\n", + " # Return the derivative of the state\n", + " return np.array([\n", + " np.cos(x[2]) * u[0], # xdot = cos(theta) v\n", + " np.sin(x[2]) * u[0], # ydot = sin(theta) v\n", + " (u[0] / l) * np.tan(delta) # thdot = v/l tan(delta)\n", + " ])\n", + "\n", + "kincar_params={'wheelbase': 3, 'maxsteer': 0.5}\n", + "\n", + "# Create nonlinear input/output system\n", + "kincar = ct.nlsys(\n", + " kincar_update, None, name=\"kincar\", params=kincar_params,\n", + " inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n", + " states=('x', 'y', 'theta'))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4c2bf8d6-7580-4712-affc-928a8b046d8a", + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function to plot lane change manuever\n", + "def plot_lanechange(t, y, u, figure=None, yf=None, label=None):\n", + " # Plot the xy trajectory\n", + " plt.subplot(3, 1, 1, label='xy')\n", + " plt.plot(y[0], y[1], label=label)\n", + " plt.xlabel(\"x [m]\")\n", + " plt.ylabel(\"y [m]\")\n", + " if yf is not None:\n", + " plt.plot(yf[0], yf[1], 'ro')\n", + "\n", + " # Plot x and y as functions of time\n", + " plt.subplot(3, 2, 3, label='x')\n", + " plt.plot(t, y[0])\n", + " plt.ylabel(\"$x$ [m]\")\n", + "\n", + " plt.subplot(3, 2, 4, label='y')\n", + " plt.plot(t, y[1])\n", + " plt.ylabel(\"$y$ [m]\")\n", + "\n", + " # Plot the inputs as a function of time\n", + " plt.subplot(3, 2, 5, label='v')\n", + " plt.plot(t, u[0])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$v$ [m/s]\")\n", + "\n", + " plt.subplot(3, 2, 6, label='delta')\n", + " plt.plot(t, u[1])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$\\\\delta$ [rad]\")\n", + "\n", + " plt.subplot(3, 1, 1)\n", + " plt.title(\"Lane change manuever\")\n", + " if label:\n", + " plt.legend()\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "64bd3c3b", + "metadata": { + "id": "64bd3c3b" + }, + "source": [ + "## Optimal trajectory generation\n", + "\n", + "The general problem we are solving is of the form:\n", + "\n", + "$$\n", + "\\min_{u(\\cdot)}\n", + " \\int_0^T L(x,u)\\, dt + V \\bigl( x(T) \\bigr)\n", + "$$\n", + "subject to\n", + "$$\n", + " \\dot x = f(x, u), \\qquad x\\in \\mathcal{X} \\subset \\mathbb{R}^n,\\, u\\in \\mathcal{U} \\subset \\mathbb{R}^m\n", + "$$\n", + "\n", + "We consider the problem of changing from one lane to another over a perod of 10 seconds while driving at a forward speed of 10 m/s." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "42dcbd79", + "metadata": {}, + "outputs": [], + "source": [ + "# Initial and final conditions\n", + "x0 = np.array([ 0., -2., 0.]); u0 = np.array([10., 0.])\n", + "xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])\n", + "Tf = 10" + ] + }, + { + "cell_type": "markdown", + "id": "5ff2e044", + "metadata": { + "id": "5ff2e044" + }, + "source": [ + "An important part of the optimization procedure is to give a good initial guess. Here are some possibilities:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "650d321a", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the time horizon (and spacing) for the optimization\n", + "# timepts = np.linspace(0, Tf, 5, endpoint=True) # Try using this and see what happens\n", + "# timepts = np.linspace(0, Tf, 10, endpoint=True) # Try using this and see what happens\n", + "timepts = np.linspace(0, Tf, 20, endpoint=True)\n", + "\n", + "# Compute some initial guesses to use\n", + "bend_left = [10, 0.01] # slight left veer (will extend over all timepts)\n", + "straight_line = ( # straight line from start to end with nominal input\n", + " np.array([x0 + (xf - x0) * t/Tf for t in timepts]).transpose(),\n", + " u0\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "4e75a2c4", + "metadata": { + "id": "4e75a2c4" + }, + "source": [ + "### Approach 1: standard quadratic cost\n", + "\n", + "We can set up the optimal control problem as trying to minimize the distance from the desired final point while at the same time as not exerting too much control effort to achieve our goal.\n", + "\n", + "$$\n", + "\\min_{u(\\cdot)}\n", + " \\int_0^T \\left[(x(\\tau) - x_\\text{f})^T Q_x (x(\\tau) - x_\\text{f}) + (u(\\tau) - u_\\text{f})^T Q_u (u(\\tau) - u_\\text{f})\\right] \\, d\\tau\n", + "$$\n", + "subject to\n", + "$$\n", + " \\dot x = f(x, u), \\qquad x \\in \\mathbb{R}^n,\\, u \\in \\mathbb{R}^m\n", + "$$\n", + "\n", + "The optimization module solves optimal control problems by choosing the values of the input at each point in the time horizon to try to minimize the cost:\n", + "\n", + "$$\n", + "u_i(t_j) = \\alpha_{i, j}, \\qquad\n", + "u_i(t) = \\frac{t_{i+1} - t}{t_{i+1} - t_i} \\alpha_{i, j} + \\frac{t - t_i}{t_{i+1} - t_i} \\alpha_{{i+1},j}\n", + "$$\n", + "\n", + "This means that each input generates a parameter value at each point in the time horizon, so the more refined your time horizon, the more parameters the optimizer has to search over." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "984c2f0b", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the cost functions\n", + "Qx = np.diag([.1, 10, .1]) # keep lateral error low\n", + "Qu = np.diag([.1, 1]) # minimize applied inputs\n", + "quad_cost = opt.quadratic_cost(kincar, Qx, Qu, x0=xf, u0=uf)\n", + "\n", + "# Compute the optimal control, setting step size for gradient calculation (eps)\n", + "start_time = time.process_time()\n", + "result1 = opt.solve_ocp(\n", + " kincar, timepts, x0, quad_cost,\n", + " initial_guess=straight_line,\n", + " # initial_guess= bend_left,\n", + " # initial_guess=u0,\n", + " # minimize_method='trust-constr',\n", + " # minimize_options={'finite_diff_rel_step': 0.01},\n", + " # trajectory_method='shooting'\n", + " # solve_ivp_method='LSODA'\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result1.states, result1.inputs, xf)\n", + "print(\"Final computed state: \", result1.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t1, u1 = result1.time, result1.inputs\n", + "t1, y1 = ct.input_output_response(kincar, timepts, u1, x0)\n", + "plot_lanechange(t1, y1, u1, yf=xf[0:2])\n", + "print(\"Final simulated state:\", y1[:,-1])\n", + "\n", + "# Label the different lines\n", + "plt.subplot(3, 1, 1)\n", + "plt.legend(['desired', 'simulated', 'endpoint'])\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "b7cade52", + "metadata": { + "id": "b7cade52" + }, + "source": [ + "Note the amount of time required to solve the problem and also any warning messages about to being able to solve the optimization (mainly in earlier versions of python-control). You can try to adjust a number of factors to try to get a better solution:\n", + "* Try changing the number of points in the time horizon\n", + "* Try using a different initial guess\n", + "* Try changing the optimization method (see commented out code)" + ] + }, + { + "cell_type": "markdown", + "id": "6a9f9d9b", + "metadata": { + "id": "6a9f9d9b" + }, + "source": [ + "### Approach 2: input cost, input constraints, terminal cost\n", + "\n", + "The previous solution integrates the position error for the entire horizon, and so the car changes lanes very quickly (at the cost of larger inputs). Instead, we can penalize the final state and impose a higher cost on the inputs, resulting in a more gradual lane change.\n", + "\n", + "$$\n", + "\\min_{u(\\cdot)}\n", + " \\int_0^T \\underbrace{\\left[x(\\tau)^T Q_x x(\\tau) + (u(\\tau) - u_\\text{f})^T Q_u (u(\\tau) - u_\\text{f})\\right]}_{L(x, u)} \\, d\\tau + \\underbrace{(x(T) - x_\\text{f})^T Q_\\text{f} (x(T) - x_\\text{f})}_{V\\left(x(T)\\right)}\n", + "$$\n", + "subject to\n", + "$$\n", + " \\dot x = f(x, u), \\qquad x \\in \\mathbb{R}^n,\\, u \\in \\mathbb{R}^m\n", + "$$\n", + "\n", + "We can also try using a different solver for this example. You can pass the solver using the `minimize_method` keyword and send options to the solver using the `minimize_options` keyword (which should be set to a dictionary of options)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a201e33c", + "metadata": {}, + "outputs": [], + "source": [ + "# Add input constraint, input cost, terminal cost\n", + "constraints = [ opt.input_range_constraint(kincar, [8, -0.1], [12, 0.1]) ]\n", + "traj_cost = opt.quadratic_cost(kincar, None, np.diag([0.1, 1]), u0=uf)\n", + "term_cost = opt.quadratic_cost(kincar, np.diag([1, 10, 100]), None, x0=xf)\n", + "\n", + "# Compute the optimal control\n", + "start_time = time.process_time()\n", + "result2 = opt.solve_ocp(\n", + " kincar, timepts, x0, traj_cost, constraints, terminal_cost=term_cost,\n", + " initial_guess=straight_line,\n", + " # minimize_method='trust-constr',\n", + " # minimize_options={'finite_diff_rel_step': 0.01},\n", + " # minimize_method='SLSQP', minimize_options={'eps': 0.01},\n", + " # log=True,\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result2.states, result2.inputs, xf)\n", + "print(\"Final computed state: \", result2.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t2, u2 = result2.time, result2.inputs\n", + "t2, y2 = ct.input_output_response(kincar, timepts, u2, x0)\n", + "plot_lanechange(t2, y2, u2, yf=xf[0:2])\n", + "print(\"Final simulated state:\", y2[:,-1])\n", + "\n", + "# Label the different lines\n", + "plt.subplot(3, 1, 1)\n", + "plt.legend(['desired', 'simulated', 'endpoint'], loc='upper left')\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "3d2ccf97", + "metadata": { + "id": "3d2ccf97" + }, + "source": [ + "### Approach 3: terminal constraints\n", + "\n", + "We can also remove the cost function on the state and replace it with a terminal *constraint* on the state as well as bounds on the inputs. If a solution is found, it guarantees we get to exactly the final state:\n", + "\n", + "$$\n", + "\\min_{u(\\cdot)}\n", + " \\int_0^T \\underbrace{(u(\\tau) - u_\\text{f})^T Q_u (u(\\tau) - u_\\text{f})}_{L(x, u)} \\, d\\tau\n", + "$$\n", + "subject to\n", + "$$\n", + " \\begin{aligned}\n", + " \\dot x &= f(x, u), & \\qquad &x \\in \\mathbb{R}^n,\\, u \\in \\mathbb{R}^m \\\\\n", + " x(T) &= x_\\text{f} & &u_\\text{lb} \\leq u(t) \\leq u_\\text{ub},\\, \\text{for all $t$}\n", + " \\end{aligned}\n", + "$$\n", + "\n", + "Note that trajectory and terminal constraints can be very difficult to satisfy for a general optimization." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "dc77a856", + "metadata": {}, + "outputs": [], + "source": [ + "# Input cost and terminal constraints\n", + "R = np.diag([1, 1]) # minimize applied inputs\n", + "cost3 = opt.quadratic_cost(kincar, np.zeros((3,3)), R, u0=uf)\n", + "constraints = [\n", + " opt.input_range_constraint(kincar, [8, -0.1], [12, 0.1]) ]\n", + "terminal = [ opt.state_range_constraint(kincar, xf, xf) ]\n", + "\n", + "# Compute the optimal control\n", + "start_time = time.process_time()\n", + "result3 = opt.solve_ocp(\n", + " kincar, timepts, x0, cost3, constraints,\n", + " terminal_constraints=terminal, initial_guess=straight_line,\n", + "# solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},\n", + "# minimize_method='trust-constr',\n", + "# minimize_options={'finite_diff_rel_step': 0.01},\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result3.states, result3.inputs, xf)\n", + "print(\"Final computed state: \", result3.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t3, u3 = result3.time, result3.inputs\n", + "t3, y3 = ct.input_output_response(kincar, timepts, u3, x0)\n", + "plot_lanechange(t3, y3, u3, yf=xf[0:2])\n", + "print(\"Final state: \", y3[:,-1])\n", + "\n", + "# Label the different lines\n", + "plt.subplot(3, 1, 1)\n", + "plt.legend(['desired', 'simulated', 'endpoint'], loc='upper left')\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "9e744463", + "metadata": { + "id": "9e744463" + }, + "source": [ + "### Approach 4: terminal constraints w/ basis functions (if time)\n", + "\n", + "As a final example, we can use a basis function to reduce the size of the problem and get faster answers with more temporal resolution:\n", + "\n", + "$$\n", + "\\min_{u(\\cdot)}\n", + " \\int_0^T L(x, u) \\, d\\tau + V\\left(x(T)\\right)\n", + "$$\n", + "subject to\n", + "$$\n", + " \\begin{aligned}\n", + " \\dot x &= f(x, u), \\qquad x \\in \\mathcal{X} \\subset \\mathbb{R}^n,\\, u \\in \\mathcal{U} \\subset \\mathbb{R}^m \\\\\n", + " u(t) &= \\sum_i \\alpha_i \\phi^i(t),\n", + " \\end{aligned}\n", + "$$\n", + "where $\\phi^i(t)$ are a set of basis functions.\n", + "\n", + "Here we parameterize the input by a set of 4 Bezier curves but solve for a much more time resolved set of inputs. Note that while we are using the `control.flatsys` module to define the basis functions, we are not exploiting the fact that the system is differentially flat." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ee82aa25", + "metadata": {}, + "outputs": [], + "source": [ + "# Get basis functions for flat systems module\n", + "import control.flatsys as flat\n", + "\n", + "# Compute the optimal control\n", + "start_time = time.process_time()\n", + "result4 = opt.solve_ocp(\n", + " kincar, timepts, x0, quad_cost, constraints,\n", + " terminal_constraints=terminal,\n", + " initial_guess=straight_line,\n", + " basis=flat.PolyFamily(4, T=Tf),\n", + " # solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},\n", + " # solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},\n", + " # minimize_method='trust-constr', minimize_options={'disp': True},\n", + " log=False\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result4.states, result4.inputs, xf)\n", + "print(\"Final computed state: \", result3.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t4, u4 = result4.time, result4.inputs\n", + "t4, y4 = ct.input_output_response(kincar, timepts, u4, x0)\n", + "plot_lanechange(t4, y4, u4, yf=xf[0:2])\n", + "print(\"Final simulated state: \", y4[:,-1])\n", + "\n", + "# Label the different lines\n", + "plt.subplot(3, 1, 1)\n", + "plt.legend(['desired', 'simulated', 'endpoint'], loc='upper left')\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "2a74388e", + "metadata": { + "id": "2a74388e" + }, + "source": [ + "Note how much smoother the inputs look, although the solver can still have a hard time satisfying the final constraints, resulting in longer computation times." + ] + }, + { + "cell_type": "markdown", + "id": "1465d149", + "metadata": { + "id": "1465d149" + }, + "source": [ + "### Additional things to try\n", + "\n", + "* Compare the results here with what we go last week exploiting the property of differential flatness (computation time, in particular)\n", + "* Try using different weights, solvers, initial guess and other properties and see how things change.\n", + "* Try using different values for `initial_guess` to get faster convergence and/or different classes of solutions." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "02bad3d5", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L6b_kincar-tracking.ipynb b/examples/cds110-L6b_kincar-tracking.ipynb new file mode 100644 index 000000000..9f4cbb475 --- /dev/null +++ b/examples/cds110-L6b_kincar-tracking.ipynb @@ -0,0 +1,509 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "exempt-legislation", + "metadata": { + "id": "exempt-legislation" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 6b

\n", + "

Trajectory Tracking for a Kinematic Car

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/12VSFMqM6HVyj8TY_3zb0AnsJrG6UeLKF)\n", + "\n", + "This notebook contains an example of using trajectory tracking for a (nonlinear) state space system. The controller is of the form\n", + "\n", + "$$\n", + " u = u_\\text{d} − K (x − x_\\text{d}),\n", + "$$\n", + "\n", + "where $x_\\text{d}, u_\\text{d}$ is a feasible trajectory, and $K$ is a feedback gain first computed around a nominal condition and then computed using gain scheduling." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "corresponding-convenience", + "metadata": {}, + "outputs": [], + "source": [ + "# Import the packages needed for the examples included in this notebook\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import itertools\n", + "from cmath import sqrt\n", + "from math import pi\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs" + ] + }, + { + "cell_type": "markdown", + "id": "corporate-sense", + "metadata": { + "id": "corporate-sense" + }, + "source": [ + "## Vehicle Steering Dynamics\n", + "\n", + "The vehicle dynamics are given by a simple bicycle model:\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\\large\n", + "\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "We take the state of the system as $(x, y, \\theta)$ where $(x, y)$ is the position of the vehicle in the plane and $\\theta$ is the angle of the vehicle with respect to horizontal. The vehicle input is given by $(v, \\delta)$ where $v$ is the forward velocity of the vehicle and $\\delta$ is the angle of the steering wheel. The model includes saturation of the vehicle steering angle." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "naval-pizza", + "metadata": {}, + "outputs": [], + "source": [ + "# Code to model vehicle steering dynamics\n", + "\n", + "# Function to compute the RHS of the system dynamics\n", + "def kincar_update(t, x, u, params):\n", + " # Get the parameters for the model\n", + " l = params['wheelbase'] # vehicle wheelbase\n", + " deltamax = params['maxsteer'] # max steering angle (rad)\n", + "\n", + " # Saturate the steering input\n", + " delta = np.clip(u[1], -deltamax, deltamax)\n", + "\n", + " # Return the derivative of the state\n", + " return np.array([\n", + " np.cos(x[2]) * u[0], # xdot = cos(theta) v\n", + " np.sin(x[2]) * u[0], # ydot = sin(theta) v\n", + " (u[0] / l) * np.tan(delta) # thdot = v/l tan(delta)\n", + " ])\n", + "\n", + "kincar_params={'wheelbase': 3, 'maxsteer': 0.5}\n", + "\n", + "# Create nonlinear input/output system\n", + "kincar = ct.nlsys(\n", + " kincar_update, None, name=\"kincar\", params=kincar_params,\n", + " inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n", + " states=('x', 'y', 'theta'))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6340dbd4-7867-47ad-aefb-1bea7f6ad566", + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function to plot lane change manuever\n", + "def plot_lanechange(t, y, u, figure=None, yf=None, label=None):\n", + " # Plot the xy trajectory\n", + " plt.subplot(3, 1, 1, label='xy')\n", + " plt.plot(y[0], y[1], label=label)\n", + " plt.xlabel(\"x [m]\")\n", + " plt.ylabel(\"y [m]\")\n", + " if yf is not None:\n", + " plt.plot(yf[0], yf[1], 'ro')\n", + "\n", + " # Plot x and y as functions of time\n", + " plt.subplot(3, 2, 3, label='x')\n", + " plt.plot(t, y[0])\n", + " plt.ylabel(\"$x$ [m]\")\n", + "\n", + " plt.subplot(3, 2, 4, label='y')\n", + " plt.plot(t, y[1])\n", + " plt.ylabel(\"$y$ [m]\")\n", + "\n", + " # Plot the inputs as a function of time\n", + " plt.subplot(3, 2, 5, label='v')\n", + " plt.plot(t, u[0])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$v$ [m/s]\")\n", + "\n", + " plt.subplot(3, 2, 6, label='delta')\n", + " plt.plot(t, u[1])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$\\\\delta$ [rad]\")\n", + "\n", + " plt.subplot(3, 1, 1)\n", + " plt.title(\"Lane change manuever\")\n", + " if label:\n", + " plt.legend()\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "BAsKLMWWK3W2", + "metadata": { + "id": "BAsKLMWWK3W2" + }, + "source": [ + "## State feedback controller\n", + "\n", + "We start by designing a state feedback controller that can be used to stabilize the system. We design the controller around a nominal forward speed of 10 m/s and then apply this to the vehicle at different speeds." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "g7DztIjmK2K_", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the linearization of the dynamics at a nominal point\n", + "x_nom = np.array([0, 0, 0])\n", + "u_nom = np.array([5, 0])\n", + "P = ct.linearize(kincar, x_nom, u_nom) # Linearized systems\n", + "print(P)\n", + "\n", + "Qx = np.diag([1, 10, 0.1])\n", + "Qu = np.diag([1, 1])\n", + "K, _, _ = ct.lqr(P.A, P.B, Qx, Qu)\n", + "print(K)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "szvKKh6rLgkt", + "metadata": {}, + "outputs": [], + "source": [ + "# Create the closed loop system using create_statefbk_iosystem\n", + "?ct.create_statefbk_iosystem\n", + "ctrl, clsys = ct.create_statefbk_iosystem(\n", + " kincar, K, xd_labels=['xd', 'yd', 'thetad'], ud_labels=['vd', 'deltad'])\n", + "print(clsys)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "gow-ZEerMCw7", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a trajectory corresponding to a slow lane change\n", + "x0 = np.array([0, -2, 0]); u0 = [10, 0]\n", + "xf = np.array([100, 2, 0])\n", + "Tf = 10\n", + "timepts = np.linspace(0, Tf, 20)\n", + "\n", + "straight_line = ( # straight line from start to end with nominal input\n", + " np.array([x0 + (xf - x0) * t/Tf for t in timepts]).transpose(),\n", + " u0\n", + ")\n", + "\n", + "desired = opt.solve_ocp(\n", + " kincar, timepts, x0,\n", + " cost=opt.quadratic_cost(kincar, None, Qu, u0=u0),\n", + " terminal_constraints=opt.state_range_constraint(kincar, xf, xf),\n", + " initial_guess=straight_line)\n", + "\n", + "plot_lanechange(desired.time, desired.states, desired.inputs, yf=xf)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "NLa4dbI8PWhY", + "metadata": {}, + "outputs": [], + "source": [ + "# Simulate the system with an initial condition error\n", + "# Use t_eval to evaluate at points between inputs\n", + "actual = ct.input_output_response(\n", + " clsys, timepts, [desired.states, desired.inputs],\n", + " X0=[-3, -5, 0], t_eval=np.linspace(0, Tf, 500))\n", + "\n", + "plot_lanechange(actual.time, actual.states, actual.outputs[3:])\n", + "plot_lanechange(desired.time, desired.states, desired.inputs, yf=xf)" + ] + }, + { + "cell_type": "markdown", + "id": "TKyc2jOiWJBe", + "metadata": { + "id": "TKyc2jOiWJBe" + }, + "source": [ + "Note that the value of $\\delta$ is very large at the start. This is truncated in the model so that it does not exceed $\\pm 0.5$ rad." + ] + }, + { + "cell_type": "markdown", + "id": "6c6c4b9b", + "metadata": { + "id": "6c6c4b9b" + }, + "source": [ + "## Reference trajectory subsystem\n", + "\n", + "In addition to generating a trajectory for the system, we can also create $x_\\text{d}$ and $u_\\text{d}$ corresponding to reference inputs $r_y$ and $r_v$.\n", + "\n", + "The reference trajectory block below generates a simple trajectory for the system given the desired speed (vref) and lateral position (yref). The trajectory consists of a straight line of the form (vref * t, yref, 0) with nominal\n", + "input (vref, 0)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "significant-november", + "metadata": {}, + "outputs": [], + "source": [ + "# System state: none\n", + "# System input: vref, yref\n", + "# System output: xd, yd, thetad, vd, deltad\n", + "# System parameters: none\n", + "#\n", + "def trajgen_output(t, x, u, params):\n", + " vref, yref = u\n", + " return np.array([vref * t, yref, 0, vref, 0])\n", + "\n", + "# Define the trajectory generator as an input/output system\n", + "trajgen = ct.nlsys(\n", + " None, trajgen_output, name='trajgen',\n", + " inputs=('vref', 'yref'),\n", + " outputs=('xd', 'yd', 'thetad', 'vd', 'deltad'))\n", + "\n", + "print(trajgen)" + ] + }, + { + "cell_type": "markdown", + "id": "0w5s56uUWw-v", + "metadata": { + "id": "0w5s56uUWw-v" + }, + "source": [ + "## Step responses\n", + "\n", + "To explore the dynamics of the system, we create a set of lane changes at different forward speeds. Since the linearization depends on the speed, this means that the closed loop performance of the system will vary." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "mtGLwMQkXEzw", + "metadata": {}, + "outputs": [], + "source": [ + "steering_fixed = ct.interconnect(\n", + " [kincar, ctrl, trajgen],\n", + " inputs=['vref', 'yref'],\n", + " outputs=kincar.output_labels + kincar.input_labels\n", + ")\n", + "print(steering_fixed)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "sz7NaJTGXua1", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the simulation conditions\n", + "yref = 1\n", + "T = np.linspace(0, 5, 100)\n", + "\n", + "# Do an iteration through different speeds\n", + "for vref in [2, 5, 20]:\n", + " # Simulate the closed loop controller response\n", + " tout, yout = ct.input_output_response(\n", + " steering_fixed, T, [vref * np.ones(len(T)), yref * np.ones(len(T))],\n", + " params={'maxsteer': 1})\n", + "\n", + " # Plot the results\n", + " plot_lanechange(tout, yout, yout[3:])\n", + "\n", + "# Label the different curves\n", + "plt.subplot(3, 1, 1)\n", + "plt.legend([\"$v_d$ = \" + f\"{vref}\" for vref in [2, 10, 20]])\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "3cc26675", + "metadata": { + "id": "3cc26675" + }, + "source": [ + "## Gain scheduled controller\n", + "\n", + "For this system we use a simple schedule on the forward vehicle velocity and\n", + "place the poles of the system at fixed values. The controller takes the\n", + "current and desired vehicle position and orientation plus the velocity\n", + "velocity as inputs, and returns the velocity and steering commands.\n", + "\n", + "Linearizing the system about the desired trajectory, we obtain\n", + "\n", + "$$\n", + " \\begin{aligned}\n", + " A(x_\\text{d}) &= \\left. \\frac{\\partial f}{\\partial x} \\right|_{(x_\\text{d}, u_\\text{d})}\n", + " = \\left.\n", + " \\begin{bmatrix}\n", + " 0 & 0 & -\\sin\\theta_\\text{d}\\, v_\\text{d} \\\\ 0 & 0 & \\cos\\theta_\\text{d}\\, v_\\text{d} \\\\ 0 & 0 & 0\n", + " \\end{bmatrix}\n", + " \\right|_{(x_\\text{d}, u_\\text{d})}\n", + " = \\begin{bmatrix}\n", + " 0 & 0 & 0 \\\\ 0 & 0 & v_\\text{d} \\\\ 0 & 0 & 0\n", + " \\end{bmatrix}, \\\\\n", + " B(x_\\text{d}) &= \\left. \\frac{\\partial f}{\\partial u} \\right|_{(x_\\text{d}, u_\\text{d})}\n", + " = \\begin{bmatrix}\n", + " 1 & 0 \\\\ 0 & 0 \\\\ 0 & v_\\text{d}/l\n", + " \\end{bmatrix}.\n", + " \\end{aligned}\n", + "$$\n", + "\n", + "We see that these matrices depend only on $\\theta_\\text{d}$ and $v_\\text{d}$, so we choose these as the scheduling variables and design a controller of the form\n", + "\n", + "$$\n", + "u = u_\\text{d} - K(\\mu) (x - x_\\text{d})\n", + "$$\n", + "\n", + "where $\\mu = (\\theta_\\text{d}, v_\\text{d})$ and we interpolate the gains based on LQR controllers computed at a fixed set of points $\\mu_i$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "another-milwaukee", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the points for the scheduling variables\n", + "gs_speeds = [2, 10, 20]\n", + "gs_angles = np.linspace(-pi, pi, 4)\n", + "\n", + "# Create controllers at each scheduling point (\n", + "points = [np.array([speed, angle])\n", + " for speed in gs_speeds for angle in gs_angles]\n", + "gains = [np.array(ct.lqr(kincar.linearize(\n", + " [0, 0, angle], [speed, 0]), Qx, Qu)[0])\n", + " for speed in gs_speeds for angle in gs_angles]\n", + "print(f\"{points=}\")\n", + "print(f\"{gains=}\")\n", + "\n", + "# Create the gain scheduled system\n", + "ctrl_gs, _ = ct.create_statefbk_iosystem(\n", + " kincar, (gains, points), name='controller',\n", + " xd_labels=['xd', 'yd', 'thetad'], ud_labels=['vd', 'deltad'],\n", + " gainsched_indices=['vd', 'theta'], gainsched_method='linear')\n", + "print(ctrl_gs)" + ] + }, + { + "cell_type": "markdown", + "id": "4ca5ab53", + "metadata": { + "id": "4ca5ab53" + }, + "source": [ + "## System construction\n", + "\n", + "The input to the full closed loop system is the desired lateral position and the desired forward velocity. The output for the system is taken as the full vehicle state plus the velocity of the vehicle.\n", + "\n", + "We construct the system using the `ct.interconnect` function and use signal labels to keep track of everything. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "editorial-satisfaction", + "metadata": {}, + "outputs": [], + "source": [ + "steering_gainsched = ct.interconnect(\n", + " [trajgen, ctrl_gs, kincar], name='steering',\n", + " inputs=['vref', 'yref'],\n", + " outputs=kincar.output_labels + kincar.input_labels\n", + ")\n", + "print(steering_gainsched)" + ] + }, + { + "cell_type": "markdown", + "id": "47f5d528", + "metadata": { + "id": "47f5d528" + }, + "source": [ + "## System simulation\n", + "\n", + "We now simulate the gain scheduled controller for a step input in the $y$ position, using a range of vehicle speeds $v_\\text{d}$:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "smoking-trail", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the reference trajectory for the y position\n", + "# plt.plot([0, 5], [yref, yref], 'k-', linewidth=0.6)\n", + "\n", + "# Find the signals we want to plot\n", + "y_index = steering_gainsched.find_output('y')\n", + "v_index = steering_gainsched.find_output('v')\n", + "\n", + "# Do an iteration through different speeds\n", + "for vref in [2, 5, 20]:\n", + " # Simulate the closed loop controller response\n", + " tout, yout = ct.input_output_response(\n", + " steering_gainsched, T, [vref * np.ones(len(T)), yref * np.ones(len(T))],\n", + " X0=[0, 0, 0], params={'maxsteer': 0.5}\n", + " )\n", + "\n", + " # Plot the results\n", + " plot_lanechange(tout, yout, yout[3:])\n", + "\n", + "# Label the different curves\n", + "plt.subplot(3, 1, 1)\n", + "plt.legend([\"$v_d$ = \" + f\"{vref}\" for vref in [2, 10, 20]])\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6f571b2b", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L6c_doubleint-rhc.ipynb b/examples/cds110-L6c_doubleint-rhc.ipynb new file mode 100644 index 000000000..d9a693a27 --- /dev/null +++ b/examples/cds110-L6c_doubleint-rhc.ipynb @@ -0,0 +1,651 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "9d41c333", + "metadata": { + "id": "9d41c333" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 6c

\n", + "

Receding Horizon Control of a Double Integrator with Bounded Input

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1AufRjpbdKcOEoWO5NEiczF3C8Rc4JuTL)\n", + "\n", + "To illustrate the implementation of a receding horizon controller, we consider a linear system corresponding to a double integrator with bounded input:\n", + "\n", + "$$\n", + " \\dot x = \\begin{bmatrix} 0 & 1 \\\\ 0 & 0 \\end{bmatrix} x + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} \\text{clip}(u)\n", + " \\qquad\\text{where}\\qquad\n", + " \\text{clip}(u) = \\begin{cases}\n", + " -1 & u < -1, \\\\\n", + " u & -1 \\leq u \\leq 1, \\\\\n", + " 1 & u > 1.\n", + " \\end{cases}\n", + "$$\n", + "\n", + "We implement a model predictive controller by choosing\n", + "\n", + "$$\n", + " Q_x = \\begin{bmatrix} 1 & 0 \\\\ 0 & 0 \\end{bmatrix}, \\qquad\n", + " Q_u = \\begin{bmatrix} 1 \\end{bmatrix}, \\qquad\n", + " P_1 = \\begin{bmatrix} 0.1 & 0 \\\\ 0 & 0.1 \\end{bmatrix}.\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4fe0af7f", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "import time\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs" + ] + }, + { + "cell_type": "markdown", + "id": "4c695f81", + "metadata": { + "id": "4c695f81" + }, + "source": [ + "## System definition\n", + "\n", + "The system is defined as a double integrator with bounded input." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5c01f571", + "metadata": {}, + "outputs": [], + "source": [ + "def doubleint_update(t, x, u, params):\n", + " # Get the parameters\n", + " lb = params.get('lb', -1)\n", + " ub = params.get('ub', 1)\n", + " assert lb < ub\n", + "\n", + " # bound the input\n", + " u_clip = np.clip(u, lb, ub)\n", + "\n", + " return np.array([x[1], u_clip[0]])\n", + "\n", + "proc = ct.nlsys(\n", + " doubleint_update, None, name=\"double integrator\",\n", + " inputs = ['u'], outputs=['x[0]', 'x[1]'], states=2)" + ] + }, + { + "cell_type": "markdown", + "id": "6c2f0d00", + "metadata": { + "id": "6c2f0d00" + }, + "source": [ + "## Receding horizon controller\n", + "\n", + "To define a receding horizon controller, we create an optimal control problem (using the `OptimalControlProblem` class) and then use the `compute_trajectory` method to solve for the trajectory from the current state.\n", + "\n", + "We start by defining the cost functions, which consists of a trajectory cost and a terminal cost:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a501efef", + "metadata": {}, + "outputs": [], + "source": [ + "Qx = np.diag([1, 0]) # state cost\n", + "Qu = np.diag([1]) # input cost\n", + "traj_cost=opt.quadratic_cost(proc, Qx, Qu)\n", + "\n", + "P1 = np.diag([0.1, 0.1]) # terminal cost\n", + "term_cost = opt.quadratic_cost(proc, P1, None)" + ] + }, + { + "cell_type": "markdown", + "id": "c5470629", + "metadata": { + "id": "c5470629" + }, + "source": [ + "We also set up a set of constraints the correspond to the fact that the input should have magnitude 1. This can be done using either the [`input_range_constraint`](https://python-control.readthedocs.io/en/0.9.3.post2/generated/control.optimal.input_range_constraint.html) function or the [`input_poly_constraint`](https://python-control.readthedocs.io/en/0.9.3.post2/generated/control.optimal.input_poly_constraint.html) function." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cb4c511a", + "metadata": {}, + "outputs": [], + "source": [ + "traj_constraints = opt.input_range_constraint(proc, -1, 1)\n", + "# traj_constraints = opt.input_poly_constraint(\n", + "# proc, np.array([[1], [-1]]), np.array([1, 1]))" + ] + }, + { + "cell_type": "markdown", + "id": "a5568374", + "metadata": { + "id": "a5568374" + }, + "source": [ + "We define the horizon for evaluating finite-time, optimal control by setting up a set of time points across the designed horizon. The input will be computed at each time point." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9edec673", + "metadata": {}, + "outputs": [], + "source": [ + "Th = 5\n", + "timepts = np.linspace(0, Th, 11, endpoint=True)\n", + "print(timepts)" + ] + }, + { + "cell_type": "markdown", + "id": "cb8fcecc", + "metadata": { + "id": "cb8fcecc" + }, + "source": [ + "Finally, we define the optimal control problem that we want to solve (without actually solving it)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e9f31be6", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the optimal control problem\n", + "ocp = opt.OptimalControlProblem(\n", + " proc, timepts, traj_cost,\n", + " terminal_cost=term_cost,\n", + " trajectory_constraints=traj_constraints,\n", + " # terminal_constraints=term_constraints,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "ee9a39dd", + "metadata": { + "id": "ee9a39dd" + }, + "source": [ + "To make sure that the problem is properly defined, we solve the problem for a specific initial condition. We also compare the amount of time required to solve the problem from a \"cold start\" (no initial guess) versus a \"warm start\" (use the previous solution, shifted forward on point in time)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "887295eb", + "metadata": {}, + "outputs": [], + "source": [ + "X0 = np.array([1, 1])\n", + "\n", + "start_time = time.process_time()\n", + "res = ocp.compute_trajectory(X0, initial_guess=0, return_states=True)\n", + "stop_time = time.process_time()\n", + "print(f'* Cold start: {stop_time-start_time:.3} sec')\n", + "\n", + "# Resolve using previous solution (shifted forward) as initial guess to compare timing\n", + "start_time = time.process_time()\n", + "u = res.inputs\n", + "u_shift = np.hstack([u[:, 1:], u[:, -1:]])\n", + "ocp.compute_trajectory(X0, initial_guess=u_shift, print_summary=False)\n", + "stop_time = time.process_time()\n", + "print(f'* Warm start: {stop_time-start_time:.3} sec')" + ] + }, + { + "cell_type": "markdown", + "id": "115dec26", + "metadata": { + "id": "115dec26" + }, + "source": [ + "(In this case the timing is not that different since the system is very simple.)\n", + "\n", + "Plotting the result, we see that the solution is properly computed." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4b98e773", + "metadata": {}, + "outputs": [], + "source": [ + "plt.plot(res.time, res.states[0], 'k-', label='$x_1$')\n", + "plt.plot(res.time, res.inputs[0], 'b-', label='u')\n", + "plt.xlabel('Time [s]')\n", + "plt.ylabel('$x_1$, $u$')\n", + "plt.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "0e85981a", + "metadata": { + "id": "0e85981a" + }, + "source": [ + "We implement the receding horizon controller using a function that we can use with different versions of the problem." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "eb2e8126", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a figure to use for plotting\n", + "def run_rhc_and_plot(\n", + " proc, ocp, X0, Tf, print_summary=False, verbose=False, ax=None, plot=True):\n", + " # Start at the initial point\n", + " x = X0\n", + "\n", + " # Initialize the axes\n", + " if plot and ax is None:\n", + " ax = plt.axes()\n", + "\n", + " # Initialize arrays to store the final trajectory\n", + " time_, inputs_, outputs_, states_ = [], [], [], []\n", + "\n", + " # Generate the individual traces for the receding horizon control\n", + " for t in ocp.timepts:\n", + " # Compute the optimal trajectory over the horizon\n", + " start_time = time.process_time()\n", + " res = ocp.compute_trajectory(x, print_summary=print_summary)\n", + " if verbose:\n", + " print(f\"{t=}: comp time = {time.process_time() - start_time:0.3}\")\n", + "\n", + " # Simulate the system for the update time, with higher res for plotting\n", + " tvec = np.linspace(0, res.time[1], 20)\n", + " inputs = res.inputs[:, 0] + np.outer(\n", + " (res.inputs[:, 1] - res.inputs[:, 0]) / (tvec[-1] - tvec[0]), tvec)\n", + " soln = ct.input_output_response(proc, tvec, inputs, x)\n", + "\n", + " # Save this segment for later use (final point will appear in next segment)\n", + " time_.append(t + soln.time[:-1])\n", + " inputs_.append(soln.inputs[:, :-1])\n", + " outputs_.append(soln.outputs[:, :-1])\n", + " states_.append(soln.states[:, :-1])\n", + "\n", + " if plot:\n", + " # Plot the results over the full horizon\n", + " h3, = ax.plot(t + res.time, res.states[0], 'k--', linewidth=0.5)\n", + " ax.plot(t + res.time, res.inputs[0], 'b--', linewidth=0.5)\n", + "\n", + " # Plot the results for this time segment\n", + " h1, = ax.plot(t + soln.time, soln.states[0], 'k-')\n", + " h2, = ax.plot(t + soln.time, soln.inputs[0], 'b-')\n", + "\n", + " # Update the state to use for the next time point\n", + " x = soln.states[:, -1]\n", + "\n", + " # Append the final point to the response\n", + " time_.append(t + soln.time[-1:])\n", + " inputs_.append(soln.inputs[:, -1:])\n", + " outputs_.append(soln.outputs[:, -1:])\n", + " states_.append(soln.states[:, -1:])\n", + "\n", + " # Label the plot\n", + " if plot:\n", + " # Adjust the limits for consistency\n", + " ax.set_ylim([-4, 3.5])\n", + "\n", + " # Add reference line for input lower bound\n", + " ax.plot([0, 7], [-1, -1], 'k--', linewidth=0.666)\n", + "\n", + " # Label the results\n", + " ax.set_xlabel(\"Time $t$ [sec]\")\n", + " ax.set_ylabel(\"State $x_1$, input $u$\")\n", + " ax.legend(\n", + " [h1, h2, h3], ['$x_1$', '$u$', 'prediction'],\n", + " loc='lower right', labelspacing=0)\n", + " plt.tight_layout()\n", + "\n", + " # Append\n", + " return ct.TimeResponseData(\n", + " np.hstack(time_), np.hstack(outputs_), np.hstack(states_), np.hstack(inputs_))" + ] + }, + { + "cell_type": "markdown", + "id": "be13e00a", + "metadata": { + "id": "be13e00a" + }, + "source": [ + "Finally, we call the controller and plot the response. The solid lines show the portions of the trajectory that we follow. The dashed lines are the trajectory over the full horizon, but which are not followed since we update the computation at each time step. (To get rid of the statistics of each optimization call, use `print_summary=False`.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "305a1127", + "metadata": {}, + "outputs": [], + "source": [ + "Tf = 10\n", + "rhc_resp = run_rhc_and_plot(proc, ocp, X0, Tf, verbose=True, print_summary=False)\n", + "print(f\"xf = {rhc_resp.states[:, -1]}\")" + ] + }, + { + "cell_type": "markdown", + "id": "6005bfb3", + "metadata": { + "id": "6005bfb3" + }, + "source": [ + "## RHC vs LQR vs LQR terminal cost\n", + "\n", + "In the example above, we used a receding horizon controller with the terminal cost as $P_1 = \\text{diag}(0.1, 0.1)$. An alternative is to set the terminal cost to be the LQR terminal cost that goes along with the trajectory cost, which then provides a \"cost to go\" that matches the LQR \"cost to go\" (but keeping in mind that the LQR controller does not necessarily respect the constraints).\n", + "\n", + "The following code compares the original RHC formulation with a receding horizon controller using an LQR terminal cost versus an LQR controller." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ea2de1f3", + "metadata": {}, + "outputs": [], + "source": [ + "# Get the LQR solution\n", + "K, P_lqr, E = ct.lqr(proc.linearize(0, 0), Qx, Qu)\n", + "print(f\"P_lqr = \\n{P_lqr}\")\n", + "\n", + "# Create an LQR controller (and run it)\n", + "lqr_ctrl, lqr_clsys = ct.create_statefbk_iosystem(proc, K)\n", + "lqr_resp = ct.input_output_response(lqr_clsys, rhc_resp.time, 0, X0)\n", + "\n", + "# Create a new optimal control problem using the LQR terminal cost\n", + "# (need use more refined time grid as well, to approximate LQR rate)\n", + "lqr_timepts = np.linspace(0, Th, 25, endpoint=True)\n", + "lqr_term_cost=opt.quadratic_cost(proc, P_lqr, None)\n", + "ocp_lqr = opt.OptimalControlProblem(\n", + " proc, lqr_timepts, traj_cost, terminal_cost=lqr_term_cost,\n", + " trajectory_constraints=traj_constraints,\n", + ")\n", + "\n", + "# Create the response for the new controller\n", + "rhc_lqr_resp = run_rhc_and_plot(\n", + " proc, ocp_lqr, X0, 10, plot=False, print_summary=False)\n", + "\n", + "# Plot the different responses to compare them\n", + "fig, ax = plt.subplots(2, 1)\n", + "ax[0].plot(rhc_resp.time, rhc_resp.states[0], label='RHC + P_1')\n", + "ax[0].plot(rhc_lqr_resp.time, rhc_lqr_resp.states[0], '--', label='RHC + P_lqr')\n", + "ax[0].plot(lqr_resp.time, lqr_resp.outputs[0], ':', label='LQR')\n", + "ax[0].legend()\n", + "\n", + "ax[1].plot(rhc_resp.time, rhc_resp.inputs[0], label='RHC + P_1')\n", + "ax[1].plot(rhc_lqr_resp.time, rhc_lqr_resp.inputs[0], '--', label='RHC + P_lqr')\n", + "ax[1].plot(lqr_resp.time, lqr_resp.outputs[2], ':', label='LQR')" + ] + }, + { + "cell_type": "markdown", + "id": "9497530b", + "metadata": { + "id": "9497530b" + }, + "source": [ + "## Discrete time RHC\n", + "\n", + "Many receding horizon control problems are solved based on a discrete time model. We show here how to implement this for a \"double integrator\" system, which in discrete time has the form\n", + "\n", + "$$\n", + " x[k+1] = \\begin{bmatrix} 1 & 1 \\\\ 0 & 1 \\end{bmatrix} x[k] + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} \\text{clip}(u[k])\n", + "$$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ae7cefa5", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# System definition\n", + "#\n", + "\n", + "def doubleint_update(t, x, u, params):\n", + " # Get the parameters\n", + " lb = params.get('lb', -1)\n", + " ub = params.get('ub', 1)\n", + " assert lb < ub\n", + "\n", + " # Get the sampling time\n", + " dt = params.get('dt', 1)\n", + "\n", + " # bound the input\n", + " u_clip = np.clip(u, lb, ub)\n", + "\n", + " return np.array([x[0] + dt * x[1], x[1] + dt * u_clip[0]])\n", + "\n", + "proc = ct.nlsys(\n", + " doubleint_update, None, name=\"double integrator\",\n", + " inputs = ['u'], outputs=['x[0]', 'x[1]'], states=2,\n", + " params={'dt': 1}, dt=1)\n", + "\n", + "#\n", + "# Linear quadratic regulator\n", + "#\n", + "\n", + "# Define the cost functions to use\n", + "Qx = np.diag([1, 0]) # state cost\n", + "Qu = np.diag([1]) # input cost\n", + "P1 = np.diag([0.1, 0.1]) # terminal cost\n", + "\n", + "# Get the LQR solution\n", + "K, P, E = ct.dlqr(proc.linearize(0, 0), Qx, Qu)\n", + "\n", + "# Test out the LQR controller, with no constraints\n", + "linsys = proc.linearize(0, 0)\n", + "clsys_lin = ct.ss(linsys.A - linsys.B @ K, linsys.B, linsys.C, 0, dt=proc.dt)\n", + "\n", + "X0 = np.array([2, 1]) # initial conditions\n", + "Tf = 10 # simulation time\n", + "res = ct.initial_response(clsys_lin, Tf, X0=X0)\n", + "\n", + "# Plot the results\n", + "plt.figure(1); plt.clf(); ax = plt.axes()\n", + "ax.plot(res.time, res.states[0], 'k-', label='$x_1$')\n", + "ax.plot(res.time, (-K @ res.states)[0], 'b-', label='$u$')\n", + "\n", + "# Test out the LQR controller with constraints\n", + "clsys_lqr = ct.feedback(proc, -K, 1)\n", + "tvec = np.arange(0, Tf, proc.dt)\n", + "res_lqr_const = ct.input_output_response(clsys_lqr, tvec, 0, X0)\n", + "\n", + "# Plot the results\n", + "ax.plot(res_lqr_const.time, res_lqr_const.states[0], 'k--', label='constrained')\n", + "ax.plot(res_lqr_const.time, (-K @ res_lqr_const.states)[0], 'b--')\n", + "ax.plot([0, 7], [-1, -1], 'k--', linewidth=0.75)\n", + "\n", + "# Adjust the limits for consistency\n", + "ax.set_ylim([-4, 3.5])\n", + "\n", + "# Label the results\n", + "ax.set_xlabel(\"Time $t$ [sec]\")\n", + "ax.set_ylabel(\"State $x_1$, input $u$\")\n", + "ax.legend(loc='lower right', labelspacing=0)\n", + "plt.title(\"Linearized LQR response from x0\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "13cfc5d8", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# Receding horizon controller\n", + "#\n", + "\n", + "# Create the constraints\n", + "traj_constraints = opt.input_range_constraint(proc, -1, 1)\n", + "term_constraints = opt.state_range_constraint(proc, [0, 0], [0, 0])\n", + "\n", + "# Define the optimal control problem we want to solve\n", + "T = 5\n", + "timepts = np.arange(0, T * proc.dt, proc.dt)\n", + "\n", + "# Set up the optimal control problems\n", + "ocp_orig = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P1, None),\n", + ")\n", + "\n", + "ocp_lqr = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P, None),\n", + ")\n", + "\n", + "ocp_low = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P/10, None),\n", + ")\n", + "\n", + "ocp_high = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P*10, None),\n", + ")\n", + "weight_list = [P1, P, P/10, P*10]\n", + "ocp_list = [ocp_orig, ocp_lqr, ocp_low, ocp_high]\n", + "\n", + "# Do a test run to figure out how long computation takes\n", + "start_time = time.process_time()\n", + "ocp_lqr.compute_trajectory(X0)\n", + "stop_time = time.process_time()\n", + "print(\"* Process time: %0.2g s\\n\" % (stop_time - start_time))\n", + "\n", + "# Create a figure to use for plotting\n", + "fig, [[ax_orig, ax_lqr], [ax_low, ax_high]] = plt.subplots(2, 2)\n", + "ax_list = [ax_orig, ax_lqr, ax_low, ax_high]\n", + "ax_name = ['orig', 'lqr', 'low', 'high']\n", + "\n", + "# Generate the individual traces for the receding horizon control\n", + "for ocp, ax, name, Pf in zip(ocp_list, ax_list, ax_name, weight_list):\n", + " x, t = X0, 0\n", + " for i in np.arange(0, Tf, proc.dt):\n", + " # Calculate the optimal trajectory\n", + " res = ocp.compute_trajectory(x, print_summary=False)\n", + " soln = ct.input_output_response(proc, res.time, res.inputs, x)\n", + "\n", + " # Plot the results for this time instant\n", + " ax.plot(res.time[:2] + t, res.inputs[0, :2], 'b-', linewidth=1)\n", + " ax.plot(res.time[:2] + t, soln.outputs[0, :2], 'k-', linewidth=1)\n", + "\n", + " # Plot the results projected forward\n", + " ax.plot(res.time[1:] + t, res.inputs[0, 1:], 'b--', linewidth=0.75)\n", + " ax.plot(res.time[1:] + t, soln.outputs[0, 1:], 'k--', linewidth=0.75)\n", + "\n", + " # Update the state to use for the next time point\n", + " x = soln.states[:, 1]\n", + " t += proc.dt\n", + "\n", + " # Adjust the limits for consistency\n", + " ax.set_ylim([-1.5, 3.5])\n", + "\n", + " # Label the results\n", + " ax.set_xlabel(\"Time $t$ [sec]\")\n", + " ax.set_ylabel(\"State $x_1$, input $u$\")\n", + " ax.set_title(f\"MPC response for {name}\")\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "015dc953", + "metadata": { + "id": "015dc953" + }, + "source": [ + "We can also implement a receding horizon controller for a discrete time system using `opt.create_mpc_iosystem`. This creates a controller that accepts the current state as the input and generates the control to apply from that state." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4f8bb594", + "metadata": {}, + "outputs": [], + "source": [ + "# Construct using create_mpc_iosystem\n", + "clsys = opt.create_mpc_iosystem(\n", + " proc, timepts, opt.quadratic_cost(proc, Qx, Qu), traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P1, None),\n", + ")\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "id": "f1b08fb4", + "metadata": { + "id": "f1b08fb4" + }, + "source": [ + "(This function needs some work to be more user-friendly, e.g. renaming of the inputs and outputs.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d2afd287", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L7_bode-nyquist.ipynb b/examples/cds110-L7_bode-nyquist.ipynb new file mode 100644 index 000000000..6e9f63337 --- /dev/null +++ b/examples/cds110-L7_bode-nyquist.ipynb @@ -0,0 +1,856 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "8c577d78-3e4a-4f08-93ed-5c60867b9a3b", + "metadata": { + "id": "hairy-humidity" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 7

\n", + "

Frequency Domain Analysis using Bode/Nyquist plots

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1-BIaln1nF41fGqavzliuWT74nBkAnM3x)\n", + "\n", + "The purpose of this lecture is to introduce tools that can be used for frequency domain modeling and analysis of linear systems. It illustrates the use of a variety of frequency domain analysis and plotting tools." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "invalid-carnival", + "metadata": {}, + "outputs": [], + "source": [ + "# Import standard packages needed for this exercise\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import math\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "\n", + "# Use ctrlplot defaults for matplotlib\n", + "plt.rcParams.update(ct.rcParams)" + ] + }, + { + "cell_type": "markdown", + "id": "P7t3Nm4Tre2Z", + "metadata": { + "id": "P7t3Nm4Tre2Z" + }, + "source": [ + "## Stable system: servomechanism\n", + "\n", + "We start with a simple example a stable system for which we wish to design a simple controller and analyze its performance, demonstrating along the way the basic frequency domain analysis functions in the Python control toolbox (python-control).\n", + "\n", + "Consider a simple mechanism for positioning a mechanical arm whose equations of motion are given by\n", + "\n", + "$$\n", + "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n", + "$$\n", + "\n", + "which can be written in state space form as\n", + "\n", + "$$\n", + "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\theta \\end{bmatrix} =\n", + " \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n", + " + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n", + "$$\n", + "\n", + "The system consists of a spring loaded arm that is driven by a motor, as shown below.\n", + "\n", + "
\"servomech-diagram\"
\n", + "\n", + "The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached.\n", + "\n", + "The system parameters are given by\n", + "\n", + "$$\n", + "k = 1,\\quad J = 100,\\quad b = 10,\n", + "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01,\n", + "$$\n", + "\n", + "and we assume that time is measured in msec and distance in cm. (The constants here are made up and don't necessarily reflect a real disk drive, though the units and time constants are motivated by computer disk drives.)" + ] + }, + { + "cell_type": "markdown", + "id": "3e476db9", + "metadata": { + "id": "3e476db9" + }, + "source": [ + "The system dynamics can be modeled in python-control using a `NonlinearIOSystem` object, which we create with the `nlsys` function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "27bb3c38", + "metadata": {}, + "outputs": [], + "source": [ + "# Parameter values\n", + "servomech_params = {\n", + " 'J': 100, # Moment of inertia of the motor\n", + " 'b': 10, # Angular damping of the arm\n", + " 'k': 1, # Spring constant\n", + " 'r': 1, # Location of spring contact on arm\n", + " 'l': 2, # Distance to the read head\n", + " 'eps': 0.01, # Magnitude of velocity-dependent perturbation\n", + "}\n", + "\n", + "# State derivative\n", + "def servomech_update(t, x, u, params):\n", + " # Extract the configuration and velocity variables from the state vector\n", + " theta = x[0] # Angular position of the disk drive arm\n", + " thetadot = x[1] # Angular velocity of the disk drive arm\n", + " tau = u[0] # Torque applied at the base of the arm\n", + "\n", + " # Get the parameter values\n", + " J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])\n", + "\n", + " # Compute the angular acceleration\n", + " dthetadot = 1/J * (\n", + " -b * thetadot - k * r * np.sin(theta) + tau)\n", + "\n", + " # Return the state update law\n", + " return np.array([thetadot, dthetadot])\n", + "\n", + "# System output (end of arm)\n", + "def servomech_output(t, x, u, params):\n", + " l = params['l']\n", + " return np.array([l * x[0]])\n", + "\n", + "# System dynamics\n", + "servomech = ct.nlsys(\n", + " servomech_update, servomech_output, name='servomech',\n", + " params=servomech_params,\n", + " states=['theta_', 'thdot_'],\n", + " outputs=['y'], inputs=['tau'])\n", + "\n", + "print(servomech)\n", + "print(\"\\nParams:\", servomech.params)" + ] + }, + { + "cell_type": "markdown", + "id": "competitive-terrain", + "metadata": { + "id": "competitive-terrain" + }, + "source": [ + "### Linearization\n", + "\n", + "To study the open loop dynamics of the system, we compute the linearization of the dynamics about the equilibrium point corresponding to $\\theta_\\text{e} = 15^\\circ$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "senior-carpet", + "metadata": {}, + "outputs": [], + "source": [ + "# Convert the equilibrium angle to radians\n", + "theta_e = (15 / 180) * np.pi\n", + "\n", + "# Compute the input required to hold this position\n", + "u_e = servomech.params['k'] * servomech.params['r'] * np.sin(theta_e)\n", + "print(\"Equilibrium torque = %g\" % u_e)\n", + "\n", + "# Linearize the system about the equilibrium point\n", + "P = servomech.linearize([theta_e, 0], u_e, name='P_ss')\n", + "P.name = 'P_ss' # TODO: fix in nlsys_improvements\n", + "print(\"Linearized dynamics:\", P)\n", + "print(\"Zeros: \", P.zeros())\n", + "print(\"Poles: \", P.poles())\n", + "print(\"\")\n", + "\n", + "# Transfer function representation\n", + "P_tf = ct.tf(P, name='P_tf')\n", + "print(P_tf)" + ] + }, + { + "cell_type": "markdown", + "id": "instant-lancaster", + "metadata": { + "id": "instant-lancaster" + }, + "source": [ + "### Open loop frequency response\n", + "\n", + "A standard method for understanding the dynamics is to plot the output of the system in response to sinusoids with unit magnitude at different frequencies.\n", + "\n", + "We use the `frequency_response` function to plot the step response of the linearized, open-loop system." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "RxXFTpwO5bGI", + "metadata": {}, + "outputs": [], + "source": [ + "# Reset the frequency response label to correspond to a time unit of ms\n", + "ct.set_defaults('freqplot', freq_label=\"Frequency [rad/ms]\")\n", + "\n", + "# Frequency response\n", + "freqresp = ct.frequency_response(P, np.logspace(-2, 0))\n", + "freqresp.plot()\n", + "\n", + "# Equivalent command\n", + "ct.bode_plot(P_tf, np.logspace(-2, 0), '--')" + ] + }, + { + "cell_type": "markdown", + "id": "stuffed-premiere", + "metadata": { + "id": "stuffed-premiere" + }, + "source": [ + "### Feedback control design\n", + "\n", + "We next design a feedback controller for the system using a proportional integral controller, which has transfer function\n", + "\n", + "$$\n", + "C(s) = \\frac{k_\\text{p} s + k_\\text{i}}{s}\n", + "$$\n", + "\n", + "We will learn how to choose $k_\\text{p}$ and $k_\\text{i}$ more formally in W9. For now we just pick different values to see how the dynamics are impacted." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8NK8O6XT7B_a", + "metadata": {}, + "outputs": [], + "source": [ + "kp = 1\n", + "ki = 1\n", + "\n", + "# Create tf from numerator/denominator coefficients\n", + "C = ct.tf([kp, ki], [1, 0], name='C')\n", + "print(C)\n", + "\n", + "# Alternative method: define \"s\" and use algebra\n", + "s = ct.tf('s')\n", + "C = ct.tf(kp + ki/s, name='C')\n", + "print(C)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "074427a3", + "metadata": {}, + "outputs": [], + "source": [ + "# Loop transfer function\n", + "L = P * C\n", + "cplt = ct.bode_plot([P, C, L], label=['P', 'C', 'L'])\n", + "cplt.set_plot_title(\"PI controller for servomechanism\")" + ] + }, + { + "cell_type": "markdown", + "id": "Bg5ga11VuRtI", + "metadata": { + "id": "Bg5ga11VuRtI" + }, + "source": [ + "Note that L = P * C corresponds to addition in both the magnitude and the phase." + ] + }, + { + "cell_type": "markdown", + "id": "UmYmSzx2rTfg", + "metadata": { + "id": "UmYmSzx2rTfg" + }, + "source": [ + "### Nyquist analysis\n", + "\n", + "To check stability (and eventually robustness), we use the Nyquist criterion." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "Qmp59pmS9GLj", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=[7, 4])\n", + "ax1 = plt.subplot(2, 2, 1)\n", + "ax2 = plt.subplot(2, 2, 3)\n", + "ct.bode_plot(L, ax=[ax1, ax2])\n", + "\n", + "# Tidy up the figure a bit\n", + "fig.align_labels()\n", + "ax1.set_title(\"Bode plot for L\")\n", + "\n", + "ax2 = plt.subplot(1, 2, 2)\n", + "ct.nyquist_plot(L, ax=ax2, title=\"\")\n", + "plt.title(\"Nyquist plot for L\")\n", + "\n", + "plt.suptitle(\"Loop analysis for (unstable) servomechanism\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "s4dDf4PrZqU3", + "metadata": { + "id": "s4dDf4PrZqU3" + }, + "source": [ + "We see from this plot that the loop transfer function encircles the -1 point => closed loop system should be unstable. We can check this by making use of additional features of Nyquist analysis." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "K7ifUBL0Z3xN", + "metadata": {}, + "outputs": [], + "source": [ + "# Get the Nyquist *response*, so that we can get back encirclements\n", + "nyqresp = ct.nyquist_response(L)\n", + "print(\"N = encirclements: \", nyqresp.count)\n", + "print(\"P = RHP poles of L: \", np.sum(np.real(L.poles()) > 0))\n", + "print(\"Z = N + P = RHP zeros of 1 + L:\", np.sum(np.real((1 + L).zeros()) > 0))\n", + "print(\"Zeros of (1 + L) = \", (1 + L).zeros())\n", + "print(\"\")\n", + "\n", + "T = ct.feedback(L)\n", + "ct.step_response(T).plot(\n", + " title=\"Step response for (unstable) servomechanism\",\n", + " time_label=\"Time [ms]\");" + ] + }, + { + "cell_type": "markdown", + "id": "p3JxLilMxdOE", + "metadata": { + "id": "p3JxLilMxdOE" + }, + "source": [ + "### Poles on the $j\\omega$ axis\n", + "\n", + "Note that we have a pole at 0 (due to the integrator in the controller). How is this handled?\n", + "\n", + "A: use a small loop to the right around poles on the $j\\omega$ axis => not inside the contour.\n", + "\n", + "To see this, we use the `nyquist_response` function, which returns the contour used to compute the Nyquist curve. If we zoom in on the contour near the origin, we see how the outer edge of the Nyquist curve is computed." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "R5IBk3Ai9Slk", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(figsize=[7, 5.8])\n", + "\n", + "# Plot the D contour\n", + "ax1 = plt.subplot(2, 2, 1)\n", + "plt.plot(np.real(nyqresp.contour), np.imag(nyqresp.contour))\n", + "plt.axis([-1e-4, 4e-4, 0, 4e-4])\n", + "plt.xlabel('Real axis')\n", + "plt.ylabel('Imaginary axis')\n", + "plt.title(\"Zoom on D-contour\")\n", + "\n", + "# Clean up the display of the units\n", + "from matplotlib import ticker\n", + "ax1.xaxis.set_major_formatter(ticker.StrMethodFormatter(\"{x:.0e}\"))\n", + "ax1.yaxis.set_major_formatter(ticker.StrMethodFormatter(\"{x:.0e}\"))\n", + "\n", + "ax2 = plt.subplot(2, 2, 2)\n", + "ct.nyquist_plot(L, ax=ax2)\n", + "plt.title(\"Nyquist curve\")\n", + "\n", + "plt.suptitle(\"Nyquist contour for pole at the origin\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "h20JRZ_r4fGy", + "metadata": { + "id": "h20JRZ_r4fGy" + }, + "source": [ + "### Second iteration feedback control design\n", + "\n", + "We now redesign the control system to give something that is stable. We can do this by moving the zero for the controller to a lower frequency, so that the phase lag from the integrator does not overlap with the phase lag from the system dynamics." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "YsM8SnXz_Kaj", + "metadata": {}, + "outputs": [], + "source": [ + "# Change the frequency response to avoid crossing over -180 with large gain\n", + "Cnew = ct.tf(kp + (ki/200)/s, name='C_new')\n", + "Lnew = ct.tf(P * Cnew, name='L_new')\n", + "\n", + "plt.figure(figsize=[7, 4])\n", + "ax1 = plt.subplot(2, 2, 1)\n", + "ax2 = plt.subplot(2, 2, 3)\n", + "ct.bode_plot([Lnew, L], ax=[ax1, ax2], label=['L_new', 'L_old'])\n", + "\n", + "# Clean up the figure a bit\n", + "ax1.loglog([1e-3, 1e1], [1, 1], 'k', linewidth=0.5)\n", + "ax1.set_title(\"Bode plot for L_new, L_old\", size='medium')\n", + "\n", + "ax3=plt.subplot(1, 2, 2)\n", + "ct.nyquist_plot(Lnew, max_curve_magnitude=5, ax=ax3)\n", + "ax3.set_title(\"Nyquist plot for Lnew\", size='medium')\n", + "\n", + "plt.suptitle(\"Loop analysis for (stable) servomechanism\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "kFjeGXzDvucx", + "metadata": { + "id": "kFjeGXzDvucx" + }, + "source": [ + "We see now that we have no encirclements, and so the system should be stable.\n", + "\n", + "Note however that the Nyquist curve is close to the -1 point => not *that* stable." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "GGfJwG716jU2", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the transfer function from r to y\n", + "Tnew = ct.feedback(Lnew)\n", + "cplt = ct.step_response(Tnew).plot(time_label=\"Time [ms]\")\n", + "cplt.set_plot_title(\"Step response for (stable) spring-mass system\")" + ] + }, + { + "cell_type": "markdown", + "id": "b5114fa7-6924-47d7-8dd2-f12060152edd", + "metadata": {}, + "source": [ + "### Third iteration feedback control design (via loop shaping)\n", + "\n", + "To get a better design, we use a PID controller to shape the frequency response so that we get high gain at low frequency and low phase at crossover." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e6da93a4-5202-45d7-9e5a-697848f4ba71", + "metadata": {}, + "outputs": [], + "source": [ + "# Design parameters\n", + "Td = 1 # Set to gain crossover frequency\n", + "Ti = Td * 10 # Set to low frequency region\n", + "kp = 500 # Tune to get desired bandwith\n", + "\n", + "# Updated gains\n", + "kp = 150\n", + "Ti = Td * 5; kp = 150\n", + "\n", + "# Compute controller parmeters\n", + "ki = kp/Ti\n", + "kd = kp * Td\n", + "\n", + "# Controller transfer function\n", + "ctrl_shape = kp + ki / s + kd * s\n", + "\n", + "# Frequency response (open loop) - use this to help tune your design\n", + "ltf_shape = ct.tf(P_tf * ctrl_shape, name='L_shape')\n", + "\n", + "cplt = ct.frequency_response([P, ctrl_shape]).plot(label=['P', 'C_shape'])\n", + "cplt = ct.frequency_response(ltf_shape).plot(margins=True)\n", + "\n", + "cplt.set_plot_title(\"Loop shaping design for servomechanism controller\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d731f372-4992-464c-9ca5-49cc1d554799", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the transfer function from r to y\n", + "T_shape = ct.feedback(ltf_shape)\n", + "cplt = ct.step_response(T_shape).plot(\n", + " time_label=\"Time [ms]\",\n", + " title = \"Step response for servomechanism with PID controller\")" + ] + }, + { + "cell_type": "markdown", + "id": "JL99vo4trep5", + "metadata": { + "id": "JL99vo4trep5" + }, + "source": [ + "### Closed loop frequency response\n", + "\n", + "We can also look at the closed loop frequency response to understand how different inputs affect different outputs. The `gangof4` function computes the standard transfer functions:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ceqcg3oM619g", + "metadata": {}, + "outputs": [], + "source": [ + "cplt = ct.gangof4(P_tf, ctrl_shape)" + ] + }, + { + "cell_type": "markdown", + "id": "gel18-iqwYYs", + "metadata": { + "id": "gel18-iqwYYs" + }, + "source": [ + "### Stability margins\n", + "\n", + "Another standard set of analysis tools is to identify the gain, phase, and stability margins for the system:\n", + "\n", + "* **Gain margin:** the maximimum amount of additional gain that we can put into the loop and still maintain stability.\n", + "* **Phase margin:** the maximum amount of additional phase (lag) that we can put into the loop and still maintain stability.\n", + "* **Stability margin:** the maximum amount of combined gain and phase at the critical frequency that can be put into the loop and still maintain stability.\n", + "\n", + "The first two of the items can be computed either by looking at the frequency response or by using the `margin` command.\n", + "\n", + "The stabilty margin is the minimum distance between -1 and $L(jw)$, which is just the minimum value of $|1 - L(j\\omega)|$.\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "m-8ItbHwxLrv", + "metadata": {}, + "outputs": [], + "source": [ + "plt.figure(figsize=[7, 4])\n", + "\n", + "# Gain and phase margin on Bode plot\n", + "ax1 = plt.subplot(2, 2, 1)\n", + "plt.title(\"Bode plot for Lnew, with margins\")\n", + "ax2 = plt.subplot(2, 2, 3)\n", + "ct.bode_plot(Lnew, ax=[ax1, ax2], margins=True)\n", + "\n", + "# Compute gain and phase margin\n", + "gm, pm, wpc, wgc = ct.margin(Lnew)\n", + "print(f\"Gm = {gm:2.2g} (at {wpc:.2g} rad/ms)\")\n", + "print(f\"Pm = {pm:3.2g} deg (at {wgc:.2g} rad/ms)\")\n", + "\n", + "# Compute the stability margin\n", + "resp = ct.frequency_response(1 + Lnew)\n", + "sm = np.min(resp.magnitude)\n", + "wsm = resp.omega[np.argmin(resp.magnitude)]\n", + "print(f\"Sm = {sm:2.2g} (at {wsm:.2g} rad/ms)\")\n", + "\n", + "# Plot the Nyquist curve\n", + "ax3 = plt.subplot(1, 2, 2)\n", + "ct.nyquist_plot(Lnew, ax=ax3)\n", + "plt.title(\"Nyquist plot for Lnew [zoomed]\")\n", + "plt.axis([-2, 3, -2.6, 2.6])\n", + "\n", + "#\n", + "# Annotate it to see the margins\n", + "#\n", + "\n", + "# Gain margin (special case here, since infinite)\n", + "Lgm = 0\n", + "plt.plot([-1, Lgm], [0, 0], 'k-', linewidth=0.5)\n", + "plt.text(-0.9, 0.1, \"1/gm\")\n", + "\n", + "# Phase margin\n", + "theta = np.linspace(0, 2 * math.pi)\n", + "plt.plot(np.cos(theta), np.sin(theta), 'k--', linewidth=0.5)\n", + "plt.text(-1.3, -0.8, \"pm\")\n", + "\n", + "# Stability margin\n", + "Lsm = Lnew(wsm * 1j)\n", + "plt.plot([-1, Lsm.real], [0, Lsm.imag], 'k-', linewidth=0.5)\n", + "plt.text(-0.4, -0.5, \"sm\")\n", + "\n", + "plt.suptitle(\"\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "WsOzQST9rFC-", + "metadata": { + "id": "WsOzQST9rFC-" + }, + "source": [ + "## Unstable system: inverted pendulum\n", + "\n", + "When we have a system that is open loop unstable, the Nyquist curve will need to have encirclements to be stable. In this case, the interpretation of the various characteristics can be more complicated.\n", + "\n", + "To explore this, we consider a simple model for an inverted pendulum, which has (normalized) dynamics:\n", + "\n", + "$$\n", + "\\dot x = \\begin{bmatrix} 0 & 1 & \\\\ -1 & 0.1 \\end{bmatrix} x + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} u, \\qquad\n", + "y = \\begin{bmatrix} 1 & 0 \\end{bmatrix} x\n", + "$$\n", + "\n", + "Transfer function for the system can be shown to be\n", + "\n", + "$$\n", + "P(s) = \\frac{1}{s^2 + 0.1 s - 1}.\n", + "$$\n", + "\n", + "This system is unstable, with poles $\\sim\\pm 1$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ZbPzrlPIrHnp", + "metadata": {}, + "outputs": [], + "source": [ + "P = ct.tf([1], [1, 0.1, -1])\n", + "P.poles()" + ] + }, + { + "cell_type": "markdown", + "id": "W-sBWxKi6SPx", + "metadata": { + "id": "W-sBWxKi6SPx" + }, + "source": [ + "### PD controller\n", + "\n", + "We construct a proportional-derivative (PD) controller for the system,\n", + "\n", + "$$\n", + "u = k_\\text{p} e + k_\\text{d} \\dot{e}\n", + "$$\n", + "\n", + "which is roughly the equivalent of using state feedback (since the system states are $\\theta$ and $\\dot\\theta$)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "hjQS_dED7yJE", + "metadata": {}, + "outputs": [], + "source": [ + "# Transfer function for a PD controller\n", + "kp = 10\n", + "kd = 2\n", + "C = ct.tf([kd, kp], [1])\n", + "\n", + "# Loop transfer function\n", + "L = P * C\n", + "L.name = 'L'\n", + "print(L)\n", + "print(\"Zeros: \", L.zeros())\n", + "print(\"Poles: \", L.poles())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "YI_KJo0E9pFd", + "metadata": {}, + "outputs": [], + "source": [ + "# Bode and Nyquist plots\n", + "plt.figure(figsize=[7, 4])\n", + "ax1 = plt.subplot(2, 2, 1)\n", + "plt.title(\"Bode plot for L\", size='medium')\n", + "ax2 = plt.subplot(2, 2, 3)\n", + "ct.bode_plot(L, ax=[ax1, ax2])\n", + "\n", + "ax3 = plt.subplot(1, 2, 2)\n", + "ct.nyquist_plot(L, ax=ax3)\n", + "plt.title(\"Nyquist plot for L\", size='medium')\n", + "\n", + "plt.suptitle(\"Loop analysis for inverted pendulum\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8dH03kv9-Da8", + "metadata": {}, + "outputs": [], + "source": [ + "# Check the Nyquist criterion\n", + "nyqresp = ct.nyquist_response(L)\n", + "print(\"N = encirclements: \", nyqresp.count)\n", + "print(\"P = RHP poles of L: \", np.sum(np.real(L.poles()) > 0))\n", + "print(\"Z = N + P = RHP zeros of 1 + L:\", np.sum(np.real((1 + L).zeros()) >= 0))\n", + "print(\"Poles of L = \", L.poles())\n", + "print(\"Zeros of 1 + L = \", (1 + L).zeros())\n", + "print(\"\")\n", + "\n", + "T = ct.feedback(L)\n", + "ct.initial_response(T, X0=[0.1, 0]).plot();" + ] + }, + { + "cell_type": "markdown", + "id": "7bb03f68-0c99-40e9-86cd-a9f2816b4096", + "metadata": {}, + "source": [ + "Note that we get a warning when we set the initial condition. This is because `T` is a transfer function and so it doesn't have a unique state space realization. If the initial state is zero this doesn't matter, but if the initial state is nonzero then the assignment of states is not well defined." + ] + }, + { + "cell_type": "markdown", + "id": "VXlYhs8X7DuN", + "metadata": { + "id": "VXlYhs8X7DuN" + }, + "source": [ + "### Gang of 4\n", + "\n", + "Another useful thing to look at is the transfer functions from noise and disturbances to the system outputs and inputs:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "oTmOun41_opt", + "metadata": {}, + "outputs": [], + "source": [ + "ct.gangof4(P, C);" + ] + }, + { + "cell_type": "markdown", + "id": "U41ve1zh7XPh", + "metadata": { + "id": "U41ve1zh7XPh" + }, + "source": [ + "We see that the response from the input $r$ (or equivalently noise $n$) to the process input is very large for large frequencies. This means that we are amplifying high frequency noise (and comes from the fact that we used derivative feedback)." + ] + }, + { + "cell_type": "markdown", + "id": "YROqmZTd8WYs", + "metadata": { + "id": "YROqmZTd8WYs" + }, + "source": [ + "### High frequency rolloff\n", + "\n", + "We can attempt to resolve this by \"rolling off\" the derivative action at high frequencies:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "vhKi_L-F_6Ws", + "metadata": {}, + "outputs": [], + "source": [ + "Cnew = (kp + kd * s) / (s/20 + 1)**2\n", + "Cnew.name = 'Cnew'\n", + "print(Cnew)\n", + "\n", + "Lnew = P * Cnew\n", + "Lnew.name = 'Lnew'\n", + "\n", + "plt.figure(figsize=[7, 4])\n", + "ax1 = plt.subplot(2, 2, 1)\n", + "ax2 = plt.subplot(2, 2, 3)\n", + "ct.bode_plot([Lnew, L], ax=[ax1, ax2])\n", + "ax1.loglog([1e-1, 1e2], [1, 1], 'k', linewidth=0.5)\n", + "ax1.set_title(\"Bode plot for L, Lnew\", size='medium')\n", + "\n", + "ax3 = plt.subplot(1, 2, 2)\n", + "ct.nyquist_plot(Lnew, ax=ax3)\n", + "ax3.set_title(\"Nyquist plot for Lnew\", size='medium')\n", + "\n", + "plt.suptitle(\"Stability analysis for inverted pendulum\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "WgrAE9XE7_nJ", + "metadata": { + "id": "WgrAE9XE7_nJ" + }, + "source": [ + "While not (yet) a very high performing controller, this change does get rid of the issues with the high frequency noise:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "FknwW6GkBLLU", + "metadata": {}, + "outputs": [], + "source": [ + "# Check the gang of 4\n", + "ct.gangof4(P, Cnew);" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "wJHJLjXwCNz-", + "metadata": {}, + "outputs": [], + "source": [ + "# See what the step response looks like\n", + "Tnew = ct.feedback(Lnew)\n", + "ct.step_response(Tnew, 10).plot()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "WUhz529a-w3q", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L8a_maglev-limits.ipynb b/examples/cds110-L8a_maglev-limits.ipynb new file mode 100644 index 000000000..5a7473ade --- /dev/null +++ b/examples/cds110-L8a_maglev-limits.ipynb @@ -0,0 +1,278 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "gToHma1nvZxz", + "metadata": { + "id": "gToHma1nvZxz" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 8a

\n", + "

Fundamental Limits for Control of a Magnetic Levitation System

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1MuDZfw72UkI4_Ji_AsEDTPi7IaSURsYP)\n", + "\n", + "This notebook contains the code used to create the magnetic levitation example in Lecture 8-1 of CDS 110, Winter 2024." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "dc288b3e-60cc-4a75-8af5-81f9d1eede41", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "from math import pi\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs" + ] + }, + { + "cell_type": "markdown", + "id": "RFi9litmZKT2", + "metadata": { + "id": "RFi9litmZKT2" + }, + "source": [ + "The magnetic leviation system consists of a metal ball, an electromagnet, and an IR sensor:\n", + "\n", + "
\"maglev-diagram\"
\n", + "\n", + "It is governed by following equation:\n", + "\n", + "$$ \\ddot{z} = g - \\frac{k_mk_A^2}{m}\\frac{u^2}{z^2} - \\frac{c}{m}\\dot{z},$$\n", + "\n", + "where $z$ is the vertical height of the ball and $u$ is the input current applied to the electromagnet. The output is given by $v_{ir}$, which is the voltage measured at the IR sensor:\n", + "\n", + "$$v_{ir} = k_T z + v_0 $$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "80da9750-1a34-4a54-ab3a-ff37ea7be0f6", + "metadata": {}, + "outputs": [], + "source": [ + "# System dynamics\n", + "maglev_params = {\n", + " 'kT': 613.65, # gain between position and voltage\n", + " 'v0': -16.18,\t # voltage offset at zero position\n", + " 'm': 0.2,\t # mass of ball, kg\n", + " 'g': 9.81, # gravitational constant\n", + " 'kA': 1,\t # electromagnet conductance\n", + " 'c': 1 # damping (added to improve visualization)\n", + "}\n", + "# gain on magnetic attractive force\n", + "maglev_params['km'] = 3.13e-3 * (maglev_params['m']/2) / maglev_params['kA']**2\n", + "\n", + "def maglev_update(t, x, u, params):\n", + " m, g, kA, km, c = map(params.get, ['m', 'g', 'kA', 'km', 'c'])\n", + " return np.array([\n", + " x[1],\n", + " g - km/m * (kA * u[0])**2 / x[0]**2 - c * x[1]\n", + " ])\n", + "\n", + "def maglev_output(t, x, u, params):\n", + " kT, v0 = map(params.get, ['kT', 'v0'])\n", + " return np.array([kT * x[0] + v0])\n", + "\n", + "maglev = ct.nlsys(\n", + " maglev_update, maglev_output, params=maglev_params, name='maglev',\n", + " inputs='Vu', outputs='Vy', states=['pos', 'vel']\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b5c56e04-03b7-4c18-be3c-3f4308aedb98", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the equilibrium point that holds the ball at the origin\n", + "xeq, ueq = ct.find_eqpt(maglev, [0.02, 0], 0.2, y0=0)\n", + "print(f\"{xeq=}, {ueq=}\", end='\\n----\\n')\n", + "\n", + "# Compute the linearization at that point\n", + "magP = ct.linearize(maglev, xeq, ueq, name='sys')\n", + "print(magP, end='\\n----\\n')\n", + "\n", + "print(\"Poles:\", magP.poles())\n", + "print(\"Zeros:\", magP.zeros())" + ] + }, + { + "cell_type": "markdown", + "id": "22a2766f-217a-4213-ba19-c11485cc42cc", + "metadata": {}, + "source": [ + "The controller for this system is implemented via an electrical circuit consisting of resistors and capacitors. We don't show the circuit here, but just write down the model for the transfer function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b4741e88-bedd-4ef0-b8b9-9deb5fa93d5d", + "metadata": {}, + "outputs": [], + "source": [ + "# Controller (analog circuit)\n", + "k1 = 0.5\t\t\t\t# gain set by gain pot\n", + "R1 = 22000\t\t\t\t# Internal resistor\n", + "R2 = 22000\t\t\t\t# Resistor plug-in\n", + "R = 2000; C = 1e-6\t\t# RC plug-in\n", + "\n", + "# Controller based on analog circuit\n", + "magC1 = -ct.tf([(R1 + R) * C, 1], [R * C, 1]) * k1 * R2/R1\n", + "magL1 = magP * magC1" + ] + }, + { + "cell_type": "markdown", + "id": "641c0df2-90f6-4573-af7f-41a305337e77", + "metadata": {}, + "source": [ + "We can now use a Nyquist plot to see if the controller is stabilizing:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "378b14b8-f8e4-4ed6-b09d-cdf577ea47d1", + "metadata": {}, + "outputs": [], + "source": [ + "# Nyquist plot\n", + "cplt = ct.nyquist_plot([magP, magL1], label=[\"sys\", \"sys * ctrl\"])" + ] + }, + { + "cell_type": "markdown", + "id": "HKGSdW5f91mZ", + "metadata": { + "id": "HKGSdW5f91mZ" + }, + "source": [ + "We see that the controller causes the system to have clockwise net encircelement of the origin. Since the open loop system has one unstable pole, this gives $Z = N + P = 0$ and so the closed loop system is stable." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7850f14d-79ab-4250-a0c7-8ddc10ebb977", + "metadata": {}, + "outputs": [], + "source": [ + "# Bode plots\n", + "magC1.name = \"ctrl\"\n", + "cplt = ct.bode_plot(\n", + " [magP, magC1, magL1], np.logspace(0, 4), initial_phase=0,\n", + " label=['P', 'C', 'L'])\n", + "cplt.axes[0, 0].set_ylim(0.06, 1.5e1)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d83c5d5c-238a-45a1-9a81-a3779e7f7bc3", + "metadata": {}, + "outputs": [], + "source": [ + "# Sensitivity function for closed loop system/.\n", + "magS1 = ct.feedback(1, magL1, name=\"S1\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3bdcb116-02fd-46d9-ab4d-5b25511d0b21", + "metadata": {}, + "outputs": [], + "source": [ + "# Step response\n", + "magT1 = ct.feedback(magL1, name=\"T1\")\n", + "ct.step_response(magT1).plot(title=\"Step response for closed loop system\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e2ddb53c-023b-466b-ac15-221c22befd6d", + "metadata": {}, + "outputs": [], + "source": [ + "# Try to improve performance by increasing DC gain\n", + "# System with gain increased\n", + "magC2 = magC1*5 \t\t\t # increased gain\n", + "magL2 = magP * magC2 \t\t\t # loop transfer function\n", + "magS2 = ct.feedback(1, magP * magC2, name=\"S2\") \t# sensitivity function\n", + "magT2 = ct.feedback(magP * magC2, 1, name=\"T2\") \t# closed loop response\n", + "\n", + "# System with gain increased even more\n", + "magC3 = magC1*20\t\t\t # increased gain\n", + "magL3 = magP*magC3\t\t\t # loop transfer function\n", + "magS3 = ct.feedback(1, magP * magC3, name=\"S3\")\t # sensitivity function\n", + "magT3 = ct.feedback(magP * magC3, 1, name=\"T3\")\t # closed loop response\n", + "\n", + "# Plot step responses for different systems\n", + "colors = ['b', 'g', '#FF7F50']\n", + "for sys in [magT1, magT2, magT3]:\n", + " ct.step_response(sys).plot(color=colors.pop())\n", + "\n", + "# Bode plot for sensitivity function\n", + "plt.figure()\n", + "cplt = ct.bode_plot([magS1, magS2, magS3], plot_phase=False)\n", + "\n", + "# Add magnitude of 1\n", + "xdata = cplt.lines[0][0][0].get_xdata()\n", + "ydata = np.ones_like(xdata)\n", + "plt.plot(xdata, ydata, color='k', linestyle='--');" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4df561a2-16aa-41b0-9971-f8c151467730", + "metadata": {}, + "outputs": [], + "source": [ + "# Bode integral calculation\n", + "omega = np.linspace(0, 1e6, 100000)\n", + "for name, sys in zip(['C1', 'C2', 'C3'], [magS1, magS2, magS3]):\n", + " freqresp = ct.frequency_response(sys, omega)\n", + " bodeint = np.trapz(np.log(freqresp.magnitude), omega)\n", + " print(\"Bode integral for\", name, \"=\", bodeint)\n", + "\n", + "print(\"pi * sum[ Re(pk) ]\", pi * np.sum(magP.poles()[magP.poles().real > 0]))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "M2EvTYHq8yRb", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L8b_pvtol-complete-limits.ipynb b/examples/cds110-L8b_pvtol-complete-limits.ipynb new file mode 100644 index 000000000..0b482c865 --- /dev/null +++ b/examples/cds110-L8b_pvtol-complete-limits.ipynb @@ -0,0 +1,1032 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "659a189e-33c9-426f-b318-7cb2f433ae4a", + "metadata": { + "id": "659a189e-33c9-426f-b318-7cb2f433ae4a" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 8b

\n", + "

Full Controller Stack for a Planar Vertical Take-Off and Landing (PVTOL) System

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1XulsQqbthMkr3g58OTctIYKYpqirOgns)\n", + "\n", + "The purpose of this lecture is to introduce tools that can be used for frequency domain modeling and analysis of linear systems." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1be7545a", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "from math import sin, cos, pi\n", + "from scipy.optimize import NonlinearConstraint\n", + "import time\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs\n", + "\n", + "# Use control parameters for plotting\n", + "plt.rcParams.update(ct.rcParams)" + ] + }, + { + "cell_type": "markdown", + "id": "c5a1858a", + "metadata": { + "id": "c5a1858a" + }, + "source": [ + "## System definition\n", + "\n", + "Consider the PVTOL system `pvtol_noisy`, defined in `pvtol.py`:\n", + "\n", + "$$\n", + " \\begin{aligned}\n", + " m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x + D_x, \\\\\n", + " m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - c \\dot y - m g + D_y, \\\\\n", + " J \\ddot \\theta &= r F_1,\n", + " \\end{aligned} \\qquad\n", + " \\vec Y =\n", + " \\begin{bmatrix} x \\\\ y \\\\ \\theta \\end{bmatrix} +\n", + " \\begin{bmatrix} N_x \\\\ N_y \\\\ N_z \\end{bmatrix}.\n", + "$$\n", + "\n", + "Assume that the input disturbances are modeled by independent, first\n", + "order Markov (Ornstein-Uhlenbeck) processes with\n", + "$Q_D = \\text{diag}(0.01, 0.01)$ and $\\omega_0 = 1$ and that the noise\n", + "is modeled as white noise with covariance matrix\n", + "\n", + "$$\n", + " Q_N = \\begin{bmatrix}\n", + " 2 \\times 10^{-4} & 0 & 1 \\times 10^{-5} \\\\\n", + " 0 & 2 \\times 10^{-4} & 1 \\times 10^{-5} \\\\\n", + " 1 \\times 10^{-5} & 1 \\times 10^{-5} & 1 \\times 10^{-4}\n", + " \\end{bmatrix}.\n", + "$$\n", + "\n", + "We will design a controller consisting of a trajectory generation module, a\n", + "gain-scheduled, trajectory tracking module, and a state estimation\n", + "module the moves the system from the origin to the equilibrum point\n", + "point $x_\\text{f}$, $y_\\text{f}$ = 10, 0 while satisfying the\n", + "constraint $0.5 \\sin(\\pi x / 10) - 0.1 \\leq y \\leq 1$." + ] + }, + { + "cell_type": "markdown", + "id": "D1aFeNuglL4a", + "metadata": { + "id": "D1aFeNuglL4a" + }, + "source": [ + "We start by creating the PVTOL system without noise or disturbances." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c32ec3f8", + "metadata": {}, + "outputs": [], + "source": [ + "# STANDARD PVTOL DYNAMICS\n", + "def _pvtol_update(t, x, u, params):\n", + "\n", + " # Get the parameter values\n", + " m = params.get('m', 4.) # mass of aircraft\n", + " J = params.get('J', 0.0475) # inertia around pitch axis\n", + " r = params.get('r', 0.25) # distance to center of force\n", + " g = params.get('g', 9.8) # gravitational constant\n", + " c = params.get('c', 0.05) # damping factor (estimated)\n", + "\n", + " # Get the inputs and states\n", + " x, y, theta, xdot, ydot, thetadot = x\n", + " F1, F2 = u\n", + "\n", + " # Constrain the inputs\n", + " F2 = np.clip(F2, 0, 1.5 * m * g)\n", + " F1 = np.clip(F1, -0.1 * F2, 0.1 * F2)\n", + "\n", + " # Dynamics\n", + " xddot = (F1 * cos(theta) - F2 * sin(theta) - c * xdot) / m\n", + " yddot = (F1 * sin(theta) + F2 * cos(theta) - m * g - c * ydot) / m\n", + " thddot = (r * F1) / J\n", + "\n", + " return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])\n", + "\n", + "# Define pvtol output function to only be x, y, and theta\n", + "def _pvtol_output(t, x, u, params):\n", + " return x[0:3]\n", + "\n", + "# Create nonlinear input-output system of nominal pvtol system\n", + "pvtol_nominal = ct.nlsys(\n", + " _pvtol_update, _pvtol_output, name=\"pvtol_nominal\",\n", + " states = [f'x{i}' for i in range(6)],\n", + " inputs = ['F1', 'F2'],\n", + " outputs = [f'x{i}' for i in range(3)]\n", + ")\n", + "\n", + "print(pvtol_nominal)" + ] + }, + { + "cell_type": "markdown", + "id": "TTMQAAhFldW7", + "metadata": { + "id": "TTMQAAhFldW7" + }, + "source": [ + "Next, we create a PVTOL system with noise and disturbances. This system will use the nominal PVTOL system and add disturbances as inputs to the state dynamics and noise to the system output." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "tqSvuzvOkps1", + "metadata": {}, + "outputs": [], + "source": [ + "# Add wind and noise to system dynamics\n", + "def _noisy_update(t, x, u, params):\n", + " # Get the inputs\n", + " F1, F2, Dx, Dy = u[:4]\n", + " if u.shape[0] > 4:\n", + " Nx, Ny, Nth = u[4:]\n", + " else:\n", + " Nx, Ny, Nth = 0, 0, 0\n", + "\n", + " # Get the system response from the original dynamics\n", + " xdot, ydot, thetadot, xddot, yddot, thddot = \\\n", + " _pvtol_update(t, x, [F1, F2], params)\n", + "\n", + " # Get the parameter values we need\n", + " m = params.get('m', 4.) # mass of aircraft\n", + " J = params.get('J', 0.0475) # inertia around pitch axis\n", + "\n", + " # Now add the disturbances\n", + " xddot += Dx / m\n", + " yddot += Dy / m\n", + "\n", + " return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])\n", + "\n", + "# Define pvtol_noisy output function to only be x, y, and theta\n", + "def _noisy_output(t, x, u, params):\n", + " F1, F2, Dx, Dy, Nx, Ny, Nth = u\n", + " return x[0:3] + np.array([Nx, Ny, Nth])\n", + "\n", + "# CREATE NONLINEAR INPUT-OUTPUT SYSTEM\n", + "pvtol_noisy = ct.nlsys(\n", + " _noisy_update, _noisy_output, name=\"pvtol_noisy\",\n", + " states = [f'x{i}' for i in range(6)],\n", + " inputs = ['F1', 'F2'] + ['Dx', 'Dy'] + ['Nx', 'Ny', 'Nth'],\n", + " outputs = ['x', 'y', 'theta'],\n", + " params = {\n", + " 'm': 4., # mass of aircraft\n", + " 'J': 0.0475, # inertia around pitch axis\n", + " 'r': 0.25, # distance to center of force\n", + " 'g': 9.8, # gravitational constant\n", + " 'c': 0.05, # damping factor (estimated)\n", + " }\n", + ")\n", + "\n", + "print(pvtol_noisy)" + ] + }, + { + "cell_type": "markdown", + "id": "057cba8f-79bd-4a45-a184-2424c569785d", + "metadata": { + "id": "057cba8f-79bd-4a45-a184-2424c569785d" + }, + "source": [ + "Note that the outputs of `pvtol_noisy` are not the full set of states, but rather the states we can measure: $x$, $y$, and $\\theta$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7ce469b3-faa0-4bac-b9d4-02e4dae7a2da", + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function tlot the trajectory in xy coordinates\n", + "def plot_results(t, x, u, fig=None):\n", + " # Set the size of the figure\n", + " if fig is None:\n", + " fig = plt.figure(figsize=(10, 6))\n", + "\n", + " # Top plot: xy trajectory\n", + " plt.subplot(2, 1, 1)\n", + " plt.plot(x[0], x[1])\n", + " plt.xlabel('x [m]')\n", + " plt.ylabel('y [m]')\n", + " plt.axis('equal')\n", + "\n", + " # Time traces of the state and input\n", + " plt.subplot(2, 4, 5)\n", + " plt.plot(t, x[1])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('y [m]')\n", + "\n", + " plt.subplot(2, 4, 6)\n", + " plt.plot(t, x[2])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('theta [rad]')\n", + "\n", + " plt.subplot(2, 4, 7)\n", + " plt.plot(t, u[0])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('$F_1$ [N]')\n", + "\n", + " plt.subplot(2, 4, 8)\n", + " plt.plot(t, u[1])\n", + " plt.xlabel('Time t [sec]')\n", + " plt.ylabel('$F_2$ [N]')\n", + " plt.tight_layout()\n", + "\n", + " return fig\n" + ] + }, + { + "cell_type": "markdown", + "id": "081764e0", + "metadata": { + "id": "081764e0" + }, + "source": [ + "## Estimator\n", + "\n", + "We start by designing an optimal estimator for the system. We choose the noise intensities\n", + "based on knowledge of the modeling errors, disturbances, and sensor characteristics:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "778fb908", + "metadata": {}, + "outputs": [], + "source": [ + "# Disturbance and noise intensities\n", + "Qv = np.diag([1e-2, 1e-2])\n", + "Qw = np.array([[2e-4, 0, 1e-5], [0, 2e-4, 1e-5], [1e-5, 1e-5, 1e-4]])\n", + "Qwinv = np.linalg.inv(Qw)\n", + "\n", + "# Initial state covariance\n", + "P0 = np.eye(pvtol_noisy.nstates)" + ] + }, + { + "cell_type": "markdown", + "id": "1Q55PHN1omJs", + "metadata": { + "id": "1Q55PHN1omJs" + }, + "source": [ + "We will use a linear quadratic estimator (Kalman filter) to design an optimal estimator for the system. Recall that the `ct.lqe` function takes in a linear system as input, so we first linear our `pvtol_noisy` system around its equilibrium point." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "WADb1-VcuR5t", + "metadata": {}, + "outputs": [], + "source": [ + "# Find the equilibrium point corresponding to the origin\n", + "xe, ue = ct.find_eqpt(\n", + " sys = pvtol_noisy,\n", + " x0 = np.zeros(pvtol_noisy.nstates),\n", + " u0 = np.zeros(pvtol_noisy.ninputs),\n", + " y0 = [0, 0, 0],\n", + " iu=range(2, pvtol_noisy.ninputs),\n", + " iy=[0, 1]\n", + ")\n", + "print(f\"{xe=}\")\n", + "print(f\"{ue=}\")\n", + "\n", + "# Linearize system for Kalman filter\n", + "pvtol_noisy_lin = pvtol_noisy.linearize(xe, ue)\n", + "\n", + "# Extract the linearization for use in LQR design\n", + "A, B, C = pvtol_noisy_lin.A, pvtol_noisy_lin.B, pvtol_noisy_lin.C" + ] + }, + { + "cell_type": "markdown", + "id": "6E9s147Cpppr", + "metadata": { + "id": "6E9s147Cpppr" + }, + "source": [ + "We want to define an estimator that takes in the measured states $x$, $y$, and $\\theta$, as well as applied inputs $F_1$ and $F_2$. As the estimator doesn't have any measurement of the noise/disturbances applied to the system, we will design our controller with only these inputs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "nvZHm0Ooqkj_", + "metadata": {}, + "outputs": [], + "source": [ + "# use ct.lqe to create an L matrix, using only measured inputs F1 and F2\n", + "L, Pf, _ = ct.lqe(A, B[:,:2], C, Qv, Qw)" + ] + }, + { + "cell_type": "markdown", + "id": "KXVetnCUrHvs", + "metadata": { + "id": "KXVetnCUrHvs" + }, + "source": [ + "We now create our estimator." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "M77vo5PgrIEv", + "metadata": {}, + "outputs": [], + "source": [ + "# Create standard (optimal) estimator update function\n", + "def estimator_update(t, xhat, u, params):\n", + "\n", + " # Extract the inputs to the estimator\n", + " y = u[0:3] # just grab the first three outputs\n", + " u_cmd = u[3:5] # get the inputs that were applied as well\n", + "\n", + " # Update the state estimate using PVTOL (non-noisy) dynamics\n", + " return _pvtol_update(t, xhat, u_cmd, params) - L @ (C @ xhat - y)\n", + "\n", + "# Create estimator\n", + "estimator = ct.nlsys(\n", + " estimator_update, None,\n", + " name = 'Estimator',\n", + " states=pvtol_noisy.nstates,\n", + " inputs= pvtol_noisy.output_labels \\\n", + " + pvtol_noisy.input_labels[0:2],\n", + " outputs=[f'xh{i}' for i in range(pvtol_noisy.nstates)],\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1JOPx1TXrnr-", + "metadata": {}, + "outputs": [], + "source": [ + "print(estimator)" + ] + }, + { + "cell_type": "markdown", + "id": "46d8463d", + "metadata": { + "id": "46d8463d" + }, + "source": [ + "## Gain scheduled controller\n", + "\n", + "We next design our (gain scheduled) controller for the system. Here, as in the case of the estimator, we will create the controller using the nominal PVTOL system, so that the applied inputs to the system are only $F_1$ and $F_2$. If we were to make a controller using the noisy PVTOL system, then the inputs applied via control action would include noise and disturbances, which is incorrect." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2e5fbef3", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the weights for the LQR problem\n", + "Qx = np.diag([100, 10, (180/np.pi) / 5, 0, 0, 0])\n", + "# Qx = np.diag([10, 100, (180/np.pi) / 5, 0, 0, 0]) # Try this out to see what changes\n", + "Qu = np.diag([10, 1])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e5cc3cc0", + "metadata": {}, + "outputs": [], + "source": [ + "# Construct the array of gains and the gain scheduled controller\n", + "import itertools\n", + "import math\n", + "\n", + "# Set up points around which to linearize (control-0.9.3: must be 2D or greater)\n", + "angles = np.linspace(-math.pi/3, math.pi/3, 10)\n", + "speeds = np.linspace(-10, 10, 3)\n", + "points = list(itertools.product(angles, speeds))\n", + "\n", + "# Compute the gains at each design point of angles and speeds\n", + "gains = []\n", + "\n", + "# Iterate through points\n", + "for point in points:\n", + "\n", + " # Compute the state that we want to linearize about\n", + " xgs = xe.copy()\n", + " xgs[2], xgs[4] = point[0], point[1]\n", + "\n", + " # Linearize the system and compute the LQR gains\n", + " linsys = pvtol_noisy.linearize(xgs, ue)\n", + " A = linsys.A\n", + " B = linsys.B[:,:2]\n", + " K, X, E = ct.lqr(A, B, Qx, Qu)\n", + " gains.append(K)\n", + "\n", + "# Construct the controller\n", + "gs_ctrl, gs_clsys = ct.create_statefbk_iosystem(\n", + " sys = pvtol_nominal,\n", + " gain = (gains, points),\n", + " gainsched_indices=['xh2', 'xh4'],\n", + " estimator=estimator\n", + ")\n", + "\n", + "print(gs_ctrl)" + ] + }, + { + "cell_type": "markdown", + "id": "ecd28a73", + "metadata": { + "id": "ecd28a73" + }, + "source": [ + "## Trajectory generation\n", + "\n", + "Finally, we need to design the trajectory that we want to follow. We consider a situation with state constraints that represent the specific experimental conditions for this system (at Caltech):\n", + "* `ceiling`: The system has limited vertical travel, so we constrain the vertical position to lie between $-0.5$ and $2$ meters.\n", + "* `nicolas`: When testing, we placed a person in between the initial and final position, and we need to avoid hitting him as we move from start to finish.\n", + "\n", + "The code below defines the initial conditions, final conditions, and constraints." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5eb12bfa", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the initial and final conditions\n", + "x_delta = np.array([10, 0, 0, 0, 0, 0])\n", + "x0, u0 = ct.find_eqpt(\n", + " sys = pvtol_nominal,\n", + " x0 = np.zeros(6),\n", + " u0 = np.zeros(2),\n", + " y0 = np.zeros(3),\n", + " iy=[0, 1]\n", + ")\n", + "xf, uf = ct.find_eqpt(\n", + " sys = pvtol_nominal,\n", + " x0 = x0 + x_delta,\n", + " u0 = u0,\n", + " y0 = (x0 + x_delta)[:3],\n", + " iy=[0, 1]\n", + ")\n", + "\n", + "# Define the time horizon for the manuever\n", + "Tf = 5\n", + "timepts = np.linspace(0, Tf, 100, endpoint=False)\n", + "\n", + "# Create a constraint corresponding to the obstacle\n", + "ceiling = (NonlinearConstraint, lambda x, u: x[1], [-0.5], [2])\n", + "nicolas = (NonlinearConstraint,\n", + " lambda x, u: x[1] - (0.5 * sin(pi * x[0] / 10) - 0.1), [0], [1])\n", + "\n", + "# # Reset the nonlinear constraint to give some extra room\n", + "# nicolas = (NonlinearConstraint,\n", + "# lambda x, u: x[1] - (0.8 * sin(pi * x[0] / 10) - 0.1), [0], [1])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "610aa247", + "metadata": {}, + "outputs": [], + "source": [ + "# Re-define the time horizon for the manuever\n", + "Tf = 5\n", + "timepts = np.linspace(0, Tf, 20, endpoint=False)\n", + "\n", + "# We provide a tent shape as an intial guess\n", + "xm = (x0 + xf) / 2 + np.array([0, 0.5, 0, 0, 0, 0])\n", + "tm = int(len(timepts)/2)\n", + "# straight line from start to midpoint to end with nominal input\n", + "tent = (\n", + " np.hstack([\n", + " np.array([x0 + (xm - x0) * t/(Tf/2) for t in timepts[0:tm]]).transpose(),\n", + " np.array([xm + (xf - xm) * t/(Tf/2) for t in timepts[0:tm]]).transpose()\n", + " ]),\n", + " u0\n", + ")\n", + "\n", + "# terminal constraint\n", + "term_constraints = opt.state_range_constraint(pvtol_nominal, xf, xf)\n", + "\n", + "# trajectory cost\n", + "traj_cost = opt.quadratic_cost(pvtol_nominal, None, Qu, x0=xf, u0=uf)\n", + "\n", + "# find optimal trajectory\n", + "start_time = time.process_time()\n", + "traj = opt.solve_ocp(\n", + " sys = pvtol_nominal,\n", + " timepts = timepts,\n", + " initial_guess=tent,\n", + " X0=x0,\n", + " cost = traj_cost,\n", + " trajectory_constraints=[ceiling, nicolas],\n", + " terminal_constraints=term_constraints,\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Create the desired trajectory\n", + "xd, ud = traj.states, traj.inputs" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e59ddc29", + "metadata": {}, + "outputs": [], + "source": [ + "# Extend the trajectory to hold the final position for Tf seconds\n", + "holdpts = np.arange(Tf, Tf + Tf, timepts[1]-timepts[0])\n", + "xd = np.hstack([xd, np.outer(xf, np.ones_like(holdpts))])\n", + "ud = np.hstack([ud, np.outer(uf, np.ones_like(holdpts))])\n", + "timepts = np.hstack([timepts, holdpts])\n", + "\n", + "# Plot the desired trajectory\n", + "plot_results(timepts, xd, ud)\n", + "plt.suptitle('Desired Trajectory')\n", + "\n", + "# Add the constraints to the plot\n", + "plt.subplot(2, 1, 1)\n", + "\n", + "plt.plot([0, 10], [2, 2], 'r--')\n", + "plt.text(5, 1.8, 'Ceiling', ha='center')\n", + "\n", + "x_nic = np.linspace(0, 10, 50)\n", + "y_nic = 0.5 * np.sin(pi * x_nic / 10) - 0.1\n", + "plt.plot(x_nic, y_nic, 'r--')\n", + "plt.text(5, 0, 'Nicolas Petit', ha='center')\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "affe55fa", + "metadata": { + "id": "affe55fa" + }, + "source": [ + "## Final Control System Implementation\n", + "\n", + "We now put together the final control system and simulate it. If you have named your inputs and outputs to each of the subsystems properly, the code below should connect everything up correctly. If you get errors about inputs or outputs that are not connected to anything, check the names of your inputs and outputs in the various\n", + "systems above and make sure everything lines up as it should." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "50dff557", + "metadata": {}, + "outputs": [], + "source": [ + "# Create the interconnected system\n", + "clsys = ct.interconnect(\n", + " [pvtol_noisy, gs_ctrl, estimator],\n", + " inputs=gs_clsys.input_labels[:8] + pvtol_noisy.input_labels[2:],\n", + " outputs=pvtol_noisy.output_labels + pvtol_noisy.input_labels[:2]\n", + ")\n", + "print(clsys)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0f24e6f5", + "metadata": {}, + "outputs": [], + "source": [ + "# Generate disturbance and noise vectors\n", + "V = ct.white_noise(timepts, Qv)\n", + "W = ct.white_noise(timepts, Qw)\n", + "for i in range(V.shape[0]):\n", + " plt.subplot(2, 3, i+1)\n", + " plt.plot(timepts, V[i])\n", + " plt.ylabel(f'V[{i}]')\n", + "\n", + "for i in range(W.shape[0]):\n", + " plt.subplot(2, 3, i+4)\n", + " plt.plot(timepts, W[i])\n", + " plt.ylabel(f'W[{i}]')\n", + " plt.xlabel('Time $t$ [s]')\n", + "\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f63091cf", + "metadata": {}, + "outputs": [], + "source": [ + "# Simulate the open loop system and plot the results (+ state trajectory)\n", + "resp = ct.input_output_response(\n", + " sys = clsys,\n", + " T = timepts,\n", + " U = [xd, ud, V, W],\n", + " X0 = np.zeros(12))\n", + "\n", + "plot_results(resp.time, resp.outputs[0:3], resp.outputs[3:5])\n", + "\n", + "# Add the constraints to the plot\n", + "plt.subplot(2, 1, 1)\n", + "plt.plot([0, 10], [1, 1], 'r--')\n", + "x_nic = np.linspace(0, 10, 50)\n", + "y_nic = 0.5 * np.sin(pi * x_nic / 10) - 0.1\n", + "plt.plot(x_nic, y_nic, 'r--')\n", + "plt.text(5, 0, 'Nicolas Petit', ha='center')\n", + "plt.suptitle(\"Measured Trajectory\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "89221230", + "metadata": { + "id": "89221230" + }, + "source": [ + "We see that with the addition of disturbances and noise, we sometimes violate the constraint 'nicolas' (if your plot doesn't show an intersection with the bottom dashed curve, try regenerating the noise and running the simulation again). This can be fixed by establishing a more conservative constraint (see commented out constraint in code block above)." + ] + }, + { + "cell_type": "markdown", + "id": "3f2e9776-0ba9-4295-9473-a17cb4854836", + "metadata": { + "id": "3f2e9776-0ba9-4295-9473-a17cb4854836" + }, + "source": [ + "## Small signal analysis\n", + "\n", + "We next look at the properties of the system using the small signal (linearized) dynamics. This analysis is useful to check the robustness and performance of the controller around trajectories and equilibrium points.\n", + "\n", + "We will carry out the analysis around the initial condition." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "JgZyPyMkcoOl", + "metadata": {}, + "outputs": [], + "source": [ + "## Small signal analysis\n", + "X0 = np.hstack([x0, x0]) # system state, estim state\n", + "U0 = np.hstack([x0, u0, np.zeros(5)]) # xd, ud, dist, noise\n", + "G = clsys.linearize(X0, U0)\n", + "print(clsys)\n", + "\n", + "# Get input/output dictionaries: inp['sig'] = index for 'sig'\n", + "inp = clsys.input_index\n", + "out = clsys.output_index\n", + "\n", + "fig, axs = plt.subplots(2, 3, figsize=[9, 6])\n", + "omega = np.logspace(-2, 2)\n", + "\n", + "# Complementary sensitivity\n", + "G_x_xd = ct.tf(G[out['x'], inp['xd[0]']])\n", + "G_y_yd = ct.tf(G[out['y'], inp['xd[1]']])\n", + "ct.bode_plot(\n", + " [G_x_xd, G_y_yd], omega,\n", + " plot_phase=False, ax=np.array([[axs[0, 0]]]))\n", + "axs[0, 0].legend(['F T_x', 'F T_y'])\n", + "axs[0, 0].loglog([omega[0], omega[-1]], [1, 1], 'k', linewidth=0.5)\n", + "axs[0, 0].set_title(\"From xd, yd\", fontsize=9)\n", + "axs[0, 0].set_ylabel(\"To x, y\")\n", + "axs[0, 0].set_xlabel(\"\")\n", + "\n", + "# Load (or input) sensitivity\n", + "G_x_dx = ct.tf(G[out['x'], inp['Dx']])\n", + "G_y_dy = ct.tf(G[out['y'], inp['Dy']])\n", + "ct.bode_plot(\n", + " [G_x_dx, G_y_dy], omega,\n", + " plot_phase=False, ax=np.array([[axs[0, 1]]]))\n", + "axs[0, 1].legend(['PS_x', 'PS_y'])\n", + "axs[0, 1].loglog([omega[0], omega[-1]], [1, 1], 'k', linewidth=0.5)\n", + "axs[0, 1].set_title(\"From Dx, Dy\", fontsize=9)\n", + "axs[0, 1].set_xlabel(\"\")\n", + "axs[0, 1].set_ylabel(\"\")\n", + "\n", + "# Sensitivity\n", + "G_x_Nx = ct.tf(G[out['x'], inp['Nx']])\n", + "G_y_Ny = ct.tf(G[out['y'], inp['Ny']])\n", + "ct.bode_plot(\n", + " [G_x_Nx, G_y_Ny], omega,\n", + " plot_phase=False, ax=np.array([[axs[0, 2]]]))\n", + "axs[0, 2].legend(['S_x', 'S_y'])\n", + "axs[0, 2].set_title(\"From Nx, Ny\", fontsize=9)\n", + "axs[0, 2].loglog([omega[0], omega[-1]], [1, 1], 'k', linewidth=0.5)\n", + "axs[0, 2].set_xlabel(\"\")\n", + "axs[0, 2].set_ylabel(\"\")\n", + "\n", + "# Noise (or output) sensitivity\n", + "G_F1_xd = ct.tf(G[out['F1'], inp['xd[0]']])\n", + "G_F2_yd = ct.tf(G[out['F2'], inp['xd[1]']])\n", + "ct.bode_plot(\n", + " [G_F1_xd, G_F2_yd], omega,\n", + " plot_phase=False, ax=np.array([[axs[1, 0]]]))\n", + "axs[1, 0].legend(['FCS_x', 'FCS_y'])\n", + "axs[1, 0].loglog([omega[0], omega[-1]], [1, 1], 'k', linewidth=0.5)\n", + "axs[1, 0].set_ylabel(\"To F1, F2\")\n", + "\n", + "G_F1_dx = ct.tf(G[out['F1'], inp['Dx']])\n", + "G_F2_dy = ct.tf(G[out['F2'], inp['Dy']])\n", + "ct.bode_plot(\n", + " [G_F1_dx, G_F2_dy], omega,\n", + " plot_phase=False, ax=np.array([[axs[1, 1]]]))\n", + "axs[1, 1].legend(['~T_x', '~T_y'])\n", + "axs[1, 1].loglog([omega[0], omega[-1]], [1, 1], 'k', linewidth=0.5)\n", + "axs[1, 1].set_ylabel(\"\")\n", + "\n", + "# Sensitivity\n", + "G_F1_Nx = ct.tf(G[out['F1'], inp['Nx']])\n", + "G_F1_Ny = ct.tf(G[out['F1'], inp['Ny']])\n", + "ct.bode_plot(\n", + " [G_F1_Nx, G_F1_Ny], omega,\n", + " plot_phase=False, ax=np.array([[axs[1, 2]]]))\n", + "axs[1, 2].legend(['C S_x', 'C S_y'])\n", + "axs[1, 2].loglog([omega[0], omega[-1]], [1, 1], 'k', linewidth=0.5)\n", + "axs[1, 2].set_ylabel(\"\")\n", + "\n", + "plt.suptitle(\"Gang of Six for PVTOL\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "xfi1mXJTe3Gm", + "metadata": {}, + "outputs": [], + "source": [ + "# Solve for the loop transfer function horizontal direction\n", + "# S = 1 / (1 + L) => S + SL = 1 => L = (1 - S)/S\n", + "Lx = (1 - G_x_Nx) / G_x_Nx; Lx.name = 'Lx'\n", + "Ly = (1 - G_y_Ny) / G_y_Ny; Ly.name = 'Ly'\n", + "\n", + "# Create Nyquist plot\n", + "ct.nyquist_plot([Lx, Ly], max_curve_magnitude=5, max_curve_offset=0.2);" + ] + }, + { + "cell_type": "markdown", + "id": "L7L6UZTn_Qtn", + "metadata": { + "id": "L7L6UZTn_Qtn" + }, + "source": [ + "### Gain Margins of $L_x$, $L_y$\n", + "\n", + "We can zoom in on the plot to see the gain, phase, and stability margins:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3FX7YXrR2cuQ", + "metadata": {}, + "outputs": [], + "source": [ + "cplt = ct.nyquist_plot([Lx, Ly])\n", + "lower_upper_bound = 1.1\n", + "cplt.axes[0, 0].set_xlim([-lower_upper_bound, lower_upper_bound])\n", + "cplt.axes[0, 0].set_ylim([-lower_upper_bound, lower_upper_bound])\n", + "cplt.axes[0, 0].set_aspect('equal')\n", + "\n", + "# Gain margin for Lx\n", + "neg1overgm_x = -0.67 # vary this manually to find intersection with curve\n", + "color = cplt.lines[0][0].get_color()\n", + "plt.plot(neg1overgm_x, 0, color=color, marker='o', fillstyle='none')\n", + "gm_x = -1/neg1overgm_x\n", + "\n", + "# Gain margin for Ly\n", + "neg1overgm_y = -0.32 # vary this manually to find intersection with curve\n", + "color = cplt.lines[1][0].get_color()\n", + "plt.plot(neg1overgm_y, 0, color=color, marker='o', fillstyle='none')\n", + "gm_y = -1/neg1overgm_y\n", + "\n", + "print('Margins obtained visually:')\n", + "print('Gain margin of Lx: '+str(gm_x))\n", + "print('Gain margin of Ly: '+str(gm_y))\n", + "print('\\n')\n", + "\n", + "# get gain margin computationally\n", + "gm_xc, pm_xc, wpc_xc, wgc_xc = ct.margin(Lx)\n", + "gm_yc, pm_yc, wpc_yc, wgc_yc = ct.margin(Ly)\n", + "\n", + "print('Margins obtained computationally:')\n", + "print('Gain margin of Lx: '+str(gm_xc))\n", + "print('Gain margin of Ly: '+str(gm_yc))\n", + "\n", + "print('\\n')" + ] + }, + { + "cell_type": "markdown", + "id": "VnrVNvhz_Zi2", + "metadata": { + "id": "VnrVNvhz_Zi2" + }, + "source": [ + "### Phase Margins of $L_x$, $L_y$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "zKb_o9ZN_ffF", + "metadata": {}, + "outputs": [], + "source": [ + "# add customizations to Nyquist plot\n", + "cplt = ct.nyquist_plot(\n", + " [Lx, Ly], max_curve_magnitude=5, max_curve_offset=0.2,\n", + " unit_circle=True)\n", + "lower_upper_bound = 2\n", + "cplt.axes[0, 0].set_xlim([-lower_upper_bound, lower_upper_bound])\n", + "cplt.axes[0, 0].set_ylim([-lower_upper_bound, lower_upper_bound])\n", + "cplt.axes[0, 0].set_aspect('equal')\n", + "\n", + "# Phase margin of Lx:\n", + "th_pm_x = 0.14*np.pi\n", + "th_plt_x = np.pi + th_pm_x\n", + "color = cplt.lines[0][0].get_color()\n", + "plt.plot(np.cos(th_plt_x), np.sin(th_plt_x), color=color, marker='o')\n", + "\n", + "# Phase margin of Ly\n", + "th_pm_y = 0.19*np.pi\n", + "th_plt_y = np.pi + th_pm_y\n", + "color = cplt.lines[1][0].get_color()\n", + "plt.plot(np.cos(th_plt_y), np.sin(th_plt_y), color=color, marker='o')\n", + "\n", + "print('Margins obtained visually:')\n", + "print('Phase margin: '+str(float(th_pm_x)))\n", + "print('Phase margin: '+str(float(th_pm_y)))\n", + "print('\\n')\n", + "\n", + "# get margin computationally\n", + "gm_xc, pm_xc, wpc_xc, wgc_xc = ct.margin(Lx)\n", + "gm_yc, pm_yc, wpc_yc, wgc_yc = ct.margin(Ly)\n", + "\n", + "print('Margins obtained computationally:')\n", + "print('Phase margin of Lx: '+str(np.deg2rad(pm_xc)))\n", + "print('Phase margin of Ly: '+str(np.deg2rad(pm_yc)))\n", + "\n", + "print('\\n')" + ] + }, + { + "cell_type": "markdown", + "id": "dF0BIq5BDXII", + "metadata": { + "id": "dF0BIq5BDXII" + }, + "source": [ + "### Stability Margins of $L_x$, $L_y$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "XQPB_h6Y1cAW", + "metadata": {}, + "outputs": [], + "source": [ + "# add customizations to Nyquist plot\n", + "cplt = ct.nyquist_plot([Lx, Ly], max_curve_magnitude=5, max_curve_offset=0.2)\n", + "lower_upper_bound = 2\n", + "cplt.axes[0, 0].set_xlim([-lower_upper_bound, lower_upper_bound])\n", + "cplt.axes[0, 0].set_ylim([-lower_upper_bound, lower_upper_bound])\n", + "cplt.axes[0, 0].set_aspect('equal')\n", + "\n", + "# Stability margin:\n", + "sm_x = 0.3 # vary this manually to find min which intersects\n", + "color = cplt.lines[0][0].get_color()\n", + "sm_circle = plt.Circle((-1, 0), sm_x, color=color, fill=False, ls=':')\n", + "cplt.axes[0, 0].add_patch(sm_circle)\n", + "\n", + "sm_y = 0.5 # vary this manually to find min which intersects\n", + "color = cplt.lines[1][0].get_color()\n", + "sm_circle = plt.Circle((-1, 0), sm_y, color=color, fill=False, ls=':')\n", + "cplt.axes[0, 0].add_patch(sm_circle)\n", + "\n", + "print('Margins obtained visually:')\n", + "print('* Stability margin of Lx: '+str(sm_x))\n", + "print('* Stability margin of Ly: '+str(sm_y))\n", + "\n", + "# Compute the stability margin computationally\n", + "print('') # blank line\n", + "print('Margins obtained computationally:')\n", + "resp = ct.frequency_response(1 + Lx)\n", + "sm = np.min(resp.magnitude)\n", + "wsm = resp.omega[np.argmin(resp.magnitude)]\n", + "\n", + "print(f\"* Stability margin of Lx = {sm:2.2g} (at {wsm:.2g} rad/s)\")\n", + "resp = ct.frequency_response(1 + Ly)\n", + "sm = np.min(resp.magnitude)\n", + "wsm = resp.omega[np.argmin(resp.magnitude)]\n", + "print(f\"* Stability margin of Ly = {sm:2.2g} (at {wsm:.2g} rad/s)\")\n", + "print('')" + ] + }, + { + "cell_type": "markdown", + "id": "boAjWk56GXYZ", + "metadata": { + "id": "boAjWk56GXYZ" + }, + "source": [ + "We see that the frequencies at which the stability margins are found corresponds to the peak of the magnitude of the sensitivity functions for $L_x$ and $L_y$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "JkbMn8pif7Ub", + "metadata": {}, + "outputs": [], + "source": [ + "# Confirm stability using Nyquist criterion\n", + "nyqresp_x = ct.nyquist_response(Lx)\n", + "nyqresp_y = ct.nyquist_response(Ly)\n", + "\n", + "print(\"Nx =\", nyqresp_x.count, \"; Px =\", np.sum(np.real(Lx.poles()) > 0))\n", + "print(\"Ny =\", nyqresp_y.count, \"; Py =\", np.sum(np.real(Ly.poles()) > 0))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4d038db9-f671-4f0f-82db-51096e8272b7", + "metadata": {}, + "outputs": [], + "source": [ + "# Take a look at the locations of the poles\n", + "np.real(Ly.poles())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9dd57510-4b03-4c0a-90ae-35011f90c41b", + "metadata": {}, + "outputs": [], + "source": [ + "# See what happened in the contour\n", + "plt.plot(np.real(nyqresp_y.contour), np.imag(nyqresp_y.contour))\n", + "plt.axis([-1e-4, 4e-4, 0, 4e-4])\n", + "plt.title(\"Zoom on D-contour\");" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e7b9a2f9-f40f-4090-ae69-6bf53fea54a9", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds110-L9_servomech-pid.ipynb b/examples/cds110-L9_servomech-pid.ipynb new file mode 100644 index 000000000..3c8f5df5a --- /dev/null +++ b/examples/cds110-L9_servomech-pid.ipynb @@ -0,0 +1,635 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "id": "FAZsjB3IN9JN" + }, + "source": [ + "
\n", + "

CDS 110, Lecture 9

\n", + "

PID Control of a Servomechanism

\n", + "

Richard M. Murray, Winter 2024

\n", + "
\n", + "\n", + "[Open in Google Colab](https://colab.research.google.com/drive/1BP0DFHh94tSxAyQetvOEbBEHKrSoVGQW)\n", + "\n", + "In this lecture we will use a variety of methods to design proportional (P), proportional-integral (PI), and proportional-integral-derivative (PID) controllers for a cart pendulum system." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "from math import pi\n", + "try:\n", + " import control as ct\n", + " print(\"python-control\", ct.__version__)\n", + "except ImportError:\n", + " !pip install control\n", + " import control as ct" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "T0rjwp1mONm1" + }, + "source": [ + "## System dynamics\n", + "\n", + "Consider a simple mechanism consisting of a spring loaded arm that is driven by a motor, as shown below:\n", + "\n", + "
\"servomech-diagram\"
\n", + "\n", + "The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached.\n", + "\n", + "The equations of motion for the system are given by\n", + "\n", + "$$\n", + "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n", + "$$\n", + "\n", + "which can be written in state space form as\n", + "\n", + "$$\n", + "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\theta \\end{bmatrix} =\n", + " \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n", + " + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n", + "$$\n", + "\n", + "The system parameters are given by\n", + "\n", + "$$\n", + "k = 1,\\quad J = 100,\\quad b = 10,\n", + "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01.\n", + "$$\n", + "\n", + "and we assume that time is measured in milliseconds (ms) and distance in centimeters (cm). (The constants here are made up and don't necessarily reflect a real disk drive, though the units and time constants are motivated by computer disk drives.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Parameter values\n", + "servomech_params = {\n", + " 'J': 100, # Moment of inertia of the motor\n", + " 'b': 10, # Angular damping of the arm\n", + " 'k': 1, # Spring constant\n", + " 'r': 1, # Location of spring contact on arm\n", + " 'l': 2, # Distance to the read head\n", + " 'eps': 0.01, # Magnitude of velocity-dependent perturbation\n", + "}\n", + "\n", + "# State derivative\n", + "def servomech_update(t, x, u, params):\n", + " # Extract the configuration and velocity variables from the state vector\n", + " theta = x[0] # Angular position of the disk drive arm\n", + " thetadot = x[1] # Angular velocity of the disk drive arm\n", + " tau = u[0] # Torque applied at the base of the arm\n", + "\n", + " # Get the parameter values\n", + " J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])\n", + "\n", + " # Compute the angular acceleration\n", + " dthetadot = 1/J * (\n", + " -b * thetadot - k * r * np.sin(theta) + tau)\n", + "\n", + " # Return the state update law\n", + " return np.array([thetadot, dthetadot])\n", + "\n", + "# System output (full state)\n", + "def servomech_output(t, x, u, params):\n", + " l = params['l']\n", + " return l * x[0]\n", + "\n", + "# System dynamics\n", + "servomech = ct.nlsys(\n", + " servomech_update, servomech_output, name='servomech',\n", + " params=servomech_params,\n", + " states=['theta_', 'thdot_'],\n", + " outputs=['y'], inputs=['tau'])" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "n4bQu0e2_aBT" + }, + "source": [ + "In addition to the system dynamics, we assume there are actuator dynamics that limit the performance of the system. We take these as first order dynamics with saturation:\n", + "\n", + "$$\n", + "\\tau = \\text{sat} \\left(\\frac{\\alpha}{s + \\alpha} u\\right)\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "actuator_params = {\n", + " 'umax': 5, # Saturation limits\n", + " 'alpha': 10, # Actuator time constant\n", + "}\n", + "\n", + "def actuator_update(t, x, u, params):\n", + " # Get parameter values\n", + " alpha = params['alpha']\n", + " umax = params['umax']\n", + "\n", + " # Clip the input\n", + " u_clip = np.clip(u, -umax, umax)\n", + "\n", + " # Actuator dynamics\n", + " return -alpha * x + alpha * u_clip\n", + "\n", + "actuator = ct.nlsys(\n", + " actuator_update, None, params=actuator_params,\n", + " inputs='u', outputs='tau', states=1, name='actuator')\n", + "\n", + "system = ct.series(actuator, servomech)\n", + "system.name = 'system' # missing feature\n", + "print(system)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "8HYyndF_saE0" + }, + "source": [ + "### Linearization\n", + "\n", + "To study the open loop dynamics of the system, we compute the linearization of the dynamics about the equilibrium point corresponding to $\\theta_\\text{e} = 15^\\circ$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Convert the equilibrium angle to radians\n", + "theta_e = (15 / 180) * np.pi\n", + "\n", + "# Compute the input required to hold this position\n", + "u_e = servomech.params['k'] * servomech.params['r'] * np.sin(theta_e)\n", + "print(\"Equilibrium torque = %g\" % u_e)\n", + "\n", + "# Linearize the system dynamics about the equilibrium point\n", + "P = ct.tf(\n", + " system.linearize([0, theta_e, 0], u_e, copy_names=True)[0, 0])\n", + "P.name = 'P' # bug\n", + "print(P, end=\"\\n\\n\")\n", + "\n", + "ct.bode_plot(P)" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "J1dwXObJSKp-" + }, + "source": [ + "## Ziegler-Nichols tuning\n", + "\n", + "Ziegler-Nichols tuning provides a method for choosing the gains of a PID controller that give reasonable closed loop response. More information can be found in [Feedback Systems](https://fbswiki.org/wiki/index.php/Feedback_Systems:_An_Introduction_for_Scientists_and_Engineers) (FBS2e), Section 11.3.\n", + "\n", + "We show here the figures and tables that we will use (from FBS2e):\n", + "\n", + "
\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "To use the Ziegler-Nichols turning rules, we plot the step response, compute the parameters (shown in the figure), and then apply the formulas in the table:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the step response\n", + "resp = ct.step_response(P)\n", + "resp.plot()\n", + "\n", + "# Find the point of the steepest slope\n", + "slope = np.diff(resp.outputs) / np.diff(resp.time)\n", + "mxi = np.argmax(slope)\n", + "mx_time = resp.time[mxi]\n", + "mx_out= resp.outputs[mxi]\n", + "plt.plot(resp.time[mxi], resp.outputs[mxi], 'ro')\n", + "\n", + "# Draw a line going through the point of max slope\n", + "mx_slope = slope[mxi]\n", + "timepts = np.linspace(0, mx_time*2)\n", + "plt.plot(timepts, mx_out + mx_slope * (timepts - mx_time), 'r-')\n", + "\n", + "# Solve for the Ziegler-Nichols parameters\n", + "a = -(mx_out - mx_slope * mx_time) # Find the value of the line at t = 0\n", + "tau = a / mx_slope # Solve a + mx_slope * tau = 0\n", + "print(f\"{a=}, {tau=}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can then construct a controller using the parameters:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "s = ct.tf('s')\n", + "\n", + "# Proportional controller\n", + "kp = 1/a\n", + "ctrl_zn_P = kp\n", + "\n", + "# PI controller\n", + "kp = 0.9/a\n", + "Ti = tau/0.3; ki = kp/Ti\n", + "ctrl_zn_PI = kp + ki / s\n", + "\n", + "# PID controller\n", + "kp = 1.2/a\n", + "Ti = tau/0.5; ki = kp/Ti\n", + "Td = 0.5 * tau; kd = kp * Td\n", + "ctrl_zn_PID = kp + ki / s + kd * s\n", + "\n", + "print(ctrl_zn_PID)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the closed loop systems and plots the step and\n", + "# frequency responses.\n", + "\n", + "clsys_zn_P = ct.feedback(P * ctrl_zn_P)\n", + "clsys_zn_P.name = 'P'\n", + "\n", + "clsys_zn_PI = ct.feedback(P * ctrl_zn_PI)\n", + "clsys_zn_PI.name = 'PI'\n", + "\n", + "clsys_zn_PID = ct.feedback(P * ctrl_zn_PID)\n", + "clsys_zn_PID.name = 'PID'\n", + "\n", + "# Plot the step responses\n", + "resp.sysname = 'open_loop'\n", + "resp.plot(color='k')\n", + "\n", + "stepresp_zn_P = ct.step_response(clsys_zn_P)\n", + "stepresp_zn_P.plot(color='b')\n", + "\n", + "stepresp_zn_PI = ct.step_response(clsys_zn_PI)\n", + "stepresp_zn_PI.plot(color='r')\n", + "\n", + "stepresp_zn_PID = ct.step_response(clsys_zn_PID)\n", + "stepresp_zn_PID.plot(color='g')\n", + "plt.legend()\n", + "\n", + "plt.figure()\n", + "ct.bode_plot([clsys_zn_P, clsys_zn_PI, clsys_zn_PID]);" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "6iZwB2WEeg8S" + }, + "source": [ + "## Loop shaping\n", + "\n", + "A better design can be obtained by looking at the loop transfer function and adjusting the controller parameters to give a loop shape that will give closed loop properties. We show the steps for such a design here:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Design parameters\n", + "Td = 1 # Set to gain crossover frequency\n", + "Ti = Td * 10 # Set to low frequency region\n", + "kp = 500 # Tune to get desired bandwith\n", + "\n", + "# Updated gains\n", + "kp = 150\n", + "Ti = Td * 5; kp = 150\n", + "\n", + "# Compute controller parmeters\n", + "ki = kp/Ti\n", + "kd = kp * Td\n", + "\n", + "# Controller transfer function\n", + "ctrl_shape = kp + ki / s + kd * s\n", + "ctrl_shape.name = 'C'\n", + "\n", + "# Frequency response (open loop) - use this to help tune your design\n", + "ltf_shape = P * ctrl_shape\n", + "ltf_shape.name = 'L'\n", + "\n", + "ct.frequency_response([P, ctrl_shape]).plot()\n", + "ct.frequency_response(ltf_shape).plot(margins=True);" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the closed loop systemsand plot the step response\n", + "# and Nyquist plot (to make sure margins look OK)\n", + "\n", + "# Create the closed loop systems\n", + "clsys_shape = ct.feedback(P * ctrl_shape)\n", + "clsys_shape.name = 'loopshape'\n", + "\n", + "# Step response\n", + "plt.subplot(2, 1, 1)\n", + "stepresp_shape = ct.step_response(clsys_shape)\n", + "stepresp_shape.plot(color='b')\n", + "plt.plot([0, stepresp_shape.time[-1]], [1, 1], 'k--')\n", + "\n", + "# Compare to the ZN controller\n", + "ax = plt.subplot(2, 1, 2)\n", + "ct.step_response(clsys_shape, stepresp_zn_PID.time).plot(\n", + " color='b', ax=np.array([[ax]]))\n", + "stepresp_zn_PID.plot(color='g', ax=np.array([[ax]]))\n", + "ax.plot([0, stepresp_shape.time[-1]], [1, 1], 'k--')\n", + "\n", + "# Nyquist plot\n", + "plt.figure()\n", + "ct.nyquist([ltf_shape])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We see that the loop shaping controller has better step response (faster rise and settling time, less overshoot)." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "GyXQXykafzWs" + }, + "source": [ + "### Gang of Four\n", + "\n", + "When designing a controller, it is important to look at all of the input/output responses, not just the response from reference to output (which is what the step response above focuses on). \n", + "\n", + "In the frequency domain, the Gang of 4 plots provide useful information on all (important) input/output pairs:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ct.gangof4(P, ctrl_shape)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "These all look pretty resonable, except that the transfer function from the reference $r$ to the system input $u$ is getting large at high frequency. This occurs because we did not filter the derivative on the PID controller, so high frequency components of the reference signal (or the measurement noise!) get amplified. We will fix this in the more advanced controller below." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "uFO3wiWXhBAK" + }, + "source": [ + "## Anti-windup + derivative filtering\n", + "\n", + "In addition to the amplification of high frequency signals due to the derivative term, another practical consideration in the use of PID controllers is integrator windup. Integrator windup occurs when there are limits on the control inputs so that the error signal may not descrease quickly. This causes the integral term in the PID controller to see an error for a long period of time, and the resulting integration of the error must be offset by making the error have opposite sign for some period of time. This is often undesireable.\n", + "\n", + "To see how to address both amplification of noise due to the derivative term and integrator windup effects in the presence of input constraints, we now implement PID controller with anti-windup and derivative filtering, as shown in the following figure (see also Figure 11.11 in [FBS2e](https://fbswiki.org/wiki/index.php/Feedback_Systems:_An_Introduction_for_Scientists_and_Engineers)):\n", + "\n", + "
\n", + "\n", + "
\n", + "\n", + "### Low pass filter\n", + "\n", + "The low pass filtered derivative has transfer function\n", + "\n", + "$$\n", + "G(s) = \\frac{a\\, s}{s + a}.\n", + "$$\n", + "\n", + "This can be implemented using the differential equation\n", + "\n", + "$$\n", + "\\dot \\xi = -a \\xi + a y, \\qquad\n", + "\\eta = -a \\xi + a y.\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "ctrl_params = {'kaw': 5 * ki, 'a': 10/Td}\n", + "\n", + "def ctrl_update(t, x, u, params):\n", + " # Get the parameter values\n", + " kaw = params['kaw']\n", + " a = params['a']\n", + " umax_ctrl = params.get('umax_ctrl', actuator.params['umax'])\n", + "\n", + " # Extract the signals into more familiar variable names\n", + " r, y = u[0], u[1]\n", + " z = x[0] # integral error\n", + " xi = x[1] # filtered derivative\n", + "\n", + " # Compute the controller components\n", + " u_prop = kp * (r - y)\n", + " u_int = z\n", + " ydt_f = -a * xi + a * (-y)\n", + " u_der = kd * ydt_f\n", + "\n", + " # Compute the commanded and saturated outputs\n", + " u_cmd = u_prop + u_int + u_der\n", + " u_sat = np.clip(u_cmd, -umax_ctrl, umax_ctrl)\n", + "\n", + " dz = ki * (r - y) + kaw * (u_sat - u_cmd)\n", + " dxi = -a * xi + a * (-y)\n", + " return np.array([dz, dxi])\n", + "\n", + "def ctrl_output(t, x, u, params):\n", + " # Get the parameter values\n", + " kaw = params['kaw']\n", + " a = params['a']\n", + " umax_ctrl = params.get('umax_ctrl', params['umax'])\n", + "\n", + " # Extract the signals into more familiar variable names\n", + " r, y = u[0], u[1]\n", + " z = x[0] # integral error\n", + " xi = x[1] # filtered derivative\n", + "\n", + " # Compute the controller components\n", + " u_prop = kp * (r - y)\n", + " u_int = z\n", + " ydt_f = -a * xi + a * (-y)\n", + " u_der = kd * ydt_f\n", + "\n", + " # Compute the commanded and saturated outputs\n", + " u_cmd = u_prop + u_int + u_der\n", + " u_sat = np.clip(u_cmd, -umax_ctrl, umax_ctrl)\n", + "\n", + " return u_cmd\n", + "\n", + "ctrl = ct.nlsys(\n", + " ctrl_update, ctrl_output, name='ctrl', params=ctrl_params,\n", + " inputs=['r', 'y'], outputs=['u'], states=2)\n", + "\n", + "clsys = ct.interconnect(\n", + " [servomech, actuator, ctrl], name='clsys',\n", + " inputs=['r'], outputs=['y', 'tau'])\n", + "print(clsys)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the step responses for the following cases:\n", + "#\n", + "# 'linear': the original linear step response (no actuation limits)\n", + "# 'clipped': PID controller with input limits, but not anti-windup\n", + "# 'anti-windup': PID controller with anti-windup compensation\n", + "\n", + "# Use more time points to get smoother response curves\n", + "timepts = np.linspace(0, 2*stepresp_shape.time[-1], 500)\n", + "\n", + "# Compute the response for the individual cases\n", + "stepsize = theta_e / 2\n", + "resp_ln = ct.input_output_response(\n", + " clsys, timepts, stepsize, params={'umax': np.inf, 'kaw': 0, 'a': 1e3})\n", + "resp_cl = ct.input_output_response(\n", + " clsys, timepts, stepsize, params={'umax': 5, 'kaw': 0, 'a': 100})\n", + "resp_aw = ct.input_output_response(\n", + " clsys, timepts, stepsize, params={'umax': 5, 'kaw': 2*ki, 'a': 100})\n", + "\n", + "# Plot the time responses in a single plot\n", + "ct.time_response_plot(resp_ln, color='b', plot_inputs=False, label=\"linear\")\n", + "ct.time_response_plot(resp_cl, color='r', plot_inputs=False, label=\"clipped\")\n", + "ct.time_response_plot(resp_aw, color='g', plot_inputs=False, label=\"anti-windup\");" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "DZS7v0EmdK3H" + }, + "source": [ + "The response of the anti-windup compensator is very sluggish, indicating that we may be setting $k_\\text{aw}$ too high." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "resp_aw = ct.input_output_response(\n", + " clsys, timepts, stepsize, params={'umax': 5, 'kaw': 0.05 * ki, 'a': 100})\n", + "\n", + "# Plot the time responses in a single plot\n", + "ct.time_response_plot(resp_ln, color='b', plot_inputs=False, label=\"linear\")\n", + "ct.time_response_plot(resp_cl, color='r', plot_inputs=False, label=\"clipped\")\n", + "ct.time_response_plot(resp_aw, color='g', plot_inputs=False, label=\"anti-windup\");" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "pCp_pu0Kh62b" + }, + "source": [ + "This gives a much better response, though the value of $k_\\text{aw}$ falls well outside the range of [2, 10]. One reason for this is that $k_\\text{aw}$ acts on the inputs, $\\tau$, which are roughly 100 larger than the size of the outputs, $y$, as seen in the above plots." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "1FVGh3k0Y7vB" + }, + "source": [ + "We can now see if this affects the Gang of Four in the expected way:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "C = ctrl.linearize([0, 0], 0, params=resp_aw.params)[0, 1]\n", + "ct.gangof4(P, C);" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "id": "vT1WfhRHb2ZU" + }, + "source": [ + "Note that in the transfer function from $r$ to $u$ (which is the same as the transfer function from $e$ to $u$, the high frequency gain is now bounded. (We could make it go back down by using a second order filter.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/cds112-L1_python-control.ipynb b/examples/cds112-L1_python-control.ipynb new file mode 100644 index 000000000..3f1a03487 --- /dev/null +++ b/examples/cds112-L1_python-control.ipynb @@ -0,0 +1,444 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "numerous-rochester", + "metadata": {}, + "source": [ + "# Introduction to the Python Control Systems Library (python-control)\n", + "\n", + "## Input/Output Systems" + ] + }, + { + "cell_type": "markdown", + "id": "69bdd3af", + "metadata": {}, + "source": [ + "Richard M. Murray, 13 Nov 2021 (updated 7 Jul 2024)\n", + "\n", + "This notebook contains an introduction to the basic operations in the Python Control Systems Library (python-control), a Python package for control system design. This notebook is focused on state space control design for a kinematic car, including trajectory generation and gain-scheduled feedback control. This illustrates the use of the input/output (I/O) system class, which can be used to construct models for nonlinear control systems." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "macro-vietnamese", + "metadata": {}, + "outputs": [], + "source": [ + "# Import the packages needed for the examples included in this notebook\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "print(\"python-control version:\", ct.__version__)" + ] + }, + { + "cell_type": "markdown", + "id": "distinct-communist", + "metadata": {}, + "source": [ + "### Installation hints\n", + "\n", + "If you get an error importing the `control` package, it may be that it is not in your current Python path. You can fix this by setting the PYTHONPATH environment variable to include the directory where the python-control package is located. If you are invoking Jupyter from the command line, try using a command of the form\n", + "\n", + " PYTHONPATH=/path/to/control jupyter notebook\n", + " \n", + "If you are using [Google Colab](https://colab.research.google.com), use the following command at the top of the notebook to install the `control` package:\n", + "\n", + " !pip install control\n", + " \n", + "For the examples below, you will need version 0.10.0 or higher of the python-control toolbox. You can find the version number using the command\n", + "\n", + " print(ct.__version__)" + ] + }, + { + "cell_type": "markdown", + "id": "5dad04d8", + "metadata": {}, + "source": [ + "### More information on Python, NumPy, python-control\n", + "\n", + "* [Python tutorial](https://docs.python.org/3/tutorial/)\n", + "* [NumPy tutorial](https://numpy.org/doc/stable/user/quickstart.html)\n", + "* [NumPy for MATLAB users](https://numpy.org/doc/stable/user/numpy-for-matlab-users.html), \n", + "* [Python Control Systems Library (python-control) documentation](https://python-control.readthedocs.io/en/latest/)" + ] + }, + { + "cell_type": "markdown", + "id": "novel-geology", + "metadata": {}, + "source": [ + "## System Definiton\n", + "\n", + "We now define the dynamics of the system that we are going to use for the control design. The dynamics of the system will be of the form\n", + "\n", + "$$\n", + "\\dot x = f(x, u), \\qquad y = h(x, u)\n", + "$$\n", + "\n", + "where $x$ is the state vector for the system, $u$ represents the vector of inputs, and $y$ represents the vector of outputs.\n", + "\n", + "The python-control package allows definition of input/output sytems using the `InputOutputSystem` class and its various subclasess, including the `NonlinearIOSystem` class that we use here. A `NonlinearIOSystem` object is created by defining the update law ($f(x, u)$) and the output map ($h(x, u)$), and then calling the factory function `ct.nlsys`.\n", + "\n", + "For the example in this notebook, we will be controlling the steering of a vehicle, using a \"bicycle\" model for the dynamics of the vehicle. A more complete description of the dynamics of this system are available in [Example 3.11](https://fbswiki.org/wiki/index.php/System_Modeling) of [_Feedback Systems_](https://fbswiki.org/wiki/index.php/FBS) by Astrom and Murray (2020)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "sufficient-douglas", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the update rule for the system, f(x, u)\n", + "# States: x, y, theta (postion and angle of the center of mass)\n", + "# Inputs: v (forward velocity), delta (steering angle)\n", + "def vehicle_update(t, x, u, params):\n", + " # Get the parameters for the model\n", + " a = params.get('refoffset', 1.5) # offset to vehicle reference point\n", + " b = params.get('wheelbase', 3.) # vehicle wheelbase\n", + " maxsteer = params.get('maxsteer', 0.5) # max steering angle (rad)\n", + "\n", + " # Saturate the steering input\n", + " delta = np.clip(u[1], -maxsteer, maxsteer)\n", + " alpha = np.arctan2(a * np.tan(delta), b)\n", + "\n", + " # Return the derivative of the state\n", + " return np.array([\n", + " u[0] * np.cos(x[2] + alpha), # xdot = cos(theta + alpha) v\n", + " u[0] * np.sin(x[2] + alpha), # ydot = sin(theta + alpha) v\n", + " (u[0] / a) * np.sin(alpha) # thdot = v sin(alpha) / a\n", + " ])\n", + "\n", + "# Define the readout map for the system, h(x, u)\n", + "# Outputs: x, y (planar position of the center of mass)\n", + "def vehicle_output(t, x, u, params):\n", + " return x\n", + "\n", + "# Default vehicle parameters (including nominal velocity)\n", + "vehicle_params={'refoffset': 1.5, 'wheelbase': 3, 'velocity': 15, \n", + " 'maxsteer': 0.5}\n", + "\n", + "# Define the vehicle steering dynamics as an input/output system\n", + "vehicle = ct.nlsys(\n", + " vehicle_update, vehicle_output, states=3, name='vehicle',\n", + " inputs=['v', 'delta'], outputs=['x', 'y', 'theta'], params=vehicle_params)" + ] + }, + { + "cell_type": "markdown", + "id": "intellectual-democrat", + "metadata": {}, + "source": [ + "## Open loop simulation\n", + "\n", + "After these operations, the `vehicle` object references the nonlinear model for the system. This system can be simulated to compute a trajectory for the system. Here we command a velocity of 10 m/s and turn the wheel back and forth at one Hertz." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "likely-hindu", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the time interval that we want to use for the simualation\n", + "timepts = np.linspace(0, 10, 1000)\n", + "\n", + "# Define the inputs\n", + "U = [\n", + " 10 * np.ones_like(timepts), # velocity\n", + " 0.1 * np.sin(timepts * 2*np.pi) # steering angle\n", + "]\n", + "\n", + "# Simulate the system dynamics, starting from the origin\n", + "response = ct.input_output_response(vehicle, timepts, U, 0)\n", + "time, outputs, inputs = response.time, response.outputs, response.inputs" + ] + }, + { + "cell_type": "markdown", + "id": "dutch-charm", + "metadata": {}, + "source": [ + "We can plot the results using standard `matplotlib` commands:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "piano-algeria", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a figure to plot the results\n", + "fig, ax = plt.subplots(2, 1)\n", + "\n", + "# Plot the results in the xy plane\n", + "ax[0].plot(outputs[0], outputs[1])\n", + "ax[0].set_xlabel(\"$x$ [m]\")\n", + "ax[0].set_ylabel(\"$y$ [m]\")\n", + "\n", + "# Plot the inputs\n", + "ax[1].plot(timepts, U[0])\n", + "ax[1].set_ylim(0, 12)\n", + "ax[1].set_xlabel(\"Time $t$ [s]\")\n", + "ax[1].set_ylabel(\"Velocity $v$ [m/s]\")\n", + "ax[1].yaxis.label.set_color('blue')\n", + "\n", + "rightax = ax[1].twinx() # Create an axis in the right\n", + "rightax.plot(timepts, U[1], color='red')\n", + "rightax.set_ylim(None, 0.5)\n", + "rightax.set_ylabel(r\"Steering angle $\\phi$ [rad]\")\n", + "rightax.yaxis.label.set_color('red')\n", + "\n", + "fig.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "alone-worry", + "metadata": {}, + "source": [ + "Notice that there is a small drift in the $y$ position despite the fact that the steering wheel is moved back and forth symmetrically around zero. Exercise: explain what might be happening." + ] + }, + { + "cell_type": "markdown", + "id": "portable-rubber", + "metadata": {}, + "source": [ + "## Linearize the system around a trajectory\n", + "\n", + "We choose a straight path along the $x$ axis at a speed of 10 m/s as our desired trajectory and then linearize the dynamics around the initial point in that trajectory." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "surprising-algorithm", + "metadata": {}, + "outputs": [], + "source": [ + "# Create the desired trajectory \n", + "Ud = np.array([10 * np.ones_like(timepts), np.zeros_like(timepts)])\n", + "Xd = np.array([10 * timepts, 0 * timepts, np.zeros_like(timepts)])\n", + "\n", + "# Now linizearize the system around this trajectory\n", + "linsys = vehicle.linearize(Xd[:, 0], Ud[:, 0])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "protecting-committee", + "metadata": {}, + "outputs": [], + "source": [ + "# Check on the eigenvalues of the open loop system\n", + "np.linalg.eigvals(linsys.A)" + ] + }, + { + "cell_type": "markdown", + "id": "trying-stereo", + "metadata": {}, + "source": [ + "We see that all eigenvalues are zero, corresponding to a single integrator in the $x$ (longitudinal) direction and a double integrator in the $y$ (lateral) direction." + ] + }, + { + "cell_type": "markdown", + "id": "pressed-delta", + "metadata": {}, + "source": [ + "## Compute a state space (LQR) control law\n", + "\n", + "We can now compute a feedback controller around the trajectory. We choose a simple LQR controller here, but any method can be used." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "auburn-caribbean", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute LQR controller\n", + "K, S, E = ct.lqr(linsys, np.diag([1, 1, 1]), np.diag([1, 1]))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "independent-lafayette", + "metadata": {}, + "outputs": [], + "source": [ + "# Check on the eigenvalues of the closed loop system\n", + "np.linalg.eigvals(linsys.A - linsys.B @ K)" + ] + }, + { + "cell_type": "markdown", + "id": "handmade-moral", + "metadata": {}, + "source": [ + "The closed loop eigenvalues have negative real part, so the closed loop (linear) system will be stable about the operating trajectory." + ] + }, + { + "cell_type": "markdown", + "id": "handy-virgin", + "metadata": {}, + "source": [ + "## Create a controller with feedforward and feedback\n", + "\n", + "We now create an I/O system representing the control law. The controller takes as an input the desired state space trajectory $x_\\text{d}$ and the nominal input $u_\\text{d}$. It outputs the control law\n", + "\n", + "$$\n", + "u = u_\\text{d} - K(x - x_\\text{d}).\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "negative-scope", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the output rule for the controller\n", + "# States: none (=> no update rule required)\n", + "# Inputs: z = [xd, ud, x]\n", + "# Outputs: v (forward velocity), delta (steering angle)\n", + "def control_output(t, x, z, params):\n", + " # Get the parameters for the model\n", + " K = params.get('K', np.zeros((2, 3))) # nominal gain\n", + " \n", + " # Split up the input to the controller into the desired state and nominal input\n", + " xd_vec = z[0:3] # desired state ('xd', 'yd', 'thetad')\n", + " ud_vec = z[3:5] # nominal input ('vd', 'deltad')\n", + " x_vec = z[5:8] # current state ('x', 'y', 'theta')\n", + " \n", + " # Compute the control law\n", + " return ud_vec - K @ (x_vec - xd_vec)\n", + "\n", + "# Define the controller system\n", + "control = ct.nlsys(\n", + " None, control_output, name='control',\n", + " inputs=['xd', 'yd', 'thetad', 'vd', 'deltad', 'x', 'y', 'theta'], \n", + " outputs=['v', 'delta'], params={'K': K})" + ] + }, + { + "cell_type": "markdown", + "id": "affected-motor", + "metadata": {}, + "source": [ + "Because we have named the signals in both the vehicle model and the controller in a compatible way, we can use the autoconnect feature of the `interconnect()` function to create the closed loop system." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "stock-regression", + "metadata": {}, + "outputs": [], + "source": [ + "# Build the closed loop system\n", + "vehicle_closed = ct.interconnect(\n", + " (vehicle, control),\n", + " inputs=['xd', 'yd', 'thetad', 'vd', 'deltad'],\n", + " outputs=['x', 'y', 'theta']\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "hispanic-monroe", + "metadata": {}, + "source": [ + "## Closed loop simulation\n", + "\n", + "We now command the system to follow in trajectory and use the linear controller to correct for any errors. \n", + "\n", + "The desired trajectory is a given by a longitudinal position that tracks a velocity of 10 m/s for the first 5 seconds and then increases to 12 m/s and a lateral position that varies sinusoidally by $\\pm 0.5$ m around the centerline. The nominal inputs are not modified, so that feedback is required to obtained proper trajectory tracking." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "american-return", + "metadata": {}, + "outputs": [], + "source": [ + "Xd = np.array([\n", + " 10 * timepts + 2 * (timepts-5) * (timepts > 5), \n", + " 0.5 * np.sin(timepts * 2*np.pi), \n", + " np.zeros_like(timepts)\n", + "])\n", + "\n", + "Ud = np.array([10 * np.ones_like(timepts), np.zeros_like(timepts)])\n", + "\n", + "# Simulate the system dynamics, starting from the origin\n", + "resp = ct.input_output_response(\n", + " vehicle_closed, timepts, np.vstack((Xd, Ud)), 0)\n", + "time, outputs = resp.time, resp.outputs" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "indirect-longitude", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the results in the xy plane\n", + "plt.plot(Xd[0], Xd[1], 'b--') # desired trajectory\n", + "plt.plot(outputs[0], outputs[1]) # actual trajectory\n", + "plt.xlabel(\"$x$ [m]\")\n", + "plt.ylabel(\"$y$ [m]\")\n", + "plt.ylim(-1, 2)\n", + "\n", + "# Add a legend\n", + "plt.legend(['desired', 'actual'], loc='upper left')\n", + "\n", + "# Compute and plot the velocity\n", + "rightax = plt.twinx() # Create an axis in the right\n", + "rightax.plot(Xd[0, :-1], np.diff(Xd[0]) / np.diff(timepts), 'r--')\n", + "rightax.plot(outputs[0, :-1], np.diff(outputs[0]) / np.diff(timepts), 'r-')\n", + "rightax.set_ylim(0, 13)\n", + "rightax.set_ylabel(\"$x$ velocity [m/s]\")\n", + "rightax.yaxis.label.set_color('red')" + ] + }, + { + "cell_type": "markdown", + "id": "weighted-directory", + "metadata": {}, + "source": [ + "We see that there is a small error in each axis. By adjusting the weights in the LQR controller we can adjust the steady state error (try it!)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f31dd981-161a-49f0-a637-84128f7ec5ff", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L2a_flatness.ipynb b/examples/cds112-L2a_flatness.ipynb new file mode 100644 index 000000000..2b7cfb3a4 --- /dev/null +++ b/examples/cds112-L2a_flatness.ipynb @@ -0,0 +1,490 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "meaning-hypothetical", + "metadata": {}, + "source": [ + "## Differential Flatness\n", + "\n", + "##### Richard M. Murray, 13 Nov 2021 (updated 7 Jul 2024)\n", + "\n", + "This notebook contains an example of using differential flatness as a mechanism for trajectory generation for a nonlinear control system. A differentially flat system is defined by creating an object using the `FlatSystem` class, which has member functions for mapping the system state and input into and out of flat coordinates. The `point_to_point()` function can be used to create a trajectory between two endpoints, written in terms of a set of basis functions defined using the `BasisFamily` class. The resulting trajectory is return as a `SystemTrajectory` object and can be evaluated using the `eval()` member function. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "historic-barbados", + "metadata": {}, + "outputs": [], + "source": [ + "# Import the packages needed for the examples included in this notebook\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "import control.flatsys as fs\n", + "import control.optimal as opt\n", + "import time" + ] + }, + { + "cell_type": "markdown", + "id": "309d3272", + "metadata": {}, + "source": [ + "## Example: bicycle model\n", + "\n", + "To illustrate the methods of generating trajectories using differential flatness, we make use of a simple model for a vehicle navigating in the plane, known as the \"bicycle model\". The kinematics of this vehicle can be written in terms of the contact point $(x, y)$ and the angle $\\theta$ of the vehicle with respect to the horizontal axis:\n", + "\n", + "
\n", + "\n", + "\n", + "\n", + "
\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "The input $v$ represents the velocity of the vehicle and the input $\\delta$ represents the turning rate. The parameter $l$ is the wheelbase." + ] + }, + { + "cell_type": "markdown", + "id": "35efac80", + "metadata": {}, + "source": [ + "We will generate trajectories for this system that correspond to a \"lane change\", in which we travel longitudinally at a fixed speed for approximately 40 meters, while moving from the right to the left by a distance of 4 meters.\n", + "\n", + "It will be convenient to define a function that we will use to plot the results in a uniform way. In addition to the subplot, we also change the size of the figure to make the figure wider." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "involved-riding", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the trajectory in xy coordinates\n", + "def plot_motion(t, x, ud):\n", + " # Set the size of the figure\n", + " # plt.figure(figsize=(10, 6))\n", + "\n", + " # Top plot: xy trajectory\n", + " plt.subplot(2, 1, 1)\n", + " plt.plot(x[0], x[1])\n", + " plt.xlabel('x [m]')\n", + " plt.ylabel('y [m]')\n", + " plt.axis([x0[0], xf[0], x0[1]-1, xf[1]+1])\n", + "\n", + " # Time traces of the state and input\n", + " plt.subplot(2, 4, 5)\n", + " plt.plot(t, x[1])\n", + " plt.ylabel('y [m]')\n", + "\n", + " plt.subplot(2, 4, 6)\n", + " plt.plot(t, x[2])\n", + " plt.ylabel('theta [rad]')\n", + "\n", + " plt.subplot(2, 4, 7)\n", + " plt.plot(t, ud[0])\n", + " plt.xlabel(\"Time t [sec]\")\n", + " plt.ylabel(\"v [m/s]\")\n", + " plt.axis([0, Tf, u0[0] - 1, uf[0] + 1])\n", + "\n", + " plt.subplot(2, 4, 8)\n", + " plt.plot(t, ud[1])\n", + " plt.xlabel(\"Time t [sec]\")\n", + " plt.ylabel(r\"$\\delta$ [rad]\")\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "3dc0d2bf", + "metadata": {}, + "source": [ + "## Flat system mappings\n", + "\n", + "To define a flat system, we have to define the functions that take the state and compute the flat \"flag\" (flat outputs and their derivatives) and that take the flat flag and return the state and input.\n", + "\n", + "The `forward()` method computes the flat flag given a state and input:\n", + "```\n", + " zflag = sys.forward(x, u)\n", + "```\n", + "The `reverse()` method computes the state and input given the flat flag:\n", + "```\n", + " x, u = sys.reverse(zflag)\n", + "```\n", + "The flag $\\bar z$ is implemented as a list of flat outputs $z_i$ and\n", + "their derivatives up to order $q_i$:\n", + "\n", + "         `zflag[i][j]` = $z_i^{(j)}$\n", + "\n", + "The number of flat outputs must match the number of system inputs.\n", + "\n", + "In addition, a flat system is an input/output system and so we define and update function ($f(x, u)$) and output (use `None` to get the full state)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "above-venezuela", + "metadata": {}, + "outputs": [], + "source": [ + "# Function to take states, inputs and return the flat flag\n", + "def bicycle_flat_forward(x, u, params={}):\n", + " # Get the parameter values\n", + " b = params.get('wheelbase', 3.)\n", + "\n", + " # Create a list of arrays to store the flat output and its derivatives\n", + " zflag = [np.zeros(3), np.zeros(3)]\n", + "\n", + " # Flat output is the x, y position of the rear wheels\n", + " zflag[0][0] = x[0]\n", + " zflag[1][0] = x[1]\n", + "\n", + " # First derivatives of the flat output\n", + " zflag[0][1] = u[0] * np.cos(x[2]) # dx/dt\n", + " zflag[1][1] = u[0] * np.sin(x[2]) # dy/dt\n", + "\n", + " # First derivative of the angle\n", + " thdot = (u[0]/b) * np.tan(u[1])\n", + "\n", + " # Second derivatives of the flat output (setting vdot = 0)\n", + " zflag[0][2] = -u[0] * thdot * np.sin(x[2])\n", + " zflag[1][2] = u[0] * thdot * np.cos(x[2])\n", + "\n", + " return zflag\n", + "\n", + "# Function to take the flat flag and return states, inputs\n", + "def bicycle_flat_reverse(zflag, params={}):\n", + " # Get the parameter values\n", + " b = params.get('wheelbase', 3.)\n", + "\n", + " # Create a vector to store the state and inputs\n", + " x = np.zeros(3)\n", + " u = np.zeros(2)\n", + "\n", + " # Given the flat variables, solve for the state\n", + " x[0] = zflag[0][0] # x position\n", + " x[1] = zflag[1][0] # y position\n", + " x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot\n", + "\n", + " # And next solve for the inputs\n", + " u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])\n", + " thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])\n", + " u[1] = np.arctan2(thdot_v, u[0]**2 / b)\n", + "\n", + " return x, u\n", + "\n", + "# Function to compute the RHS of the system dynamics\n", + "def bicycle_update(t, x, u, params):\n", + " b = params.get('wheelbase', 3.) # get parameter values\n", + " dx = np.array([\n", + " np.cos(x[2]) * u[0],\n", + " np.sin(x[2]) * u[0],\n", + " (u[0]/b) * np.tan(u[1])\n", + " ])\n", + " return dx\n", + "\n", + "# Return the entire state as output (instead of default flat outputs)\n", + "def bicycle_output(t, x, u, params):\n", + " return x\n", + "\n", + "# Create differentially flat input/output system\n", + "bicycle_flat = fs.FlatSystem(\n", + " bicycle_flat_forward, bicycle_flat_reverse, \n", + " bicycle_update, bicycle_output,\n", + " inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n", + " states=('x', 'y', 'theta'), name='bicycle_model')\n", + "\n", + "print(bicycle_flat)" + ] + }, + { + "cell_type": "markdown", + "id": "75cb8cf6", + "metadata": {}, + "source": [ + "## Point to point trajectory generation\n", + "\n", + "In addition to the flat system description, a set of basis functions\n", + "$\\phi_i(t)$ must be chosen. The `BasisFamily` class is used to\n", + "represent the basis functions. A polynomial basis function of the form\n", + "$1$, $t$, $t^2$, $\\ldots$ can be computed using the `PolyFamily` class,\n", + "which is initialized by passing the desired order of the polynomial\n", + "basis set:\n", + "```\n", + "polybasis = control.flatsys.PolyFamily(N)\n", + "```\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "feef608a", + "metadata": {}, + "outputs": [], + "source": [ + "print(fs.BasisFamily.__doc__)\n", + "print(fs.PolyFamily.__doc__)\n", + "\n", + "# Define a set of basis functions to use for the trajectories\n", + "poly = fs.PolyFamily(6)\n", + "\n", + "# Plot out the basis functions\n", + "t = np.linspace(0, 1.5)\n", + "for k in range(poly.N):\n", + " plt.plot(t, poly(k, t), label=f'k = {k}')\n", + " \n", + "plt.legend()\n", + "plt.title(\"Polynomial basis functions\")\n", + "plt.xlabel(\"Time $t$\")\n", + "plt.ylabel(r\"$\\psi_i(t)$\");" + ] + }, + { + "cell_type": "markdown", + "id": "7aacca93", + "metadata": {}, + "source": [ + "### Approach 1: point to point solution, no cost or constraints\n", + "\n", + "Once the system and basis function have been defined, the\n", + "`point_to_point()` function can be used to compute a trajectory\n", + "between initial and final states and inputs:\n", + "```\n", + "traj = control.flatsys.point_to_point(sys, Tf, x0, u0, xf, uf, basis=polybasis)\n", + "```\n", + "The returned object has class `SystemTrajectory` and can be used\n", + "to compute the state and input trajectory between the initial and final\n", + "condition:\n", + "```\n", + "xd, ud = traj.eval(timepts)\n", + "```\n", + "where `timepts` is a list of times on which the trajectory should be\n", + "evaluated (e.g., `timepts = numpy.linspace(0, Tf, M)`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "surface-piano", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the endpoints of the trajectory\n", + "x0 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])\n", + "xf = np.array([40., 2., 0.]); uf = np.array([10., 0.])\n", + "Tf = 4\n", + "\n", + "# Generate a normalized set of basis functions\n", + "poly = fs.PolyFamily(6, Tf)\n", + "\n", + "# Find a trajectory between the initial condition and the final condition\n", + "traj = fs.point_to_point(bicycle_flat, Tf, x0, u0, xf, uf, basis=poly)\n", + "\n", + "# Create the desired trajectory between the initial and final condition\n", + "timepts = np.linspace(0, Tf, 500)\n", + "xd, ud = traj.eval(timepts)\n", + "\n", + "# Simulation the open system dynamics with the full input\n", + "t, y, x = ct.input_output_response(\n", + " bicycle_flat, timepts, ud, x0, return_x=True)\n", + "\n", + "# Plot the open loop system dynamics\n", + "plt.figure(1)\n", + "plt.suptitle(\"Open loop trajectory for unicycle lane change\")\n", + "plot_motion(t, x, ud)\n", + "\n", + "# Make sure the initial and final points are correct\n", + "print(\"x[0] = \", xd[:, 0])\n", + "print(\"x[T] = \", xd[:, -1])" + ] + }, + { + "cell_type": "markdown", + "id": "82a3318a", + "metadata": {}, + "source": [ + "### A look inside the code\n", + "\n", + "The code to solve this problem is inside the file [flatsys.py](https://github.com/python-control/python-control/blob/main/control/flatsys/flatsys.py) in the python-control package. Here is what operative code inside the `point_to_point()` looks like:\n", + "\n", + " #\n", + " # Map the initial and final conditions to flat output conditions\n", + " #\n", + " # We need to compute the output \"flag\": [z(t), z'(t), z''(t), ...]\n", + " # and then evaluate this at the initial and final condition.\n", + " #\n", + "\n", + " zflag_T0 = sys.forward(x0, u0)\n", + " zflag_Tf = sys.forward(xf, uf)\n", + "\n", + " #\n", + " # Compute the matrix constraints for initial and final conditions\n", + " #\n", + " # This computation depends on the basis function we are using. It\n", + " # essentially amounts to evaluating the basis functions and their\n", + " # derivatives at the initial and final conditions.\n", + "\n", + " # Compute the flags for the initial and final states\n", + " M_T0 = _basis_flag_matrix(sys, basis, zflag_T0, T0)\n", + " M_Tf = _basis_flag_matrix(sys, basis, zflag_Tf, Tf)\n", + "\n", + " # Stack the initial and final matrix/flag for the point to point problem\n", + " M = np.vstack([M_T0, M_Tf])\n", + " Z = np.hstack([np.hstack(zflag_T0), np.hstack(zflag_Tf)])\n", + "\n", + " #\n", + " # Solve for the coefficients of the flat outputs\n", + " #\n", + " # At this point, we need to solve the equation M alpha = zflag, where M\n", + " # is the matrix constrains for initial and final conditions and zflag =\n", + " # [zflag_T0; zflag_tf].\n", + " #\n", + " # If there are no constraints, then we just need to solve a linear\n", + " # system of equations => use least squares. Otherwise, we have a\n", + " # nonlinear optimal control problem with equality constraints => use\n", + " # scipy.optimize.minimize().\n", + " #\n", + "\n", + " # Start by solving the least squares problem\n", + " alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)" + ] + }, + { + "cell_type": "markdown", + "id": "f0397b3e", + "metadata": {}, + "source": [ + "### Approach #2: add cost function to make lane change quicker" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "appreciated-baghdad", + "metadata": {}, + "outputs": [], + "source": [ + "# Define timepoints for evaluation plus basis function to use\n", + "timepts = np.linspace(0, Tf, 20)\n", + "basis = fs.PolyFamily(12, Tf)\n", + "\n", + "# Define the cost function (penalize lateral error and steering)\n", + "traj_cost = opt.quadratic_cost(\n", + " bicycle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 1]), x0=xf, u0=uf)\n", + "\n", + "# Solve for an optimal solution\n", + "start_time = time.process_time()\n", + "traj = fs.point_to_point(\n", + " bicycle_flat, timepts, x0, u0, xf, uf, cost=traj_cost, basis=basis,\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "xd, ud = traj.eval(timepts)\n", + "\n", + "plt.figure(2)\n", + "plt.suptitle(\"Lane change with lateral error + steering penalties\")\n", + "plot_motion(timepts, xd, ud);" + ] + }, + { + "cell_type": "markdown", + "id": "ff7363ca", + "metadata": {}, + "source": [ + "Note that the solution has a very large steering angle (0.2 rad = ~12 degrees)." + ] + }, + { + "cell_type": "markdown", + "id": "3c533abe", + "metadata": {}, + "source": [ + "### Approach #3: optimal cost with trajectory constraints\n", + "\n", + "To get a smaller steering angle, we add constraints on the inputs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "stable-network", + "metadata": {}, + "outputs": [], + "source": [ + "constraints = [\n", + " opt.input_range_constraint(bicycle_flat, [8, -0.1], [12, 0.1]) ]\n", + "\n", + "# Solve for an optimal solution\n", + "traj = fs.point_to_point(\n", + " bicycle_flat, timepts, x0, u0, xf, uf, cost=traj_cost,\n", + " trajectory_constraints=constraints, basis=basis,\n", + ")\n", + "xd, ud = traj.eval(timepts)\n", + "\n", + "plt.figure(3)\n", + "plt.suptitle(\"Lane change with penalty + steering constraints\")\n", + "plot_motion(timepts, xd, ud)" + ] + }, + { + "cell_type": "markdown", + "id": "677750b0", + "metadata": {}, + "source": [ + "## Ideas to explore\n", + "* Change the number of basis functions\n", + "* Change the number of time points\n", + "* Change the type of basis functions: BezierFamily" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1622bccd", + "metadata": {}, + "outputs": [], + "source": [ + "# Define a set of basis functions to use for the trajectories\n", + "poly = fs.BezierFamily(6, 2)\n", + "\n", + "# Plot out the basis functions\n", + "t = np.linspace(0, 2)\n", + "for k in range(poly.N):\n", + " plt.plot(t, poly(k, t), label=f'k = {k}')\n", + " \n", + "plt.legend()\n", + "plt.title(\"Bezier basis functions\")\n", + "plt.xlabel(\"Time $t$\")\n", + "plt.ylabel(r\"$\\psi_i(t)$\");" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "fc566fb2", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L2b_gainsched.ipynb b/examples/cds112-L2b_gainsched.ipynb new file mode 100644 index 000000000..d915f9e3d --- /dev/null +++ b/examples/cds112-L2b_gainsched.ipynb @@ -0,0 +1,408 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "exempt-legislation", + "metadata": {}, + "source": [ + "# Gain Scheduling\n", + "\n", + "##### Richard M. Murray, 19 Nov 2021 (updated 7 Jul 2024)\n", + "\n", + "This notebook contains an example of using gain scheduling for feedback control of a nonlinear system. A gain scheduled controller has feedback gains that depend on a set of measured parameters in the system. For exampe:\n", + "\n", + "$$\n", + " u = u_\\text{d} − K(x_\\text{d}, u_\\text{d}) (x − x_\\text{d}),\n", + "$$\n", + "\n", + "where $K(x_\\text{d}, u_\\text{d})$ depends on the desired system state and input.\n", + "\n", + "In this notebook, we work through the gain scheduled controller in Example 2.1 of OBC." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "corresponding-convenience", + "metadata": {}, + "outputs": [], + "source": [ + "# Import the packages needed for the examples included in this notebook\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "from cmath import sqrt\n", + "import control as ct" + ] + }, + { + "cell_type": "markdown", + "id": "corporate-sense", + "metadata": {}, + "source": [ + "## Vehicle Steering Dynamics\n", + "\n", + "The vehicle dynamics are given by a simple bicycle model:\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "We take the state of the system as $(x, y, \\theta)$ where $(x, y)$ is the position of the vehicle in the plane and $\\theta$ is the angle of the vehicle with respect to horizontal. The vehicle input is given by $(v, \\delta)$ where $v$ is the forward velocity of the vehicle and $\\delta$ is the angle of the steering wheel. The model includes saturation of the vehicle steering angle." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "naval-pizza", + "metadata": {}, + "outputs": [], + "source": [ + "# Bicycle model dynamics\n", + "#\n", + "# System state: x, y, theta\n", + "# System input: v, delta\n", + "# System output: x, y\n", + "# System parameters: wheelbase, maxsteer\n", + "#\n", + "def bicycle_update(t, x, u, params):\n", + " # Get the parameters for the model\n", + " l = params.get('wheelbase', 3.) # vehicle wheelbase\n", + " deltamax = params.get('maxsteer', 0.5) # max steering angle (rad)\n", + "\n", + " # Saturate the steering input\n", + " delta = np.clip(u[1], -deltamax, deltamax)\n", + "\n", + " # Return the derivative of the state\n", + " return np.array([\n", + " np.cos(x[2]) * u[0], # xdot = cos(theta) v\n", + " np.sin(x[2]) * u[0], # ydot = sin(theta) v\n", + " (u[0] / l) * np.tan(delta) # thdot = v/l tan(delta)\n", + " ])\n", + "\n", + "def bicycle_output(t, x, u, params):\n", + " return x # return x, y, theta (full state)\n", + "\n", + "# Define the vehicle steering dynamics as an input/output system\n", + "bicycle = ct.nlsys(\n", + " bicycle_update, bicycle_output, states=3, name='bicycle',\n", + " inputs=('v', 'delta'),\n", + " outputs=('x', 'y', 'theta'))" + ] + }, + { + "cell_type": "markdown", + "id": "3cc26675", + "metadata": {}, + "source": [ + "## Gain scheduled controller\n", + "\n", + "For this system we use a simple schedule on the forward vehicle velocity and\n", + "place the poles of the system at fixed values. The controller takes the\n", + "current and desired vehicle position and orientation plus the velocity\n", + "velocity as inputs, and returns the velocity and steering commands.\n", + "\n", + "Linearizing the system about the desired trajectory, we obtain\n", + "\n", + "$$\n", + " \\begin{aligned}\n", + " A(x_\\text{d}) &= \\left. \\frac{\\partial f}{\\partial x} \\right|_{(x_\\text{d}, u_\\text{d})}\n", + " = \\left.\n", + " \\begin{bmatrix}\n", + " 0 & 0 & -\\sin\\theta_\\text{d}\\, v_\\text{d} \\\\ 0 & 0 & \\cos\\theta_\\text{d}\\, v_\\text{d} \\\\ 0 & 0 & 0\n", + " \\end{bmatrix}\n", + " \\right|_{(x_\\text{d}, u_\\text{d})}\n", + " = \\begin{bmatrix}\n", + " 0 & 0 & 0 \\\\ 0 & 0 & v_\\text{d} \\\\ 0 & 0 & 0\n", + " \\end{bmatrix}, \\\\\n", + " B(x_\\text{d}) &= \\left. \\frac{\\partial f}{\\partial u} \\right|_{(x_\\text{d}, u_\\text{d})}\n", + " = \\begin{bmatrix}\n", + " 1 & 0 \\\\ 0 & 0 \\\\ 0 & v_\\text{d}/l\n", + " \\end{bmatrix}.\n", + " \\end{aligned}\n", + "$$\n", + "\n", + "We form the error dynamics by setting $e = x - x_\\text{d}$ and $w = u -\n", + "u_\\text{d}$:\n", + "$$\n", + " \\dot e_x = w_1, \\qquad \\dot e_y = e_\\theta, \\qquad \\dot e_\\theta =\n", + " \\frac{v_\\text{d}}{l} w_2.\n", + "$$\n", + "We see that the first state is decoupled from the second two states\n", + "and hence we can design a controller by treating these two subsystems\n", + "separately. \n", + "\n", + "Suppose that we wish to place the closed loop eigenvalues\n", + "of the longitudinal dynamics ($e_x$) at $-\\lambda_1$ and place the\n", + "closed loop eigenvalues of the lateral dynamics ($e_y$, $e_\\theta$) at\n", + "the roots of the polynomial equation $s^2 + a_1 s + a_2 = 0$.\n", + "\n", + "This can accomplished by setting\n", + "\n", + "$$\n", + " \\begin{aligned}\n", + " w_1 &= -\\lambda_1 e_x \\\\\n", + " w_2 &= -\\frac{l}{v_\\text{r}}(\\frac{a_2}{v_\\text{r}} e_y + a_1 e_\\theta).\n", + " \\end{aligned}\n", + "$$\n", + "\n", + "Note that the gains depend on the velocity $v_\\text{r}$ (or equivalently on\n", + "the nominal input $u_\\text{d}$), giving us a gain scheduled controller." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "another-milwaukee", + "metadata": {}, + "outputs": [], + "source": [ + "# System state: none\n", + "# System input: x, y, theta, xd, yd, thetad, vd, delta\n", + "# System output: v, delta\n", + "# System parameters: longpole, latomega_c, latzeta_c\n", + "def gainsched_output(t, x, u, params):\n", + " # Get the controller parameters\n", + " longpole = params.get('longpole', -2.)\n", + " latomega_c = params.get('latomega_c', 2)\n", + " latzeta_c = params.get('latzeta_c', 0.5)\n", + " l = params.get('wheelbase', 3)\n", + " vref = params.get('vref', None)\n", + " \n", + " # Extract the system inputs and compute the errors\n", + " x, y, theta, xd, yd, thetad, vd, deltad = u\n", + " ex, ey, etheta = x - xd, y - yd, theta - thetad\n", + "\n", + " # Determine the controller gains\n", + " lambda1 = -longpole\n", + " a1 = 2 * latzeta_c * latomega_c\n", + " a2 = latomega_c**2\n", + " \n", + " # Determine the speed to use for computing the gains\n", + " if vref is None:\n", + " vref = vd\n", + "\n", + " # Compute and return the control law\n", + " v = -lambda1 * ex # leave off feedforward to generate transient\n", + " if vd != 0:\n", + " delta = deltad - ((a2 * l) / vref**2) * ey - ((a1 * l) / vref) * etheta\n", + " else:\n", + " # We aren't moving, so don't turn the steering wheel\n", + " delta = deltad\n", + " \n", + " return np.array([v, delta])\n", + "\n", + "# Define the controller as an input/output system\n", + "gainsched = ct.nlsys(\n", + " None, gainsched_output, name='controller', # static system\n", + " inputs=('x', 'y', 'theta', 'xd', 'yd', 'thetad', # system inputs\n", + " 'vd', 'deltad'),\n", + " outputs=('v', 'delta') # system outputs\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "6c6c4b9b", + "metadata": {}, + "source": [ + "## Reference trajectory subsystem\n", + "\n", + "The reference trajectory block generates a simple trajectory for the system\n", + "given the desired speed (vref) and lateral position (yref). The trajectory\n", + "consists of a straight line of the form (vref * t, yref, 0) with nominal\n", + "input (vref, 0)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "significant-november", + "metadata": {}, + "outputs": [], + "source": [ + "# System state: none\n", + "# System input: vref, yref\n", + "# System output: xd, yd, thetad, vd, deltad\n", + "# System parameters: none\n", + "#\n", + "def trajgen_output(t, x, u, params):\n", + " vref, yref = u\n", + " return np.array([vref * t, yref, 0, vref, 0])\n", + "\n", + "# Define the trajectory generator as an input/output system\n", + "trajgen = ct.nlsys(\n", + " None, trajgen_output, name='trajgen',\n", + " inputs=('vref', 'yref'),\n", + " outputs=('xd', 'yd', 'thetad', 'vd', 'deltad'))\n" + ] + }, + { + "cell_type": "markdown", + "id": "4ca5ab53", + "metadata": {}, + "source": [ + "## System construction\n", + "\n", + "The input to the full closed loop system is the desired lateral position and\n", + "the desired forward velocity. The output for the system is taken as the\n", + "full vehicle state plus the velocity of the vehicle.\n", + "\n", + "We construct the system using the InterconnectedSystem constructor and using\n", + "signal labels to keep track of everything. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "editorial-satisfaction", + "metadata": {}, + "outputs": [], + "source": [ + "steering_gainsched = ct.interconnect(\n", + " # List of subsystems\n", + " (trajgen, gainsched, bicycle), name='steering',\n", + "\n", + " # System inputs\n", + " inplist=['trajgen.vref', 'trajgen.yref'],\n", + " inputs=['yref', 'vref'],\n", + "\n", + " # System outputs\n", + " outlist=['bicycle.x', 'bicycle.y', 'bicycle.theta', 'controller.v',\n", + " 'controller.delta'],\n", + " outputs=['x', 'y', 'theta', 'v', 'delta']\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "61fe3404", + "metadata": {}, + "source": [ + "Note the use of signals of the form `sys.sig` to get the signals from a specific subsystem." + ] + }, + { + "cell_type": "markdown", + "id": "47f5d528", + "metadata": {}, + "source": [ + "## System simulation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "smoking-trail", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the simulation conditions\n", + "yref = 1\n", + "T = np.linspace(0, 5, 100)\n", + "\n", + "# Plot the reference trajectory for the y position\n", + "plt.plot([0, 5], [yref, yref], 'k-', linewidth=0.6)\n", + "\n", + "# Find the signals we want to plot\n", + "y_index = steering_gainsched.find_output('y')\n", + "v_index = steering_gainsched.find_output('v')\n", + "\n", + "# Do an iteration through different speeds\n", + "for vref in [5, 10, 15]:\n", + " # Simulate the closed loop controller response\n", + " tout, yout = ct.input_output_response(\n", + " steering_gainsched, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])\n", + "\n", + " # Plot the reference speed\n", + " plt.plot([0, 5], [vref, vref], 'k-', linewidth=0.6)\n", + "\n", + " # Plot the system output\n", + " y_line, = plt.plot(tout, yout[y_index, :], 'r-') # lateral position\n", + " v_line, = plt.plot(tout, yout[v_index, :], 'b--') # vehicle velocity\n", + "\n", + "# Add axis labels\n", + "plt.xlabel(\"Time [s]\")\n", + "plt.ylabel(r\"$\\dot{x}$ [m/s], $y$ [m]\")\n", + "plt.legend((v_line, y_line), (r\"$\\dot{x}$\", \"$y$\"),\n", + " loc='center right', frameon=False);" + ] + }, + { + "cell_type": "markdown", + "id": "8f31bc48", + "metadata": {}, + "source": [ + "## Comparison to fixed controller" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "homeless-gibson", + "metadata": {}, + "outputs": [], + "source": [ + "# Rerun with no gain-scheduling\n", + "\n", + "# Plot the reference trajectory for the y position\n", + "plt.plot([0, 5], [yref, yref], 'k-', linewidth=0.6)\n", + "\n", + "# Do an iteration through different speeds\n", + "for vref in [5, 10, 15]:\n", + " # Simulate the closed loop controller response\n", + " tout, yout = ct.input_output_response(\n", + " steering_gainsched, T, [vref * np.ones(len(T)), yref * np.ones(len(T))], \n", + " params={'vref': 15})\n", + "\n", + " # Plot the reference speed\n", + " plt.plot([0, 5], [vref, vref], 'k-', linewidth=0.6)\n", + "\n", + " # Plot the system output\n", + " y_line, = plt.plot(tout, yout[y_index, :], 'r-') # lateral position\n", + " v_line, = plt.plot(tout, yout[v_index, :], 'b--') # vehicle velocity\n", + "\n", + "# Add axis labels\n", + "plt.xlabel(\"Time [s]\")\n", + "plt.ylabel(r\"$\\dot{x}$ [m/s], $y$ [m]\")\n", + "plt.legend((v_line, y_line), (r\"$\\dot{x}$\", \"$y$\"),\n", + " loc='center right', frameon=False);" + ] + }, + { + "cell_type": "markdown", + "id": "5811a6e4", + "metadata": {}, + "source": [ + "## Things to try\n", + "* Use different reference trajectories (eg, flatness-based trajectory)\n", + "* Try scheduling on the current state rather than the desired state" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6f571b2b", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L3a_linquad.ipynb b/examples/cds112-L3a_linquad.ipynb new file mode 100644 index 000000000..11ac54771 --- /dev/null +++ b/examples/cds112-L3a_linquad.ipynb @@ -0,0 +1,399 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "dd522981", + "metadata": {}, + "source": [ + "# Linear quadratic optimal control example\n", + "\n", + "Richard M. Murray, 20 Jan 2022 (updated 7 Jul 2024)\n", + "\n", + "This example works through the linear quadratic finite time optimal control problem. We assume that we have a linear system of the form\n", + "$$\n", + "\\dot x = A x + Bu \n", + "$$\n", + "and that we want to minimize a cost function of the form\n", + "$$\n", + "\\int_0^T (x^T Q_x x + u^T Q_u u) dt + x^T P_1 x.\n", + "$$\n", + "We show how to compute the solution the Riccati ODE and use this to obtain an optimal (time-varying) linear controller." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "866842ea", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs\n", + "import time" + ] + }, + { + "cell_type": "markdown", + "id": "83a32e85", + "metadata": {}, + "source": [ + "## System dynamics\n", + "\n", + "We use the linearized dynamics of the vehicle steering problem as our linear system. This is mainly for convenient (since we have some intuition about it). " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "48c1bd7f-0db6-4488-af41-41f685280ec9", + "metadata": {}, + "outputs": [], + "source": [ + "# Vehicle dynamics (bicycle model)\n", + "\n", + "# Function to take states, inputs and return the flat flag\n", + "def _kincar_flat_forward(x, u, params={}):\n", + " # Get the parameter values\n", + " b = params.get('wheelbase', 3.)\n", + " #! TODO: add dir processing\n", + "\n", + " # Create a list of arrays to store the flat output and its derivatives\n", + " zflag = [np.zeros(3), np.zeros(3)]\n", + "\n", + " # Flat output is the x, y position of the rear wheels\n", + " zflag[0][0] = x[0]\n", + " zflag[1][0] = x[1]\n", + "\n", + " # First derivatives of the flat output\n", + " zflag[0][1] = u[0] * np.cos(x[2]) # dx/dt\n", + " zflag[1][1] = u[0] * np.sin(x[2]) # dy/dt\n", + "\n", + " # First derivative of the angle\n", + " thdot = (u[0]/b) * np.tan(u[1])\n", + "\n", + " # Second derivatives of the flat output (setting vdot = 0)\n", + " zflag[0][2] = -u[0] * thdot * np.sin(x[2])\n", + " zflag[1][2] = u[0] * thdot * np.cos(x[2])\n", + "\n", + " return zflag\n", + "\n", + "# Function to take the flat flag and return states, inputs\n", + "def _kincar_flat_reverse(zflag, params={}):\n", + " # Get the parameter values\n", + " b = params.get('wheelbase', 3.)\n", + " dir = params.get('dir', 'f')\n", + "\n", + " # Create a vector to store the state and inputs\n", + " x = np.zeros(3)\n", + " u = np.zeros(2)\n", + "\n", + " # Given the flat variables, solve for the state\n", + " x[0] = zflag[0][0] # x position\n", + " x[1] = zflag[1][0] # y position\n", + " if dir == 'f':\n", + " x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot\n", + " elif dir == 'r':\n", + " # Angle is flipped by 180 degrees (since v < 0)\n", + " x[2] = np.arctan2(-zflag[1][1], -zflag[0][1])\n", + " else:\n", + " raise ValueError(\"unknown direction:\", dir)\n", + "\n", + " # And next solve for the inputs\n", + " u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])\n", + " thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])\n", + " u[1] = np.arctan2(thdot_v, u[0]**2 / b)\n", + "\n", + " return x, u\n", + "\n", + "# Function to compute the RHS of the system dynamics\n", + "def _kincar_update(t, x, u, params):\n", + " b = params.get('wheelbase', 3.) # get parameter values\n", + " #! TODO: add dir processing\n", + " dx = np.array([\n", + " np.cos(x[2]) * u[0],\n", + " np.sin(x[2]) * u[0],\n", + " (u[0]/b) * np.tan(u[1])\n", + " ])\n", + " return dx\n", + "\n", + "def _kincar_output(t, x, u, params):\n", + " return x # return x, y, theta (full state)\n", + "\n", + "# Create differentially flat input/output system\n", + "kincar = fs.FlatSystem(\n", + " _kincar_flat_forward, _kincar_flat_reverse, name=\"kincar\",\n", + " updfcn=_kincar_update, outfcn=_kincar_output,\n", + " inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n", + " states=('x', 'y', 'theta'))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "fbdd78c0-30e9-43f7-9e8d-198ae38c2988", + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function to plot lane change manuever\n", + "def plot_lanechange(t, y, u, figure=None, yf=None):\n", + " # Plot the xy trajectory\n", + " plt.subplot(3, 1, 1, label='xy')\n", + " plt.plot(y[0], y[1])\n", + " plt.xlabel(\"x [m]\")\n", + " plt.ylabel(\"y [m]\")\n", + " if yf is not None:\n", + " plt.plot(yf[0], yf[1], 'ro')\n", + "\n", + " # Plot the inputs as a function of time\n", + " plt.subplot(3, 1, 2, label='v')\n", + " plt.plot(t, u[0])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$v$ [m/s]\")\n", + "\n", + " plt.subplot(3, 1, 3, label='delta')\n", + " plt.plot(t, u[1])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$\\\\delta$ [rad]\")\n", + "\n", + " plt.suptitle(\"Lane change manuever\")\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "de9d85f3", + "metadata": {}, + "outputs": [], + "source": [ + "# Initial conditions\n", + "x0 = np.array([-40, -2., 0.])\n", + "u0 = np.array([10, 0]) # only used for linearization\n", + "Tf = 4\n", + "\n", + "# Linearized dynamics\n", + "sys = kincar.linearize(x0, u0)\n", + "print(sys)" + ] + }, + { + "cell_type": "markdown", + "id": "c5c0abe9", + "metadata": {}, + "source": [ + "## Optimal trajectory generation\n", + "\n", + "We generate an trajectory for the system that minimizes the cost function above. Namely, starting from some initial function $x(0) = x_0$, we wish to bring the system toward the origin without using too much control effort." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "02e9f87c", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the cost function and the terminal cost\n", + "# (try changing these later to see what happens)\n", + "Qx = np.diag([1, 1, 1]) # state costs\n", + "Qu = np.diag([1, 1]) # input costs\n", + "Pf = np.diag([1, 1, 1]) # terminal costs" + ] + }, + { + "cell_type": "markdown", + "id": "62c76e5e", + "metadata": {}, + "source": [ + "### Finite time, linear quadratic optimization\n", + "\n", + "The optimal solution satisfies the following equations, which follow from the maximum principle:\n", + "\n", + "$$\n", + " \\begin{aligned}\n", + " \\dot x &= \\left(\\frac{\\partial H}{\\partial \\lambda}\\right)^T\n", + " = A x + Bu, \\qquad & x(0) &= x_0, \\\\\n", + " -\\dot \\lambda &= \\left(\\frac{\\partial H}{\\partial x}\\right)^T\n", + " = Q_x x + A^T \\lambda, \\qquad\n", + " & \\lambda(T) &= P_1 x(T), \\\\\n", + " 0 &= \\left(\\frac{\\partial H}{\\partial u}\\right)^T\n", + " = Q_u u + B^T \\lambda. &&\n", + " \\end{aligned}\n", + "$$\n", + "\n", + "The last condition can be solved to obtain the optimal controller\n", + "\n", + "$$\n", + " u = -Q_u^{-1} B^T \\lambda,\n", + "$$\n", + "\n", + "which can be substituted into the equations for the optimal solution.\n", + "\n", + "Given the linear nature of the dynamics, we attempt to find a solution\n", + "by setting $\\lambda(t) = P(t) x(t)$ where $P(t) \\in {\\mathbb R}^{n \\times\n", + "n}$. Substituting this into the necessary condition, we obtain\n", + "\n", + "$$\n", + " \\begin{aligned}\n", + " & \\dot\\lambda =\n", + " \\dot P x + P \\dot x = \\dot P x + P(Ax - BQ_u^{-1} B^T P) x, \\\\\n", + " & \\quad\\implies\\quad\n", + " -\\dot P x - PA x + PBQ_u^{-1}B P x = Q_xx + A^T P x.\n", + " \\end{aligned}\n", + "$$\n", + "\n", + "This equation is satisfied if we can find $P(t)$ such that\n", + "\n", + "$$\n", + " -\\dot P = PA + A^T P - P B Q_u^{-1} B^T P + Q_x,\n", + " \\qquad P(T) = P_1.\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "id": "b63aed88", + "metadata": {}, + "source": [ + "To solve a final value problem with $P(T) = P_1$, we set the \"initial\" condition to $P_1$ and then invert time, so that we solve\n", + "\n", + "$$\n", + "\\frac{dP}{d(-t)} = -\\frac{dP}{dt} = -F(P), \\qquad P(0) = P_1\n", + "$$\n", + "\n", + "Solving this equation from time $t = 0$ to time $t = T$ will give us an solution that goes from $P(T)$ to $P(0)$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "02d74789", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the Riccatti ODE\n", + "def Pdot_reverse(t, x):\n", + " # Get the P matrix from the state by resizing\n", + " P = np.reshape(x, (sys.nstates, sys.nstates))\n", + " \n", + " # Compute the right hand side of Riccati ODE\n", + " Prhs = P @ sys.A + sys.A.T @ P + Qx - \\\n", + " P @ sys.B @ np.linalg.inv(Qu) @ sys.B.T @ P\n", + " \n", + " # Return P as a vector, *backwards* in time (no minus sign)\n", + " return Prhs.reshape((-1))\n", + "\n", + "# Solve the Riccati ODE (converting from matrix to vector and back)\n", + "P0 = np.reshape(Pf, (-1))\n", + "Psol = sp.integrate.solve_ivp(Pdot_reverse, (0, Tf), P0)\n", + "Pfwd = np.reshape(Psol.y, (sys.nstates, sys.nstates, -1))\n", + "\n", + "# Reorder the solution in time\n", + "Prev = Pfwd[:, :, ::-1] \n", + "trev = Tf - Psol.t[::-1]\n", + "\n", + "print(\"Trange = \", trev[0], \"to\", trev[-1])\n", + "print(\"P[Tf] =\", Prev[:,:,-1])\n", + "print(\"P[0] =\", Prev[:,:,0])\n", + "\n", + "# Internal comparison: show that initial value is close to algebraic solution\n", + "_, P_lqr, _ = ct.lqr(sys.A, sys.B, Qx, Qu)\n", + "print(\"P_lqr =\", P_lqr)" + ] + }, + { + "cell_type": "markdown", + "id": "f4fb1166", + "metadata": {}, + "source": [ + "For solving the $x$ dynamics, we need a function to evaluate $P(t)$ at an arbitrary time (used by the integrator). We can do this with the SciPy `interp1d` function:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "728f675b", + "metadata": {}, + "outputs": [], + "source": [ + "# Define an interpolation function for P\n", + "P = sp.interpolate.interp1d(trev, Prev)\n", + "\n", + "print(\"P(0) =\", P(0))\n", + "print(\"P(3.5) =\", P(3.5))\n", + "print(\"P(4) =\", P(4))" + ] + }, + { + "cell_type": "markdown", + "id": "eb30c3fa", + "metadata": {}, + "source": [ + "We now solve the $\\dot x$ equations *forward* in time, using $P(t)$:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "84092dcd", + "metadata": {}, + "outputs": [], + "source": [ + "# Now solve the state forward in time\n", + "def xdot_forward(t, x):\n", + " u = -np.linalg.inv(Qu) @ sys.B.T @ P(t) @ x\n", + " return sys.A @ x + sys.B @ u\n", + "\n", + "# Now simulate from a shifted initial condition\n", + "xsol = sp.integrate.solve_ivp(xdot_forward, (0, Tf), x0)\n", + "tvec = xsol.t\n", + "x = xsol.y\n", + "print(\"x[0] =\", x[:, 0])\n", + "print(\"x[Tf] =\", x[:, -1])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8488acad", + "metadata": {}, + "outputs": [], + "source": [ + "# Finally compute the \"desired\" state and input values\n", + "xd = x\n", + "ud = np.zeros((sys.ninputs, tvec.size))\n", + "for i, t in enumerate(tvec):\n", + " ud[:, i] = -np.linalg.inv(Qu) @ sys.B.T @ P(t) @ x[:, i]\n", + "\n", + "plot_lanechange(tvec, xd, ud)" + ] + }, + { + "cell_type": "markdown", + "id": "89483f4b", + "metadata": {}, + "source": [ + "Note here that we are stabilizing the system to the origin (compared to some of other examples where we change langes and so the final $y$ position is $y_\\text{f} = 2$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7ed4c5eb", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L3b_optimal.ipynb b/examples/cds112-L3b_optimal.ipynb new file mode 100644 index 000000000..1c7e0e1c2 --- /dev/null +++ b/examples/cds112-L3b_optimal.ipynb @@ -0,0 +1,461 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "edb7e2c6", + "metadata": {}, + "source": [ + "## Optimal Control\n", + "\n", + "Richard M. Murray, 31 Dec 2021 (updated 7 Jul 2024)\n", + "\n", + "This notebook contains an example of using optimal control for a vehicle steering system. It illustrates different methods of setting up optimal control problems and solving them using python-control." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7066eb69", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs\n", + "import time" + ] + }, + { + "cell_type": "markdown", + "id": "4afb09dd", + "metadata": {}, + "source": [ + "## Vehicle steering dynamics\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta, \\qquad |\\delta| \\leq \\delta_\\text{max}\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "The vehicle dynamics are given by a simple bicycle model. We take the state of the system as $(x, y, \\theta)$ where $(x, y)$ is the position of the vehicle in the plane and $\\theta$ is the angle of the vehicle with respect to horizontal. The vehicle input is given by $(v, \\delta)$ where $v$ is the forward velocity of the vehicle and $\\delta$ is the angle of the steering wheel. The model includes saturation of the vehicle steering angle." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a6143a8a", + "metadata": {}, + "outputs": [], + "source": [ + "# Vehicle dynamics (bicycle model)\n", + "\n", + "# Function to take states, inputs and return the flat flag\n", + "def _kincar_flat_forward(x, u, params={}):\n", + " # Get the parameter values\n", + " b = params.get('wheelbase', 3.)\n", + " #! TODO: add dir processing\n", + "\n", + " # Create a list of arrays to store the flat output and its derivatives\n", + " zflag = [np.zeros(3), np.zeros(3)]\n", + "\n", + " # Flat output is the x, y position of the rear wheels\n", + " zflag[0][0] = x[0]\n", + " zflag[1][0] = x[1]\n", + "\n", + " # First derivatives of the flat output\n", + " zflag[0][1] = u[0] * np.cos(x[2]) # dx/dt\n", + " zflag[1][1] = u[0] * np.sin(x[2]) # dy/dt\n", + "\n", + " # First derivative of the angle\n", + " thdot = (u[0]/b) * np.tan(u[1])\n", + "\n", + " # Second derivatives of the flat output (setting vdot = 0)\n", + " zflag[0][2] = -u[0] * thdot * np.sin(x[2])\n", + " zflag[1][2] = u[0] * thdot * np.cos(x[2])\n", + "\n", + " return zflag\n", + "\n", + "# Function to take the flat flag and return states, inputs\n", + "def _kincar_flat_reverse(zflag, params={}):\n", + " # Get the parameter values\n", + " b = params.get('wheelbase', 3.)\n", + " dir = params.get('dir', 'f')\n", + "\n", + " # Create a vector to store the state and inputs\n", + " x = np.zeros(3)\n", + " u = np.zeros(2)\n", + "\n", + " # Given the flat variables, solve for the state\n", + " x[0] = zflag[0][0] # x position\n", + " x[1] = zflag[1][0] # y position\n", + " if dir == 'f':\n", + " x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot\n", + " elif dir == 'r':\n", + " # Angle is flipped by 180 degrees (since v < 0)\n", + " x[2] = np.arctan2(-zflag[1][1], -zflag[0][1])\n", + " else:\n", + " raise ValueError(\"unknown direction:\", dir)\n", + "\n", + " # And next solve for the inputs\n", + " u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])\n", + " thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])\n", + " u[1] = np.arctan2(thdot_v, u[0]**2 / b)\n", + "\n", + " return x, u\n", + "\n", + "# Function to compute the RHS of the system dynamics\n", + "def _kincar_update(t, x, u, params):\n", + " b = params.get('wheelbase', 3.) # get parameter values\n", + " #! TODO: add dir processing\n", + " dx = np.array([\n", + " np.cos(x[2]) * u[0],\n", + " np.sin(x[2]) * u[0],\n", + " (u[0]/b) * np.tan(u[1])\n", + " ])\n", + " return dx\n", + "\n", + "def _kincar_output(t, x, u, params):\n", + " return x # return x, y, theta (full state)\n", + "\n", + "# Create differentially flat input/output system\n", + "kincar = fs.FlatSystem(\n", + " _kincar_flat_forward, _kincar_flat_reverse, name=\"kincar\",\n", + " updfcn=_kincar_update, outfcn=_kincar_output,\n", + " inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n", + " states=('x', 'y', 'theta'))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "43377b51-35db-4e8f-9101-b22af1de1cb2", + "metadata": {}, + "outputs": [], + "source": [ + "# Utility function to plot lane change manuever\n", + "def plot_lanechange(t, y, u, figure=None, yf=None):\n", + " # Plot the xy trajectory\n", + " plt.subplot(3, 1, 1, label='xy')\n", + " plt.plot(y[0], y[1])\n", + " plt.xlabel(\"x [m]\")\n", + " plt.ylabel(\"y [m]\")\n", + " if yf is not None:\n", + " plt.plot(yf[0], yf[1], 'ro')\n", + "\n", + " # Plot the inputs as a function of time\n", + " plt.subplot(3, 1, 2, label='v')\n", + " plt.plot(t, u[0])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$v$ [m/s]\")\n", + "\n", + " plt.subplot(3, 1, 3, label='delta')\n", + " plt.plot(t, u[1])\n", + " plt.xlabel(\"Time $t$ [sec]\")\n", + " plt.ylabel(\"$\\\\delta$ [rad]\")\n", + "\n", + " plt.suptitle(\"Lane change manuever\")\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "64bd3c3b", + "metadata": {}, + "source": [ + "## Optimal trajectory generation\n", + "\n", + "We consider the problem of changing from one lane to another over a perod of 10 seconds while driving at a forward speed of 10 m/s." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "42dcbd79", + "metadata": {}, + "outputs": [], + "source": [ + "# Initial and final conditions\n", + "x0 = np.array([ 0., -2., 0.]); u0 = np.array([10., 0.])\n", + "xf = np.array([100., 2., 0.]); uf = np.array([10., 0.])\n", + "Tf = 10" + ] + }, + { + "cell_type": "markdown", + "id": "5ff2e044", + "metadata": {}, + "source": [ + "An important part of the optimization procedure is to give a good initial guess. Here are some possibilities:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "650d321a", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the time horizon (and spacing) for the optimization\n", + "# timepts = np.linspace(0, Tf, 5, endpoint=True)\n", + "# timepts = np.linspace(0, Tf, 10, endpoint=True)\n", + "timepts = np.linspace(0, Tf, 20, endpoint=True)\n", + "\n", + "# Compute some initial guesses to use\n", + "bend_left = [10, 0.01] # slight left veer (will extend over all timepts)\n", + "straight_line = ( # straight line from start to end with nominal input\n", + " np.array([x0 + (xf - x0) * t/Tf for t in timepts]).transpose(), \n", + " u0\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "4e75a2c4", + "metadata": {}, + "source": [ + "### Approach 1: standard quadratic cost\n", + "\n", + "We can set up the optimal control problem as trying to minimize the distance form the desired final point while at the same time as not exerting too much control effort to achieve our goal.\n", + "\n", + "(The optimization module solves optimal control problems by choosing the values of the input at each point in the time horizon to try to minimize the cost. This means that each input generates a parameter value at each point in the time horizon, so the more refined your time horizon, the more parameters the optimizer has to search over.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "984c2f0b", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the cost functions\n", + "Qx = np.diag([.1, 10, .1]) # keep lateral error low\n", + "Qu = np.diag([.1, 1]) # minimize applied inputs\n", + "quad_cost = opt.quadratic_cost(kincar, Qx, Qu, x0=xf, u0=uf)\n", + "\n", + "# Compute the optimal control, setting step size for gradient calculation (eps)\n", + "start_time = time.process_time()\n", + "result1 = opt.solve_ocp(\n", + " kincar, timepts, x0, quad_cost, \n", + " initial_guess=straight_line,\n", + " # initial_guess= bend_left,\n", + " # initial_guess=u0,\n", + " # minimize_method='trust-constr',\n", + " # minimize_options={'finite_diff_rel_step': 0.01},\n", + " # trajectory_method='shooting'\n", + " # solve_ivp_method='LSODA'\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result1.states, result1.inputs, xf)\n", + "print(\"Final computed state: \", result1.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t1, u1 = result1.time, result1.inputs\n", + "t1, y1 = ct.input_output_response(kincar, timepts, u1, x0)\n", + "plot_lanechange(t1, y1, u1, yf=xf[0:2])\n", + "print(\"Final simulated state:\", y1[:,-1])" + ] + }, + { + "cell_type": "markdown", + "id": "b7cade52", + "metadata": {}, + "source": [ + "Note the amount of time required to solve the problem and also any warning messages about to being able to solve the optimization (mainly in earlier versions of python-control). You can try to adjust a number of factors to try to get a better solution:\n", + "* Try changing the number of points in the time horizon\n", + "* Try using a different initial guess\n", + "* Try changing the optimization method (see commented out code)" + ] + }, + { + "cell_type": "markdown", + "id": "6a9f9d9b", + "metadata": {}, + "source": [ + "### Approach 2: input cost, input constraints, terminal cost\n", + "\n", + "The previous solution integrates the position error for the entire horizon, and so the car changes lanes very quickly (at the cost of larger inputs). Instead, we can penalize the final state and impose a higher cost on the inputs, resuling in a more gradual lane change.\n", + "\n", + "We can also try using a different solver for this example. You can pass the solver using the `minimize_method` keyword and send options to the solver using the `minimize_options` keyword (which should be set to a dictionary of options)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a201e33c", + "metadata": {}, + "outputs": [], + "source": [ + "# Add input constraint, input cost, terminal cost\n", + "constraints = [ opt.input_range_constraint(kincar, [8, -0.1], [12, 0.1]) ]\n", + "traj_cost = opt.quadratic_cost(kincar, None, np.diag([0.1, 1]), u0=uf)\n", + "term_cost = opt.quadratic_cost(kincar, np.diag([1, 10, 100]), None, x0=xf)\n", + "\n", + "# Compute the optimal control\n", + "start_time = time.process_time()\n", + "result2 = opt.solve_ocp(\n", + " kincar, timepts, x0, traj_cost, constraints, terminal_cost=term_cost,\n", + " initial_guess=straight_line, \n", + " # minimize_method='trust-constr',\n", + " # minimize_options={'finite_diff_rel_step': 0.01},\n", + " # minimize_method='SLSQP', minimize_options={'eps': 0.01},\n", + " # log=True,\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result2.states, result2.inputs, xf)\n", + "print(\"Final computed state: \", result2.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t2, u2 = result2.time, result2.inputs\n", + "t2, y2 = ct.input_output_response(kincar, timepts, u2, x0)\n", + "plot_lanechange(t2, y2, u2, yf=xf[0:2])\n", + "print(\"Final simulated state:\", y2[:,-1])" + ] + }, + { + "cell_type": "markdown", + "id": "3d2ccf97", + "metadata": {}, + "source": [ + "### Approach 3: terminal constraints\n", + "\n", + "We can also remove the cost function on the state and replace it with a terminal *constraint* on the state. If a solution is found, it guarantees we get to exactly the final state. Note however, that terminal constraints can be very difficult to satisfy for a general optimization (compare the solution times here with what we saw last week when we used differential flatness)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "dc77a856", + "metadata": {}, + "outputs": [], + "source": [ + "# Input cost and terminal constraints\n", + "R = np.diag([1, 1]) # minimize applied inputs\n", + "cost3 = opt.quadratic_cost(kincar, np.zeros((3,3)), R, u0=uf)\n", + "constraints = [\n", + " opt.input_range_constraint(kincar, [8, -0.1], [12, 0.1]) ]\n", + "terminal = [ opt.state_range_constraint(kincar, xf, xf) ]\n", + "\n", + "# Compute the optimal control\n", + "start_time = time.process_time()\n", + "result3 = opt.solve_ocp(\n", + " kincar, timepts, x0, cost3, constraints,\n", + " terminal_constraints=terminal, initial_guess=straight_line,\n", + "# solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},\n", + "# minimize_method='trust-constr',\n", + "# minimize_options={'finite_diff_rel_step': 0.01},\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result3.states, result3.inputs, xf)\n", + "print(\"Final computed state: \", result3.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t3, u3 = result3.time, result3.inputs\n", + "t3, y3 = ct.input_output_response(kincar, timepts, u3, x0)\n", + "plot_lanechange(t3, y3, u3, yf=xf[0:2])\n", + "print(\"Final state: \", y3[:,-1])" + ] + }, + { + "cell_type": "markdown", + "id": "9e744463", + "metadata": {}, + "source": [ + "### Approach 4: terminal constraints w/ basis functions\n", + "\n", + "As a final example, we can use a basis function to reduce the size\n", + "of the problem and get faster answers with more temporal resolution.\n", + "\n", + "Here we parameterize the input by a set of 4 Bezier curves but solve for a much more time resolved set of inputs. Note that while we are using the `control.flatsys` module to define the basis functions, we are not exploiting the fact that the system is differentially flat." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ee82aa25", + "metadata": {}, + "outputs": [], + "source": [ + "# Get basis functions for flat systems module\n", + "import control.flatsys as flat\n", + "\n", + "# Compute the optimal control\n", + "start_time = time.process_time()\n", + "result4 = opt.solve_ocp(\n", + " kincar, timepts, x0, quad_cost, constraints,\n", + " terminal_constraints=terminal,\n", + " initial_guess=straight_line,\n", + " basis=flat.PolyFamily(4, T=Tf),\n", + " # solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},\n", + " # solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},\n", + " # minimize_method='trust-constr', minimize_options={'disp': True},\n", + " log=False\n", + ")\n", + "print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n", + "\n", + "# Plot the results from the optimization\n", + "plot_lanechange(timepts, result4.states, result4.inputs, xf)\n", + "print(\"Final computed state: \", result3.states[:,-1])\n", + "\n", + "# Simulate the system and see what happens\n", + "t4, u4 = result4.time, result4.inputs\n", + "t4, y4 = ct.input_output_response(kincar, timepts, u4, x0)\n", + "plot_lanechange(t4, y4, u4, yf=xf[0:2])\n", + "plt.legend(['optimal', 'simulation'])\n", + "print(\"Final simulated state: \", y4[:,-1])" + ] + }, + { + "cell_type": "markdown", + "id": "2a74388e", + "metadata": {}, + "source": [ + "Note how much smoother the inputs look, although the solver can still have a hard time satisfying the final constraints, resulting in longer computation times." + ] + }, + { + "cell_type": "markdown", + "id": "1465d149", + "metadata": {}, + "source": [ + "### Additional things to try\n", + "\n", + "* Compare the results here with what we go last week exploiting the property of differential flatness (computation time, in particular)\n", + "* Try using different weights, solvers, initial guess and other properties and see how things change.\n", + "* Try using different values for `initial_guess` to get faster convergence and/or different classes of solutions." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "02bad3d5", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L4a_lqr-tracking.ipynb b/examples/cds112-L4a_lqr-tracking.ipynb new file mode 100644 index 000000000..0687f4cc5 --- /dev/null +++ b/examples/cds112-L4a_lqr-tracking.ipynb @@ -0,0 +1,279 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "af1717f2", + "metadata": {}, + "source": [ + "# LQR Tracking Example\n", + "\n", + "Richard M. Murray, 25 Jan 2022\n", + "\n", + "This example uses a linear system to show how to implement LQR based tracking and some of the tradeoffs between feedfoward and feedback. Integral action is also implemented." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "50d5c4d3", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import control as ct" + ] + }, + { + "cell_type": "markdown", + "id": "a23d6f89", + "metadata": {}, + "source": [ + "## System definition\n", + "\n", + "We use a simple linear system to illustrate the concepts. This system corresponds to the linearized lateral dynamics of a vehicle driving down a road at 10 m/s." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b5923c88", + "metadata": {}, + "outputs": [], + "source": [ + "# Define a simple linear system that we want to control\n", + "sys = ct.ss([[0, 10], [-1, 0]], [[0], [1]], np.eye(2), 0, name='sys')\n", + "print(sys)" + ] + }, + { + "cell_type": "markdown", + "id": "dba5ea2b", + "metadata": {}, + "source": [ + "## Controller design\n", + "\n", + "We start by defining the equilibrium point that we plan to stabilize." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "874c1479", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the desired equilibrium point for the system\n", + "x0 = np.array([2, 0])\n", + "u0 = np.array([2])\n", + "Tf = 4" + ] + }, + { + "cell_type": "markdown", + "id": "99f036ea", + "metadata": {}, + "source": [ + "Then construct a simple LQR controller (gain matrix) and create the controller + closed loop system models:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3ce6a230", + "metadata": {}, + "outputs": [], + "source": [ + "# Construct an LQR controller for the system\n", + "K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs))\n", + "ctrl, clsys = ct.create_statefbk_iosystem(sys, K)\n", + "print(ctrl)\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "id": "5c711b56", + "metadata": {}, + "source": [ + "Note that the name of the second system is `u[0]`. This is a bug in control-0.9.3 that will be fixed in a [future release](https://github.com/python-control/python-control/pull/849)." + ] + }, + { + "cell_type": "markdown", + "id": "84422c3f", + "metadata": {}, + "source": [ + "## System simulations\n", + "\n", + "### Baseline controller\n", + "\n", + "To see how the baseline controller performs, we ask it to track a step change in (xd, ud):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b763b91b", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the step response with respect to the reference input\n", + "tvec = np.linspace(0, Tf, 100)\n", + "xd = x0\n", + "ud = u0\n", + "\n", + "# U = np.hstack([xd, ud])\n", + "U = np.outer(np.hstack([xd, ud]), np.ones_like(tvec))\n", + "time, output = ct.input_output_response(clsys, tvec, U)\n", + "plt.plot(time, output[0], time, output[1])\n", + "plt.plot([time[0], time[-1]], [xd[0], xd[0]], '--');\n", + "plt.legend(['x[0]', 'x[1]']);" + ] + }, + { + "cell_type": "markdown", + "id": "84ee7635", + "metadata": {}, + "source": [ + "### Disturbance rejection\n", + "\n", + "We add a disturbance to the system by modifying ud (since this enters directly at the system input u)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1ecbb3a0", + "metadata": {}, + "outputs": [], + "source": [ + "# Resimulate with a disturbance input\n", + "delta = 0.5\n", + "U = np.outer(np.hstack([xd, ud + delta]), np.ones_like(tvec))\n", + "time, output = ct.input_output_response(clsys, tvec, U)\n", + "plt.plot(time, output[0], time, output[1])\n", + "plt.plot([time[0], time[-1]], [xd[0], xd[0]], '--')\n", + "plt.legend(['x[0]', 'x[1]']);" + ] + }, + { + "cell_type": "markdown", + "id": "ea2d1c59", + "metadata": {}, + "source": [ + "We see that this leads to steady state error, since some amount of system error is required to generate the force to offset the disturbance." + ] + }, + { + "cell_type": "markdown", + "id": "84a9e61c", + "metadata": {}, + "source": [ + "### Integral feedback\n", + "\n", + "A standard approach to compensate for constant disturbances is to use integral feedback. To do this, we have to decide what output we want to track and create a new controller with integral feedback.\n", + "\n", + "We do this by creating an \"augmented\" system that includes the dynamics of the process along with the dynamics of the controller (= integrators for the errors that we choose):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ee2ecc51", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a controller with integral feedback\n", + "C = np.array([[1, 0]])\n", + "\n", + "# Define an augmented state space for use with LQR\n", + "A_aug = np.block([\n", + " [sys.A, np.zeros((sys.nstates, 1))], \n", + " [C, 0]\n", + "])\n", + "B_aug = np.vstack([sys.B, 0])\n", + "print(\"A =\", A_aug, \"\\nB =\", B_aug)" + ] + }, + { + "cell_type": "markdown", + "id": "463d9b85", + "metadata": {}, + "source": [ + "Now generate an LQR controller for the augmented system:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3dd3479f", + "metadata": {}, + "outputs": [], + "source": [ + "# Create an LQR controller for the augmented system\n", + "K_aug, _, _ = ct.lqr(\n", + " A_aug, B_aug, np.diag([1, 1, 1]), np.eye(sys.ninputs))\n", + "print(K_aug)" + ] + }, + { + "cell_type": "markdown", + "id": "19bb6592", + "metadata": {}, + "source": [ + "We can think about this gain as `K_aug = [K, ki]` and the resulting contoller becomes\n", + "\n", + "$$\n", + "u = u_\\text{d} - K(x - x_\\text{d}) - k_\\text{i} \\int_0^t (y - y_\\text{d})\\, d\\tau.\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e183a822", + "metadata": {}, + "outputs": [], + "source": [ + "# Construct an LQR controller for the system\n", + "integral_ctrl, sys_integral = ct.create_statefbk_iosystem(sys, K_aug, integral_action=C)\n", + "print(integral_ctrl)\n", + "print(sys_integral)\n", + "\n", + "# Resimulate with a disturbance input\n", + "delta = 0.5\n", + "U = np.outer(np.hstack([xd, ud + delta]), np.ones_like(tvec))\n", + "time, output = ct.input_output_response(sys_integral, tvec, U)\n", + "plt.plot(time, output[0], time, output[1])\n", + "plt.plot([time[0], time[-1]], [xd[0], xd[0]], '--')\n", + "plt.legend(['x[0]', 'x[1]']);" + ] + }, + { + "cell_type": "markdown", + "id": "437487da", + "metadata": {}, + "source": [ + "## Things to try\n", + "* Play around with the gains and see whether you can reduce the overshoot (50%!)\n", + "* Try following more complicated trajectories (hint: linear systems are differentially flat...)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "99394ace", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L4b_pvtol-lqr.ipynb b/examples/cds112-L4b_pvtol-lqr.ipynb new file mode 100644 index 000000000..b472429e2 --- /dev/null +++ b/examples/cds112-L4b_pvtol-lqr.ipynb @@ -0,0 +1,355 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "f8bfc15c", + "metadata": {}, + "source": [ + "# PVTOL Linear Quadratic Regulator Example\n", + "\n", + "Richard M. Murray, 25 Jan 2022\n", + "\n", + "This notebook contains an example of LQR control applied to the PVTOL system. It demonstrates how to construct an LQR controller and also the importance of the feedforward component of the controller. A gain scheduled design is also demonstrated." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c120d65c", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import control as ct" + ] + }, + { + "cell_type": "markdown", + "id": "77e2ed47", + "metadata": {}, + "source": [ + "## System description\n", + "\n", + "We use the PVTOL dynamics from the textbook, which are contained in the `pvtol` module. The vehicle model is both an I/O system model and a flat system model (for the case when the viscous damping coefficient $c$ is zero).\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x, \\\\\n", + " m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - m g - c \\dot y, \\\\\n", + " J \\ddot \\theta &= r F_1.\n", + "\\end{aligned}\n", + "$$\n", + "
" + ] + }, + { + "cell_type": "markdown", + "id": "0a12fc3d", + "metadata": {}, + "source": [ + "The parameter values for the PVTOL system come from the Caltech ducted fan experiment, shown in the video below (the wing forces are not included in the PVTOL model):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7adc6cf1", + "metadata": {}, + "outputs": [], + "source": [ + "from IPython.display import YouTubeVideo\n", + "display(YouTubeVideo('ZFb5kFpgCm4', width=640, height=480))\n", + "\n", + "from pvtol import pvtol, plot_results\n", + "print(pvtol)" + ] + }, + { + "cell_type": "markdown", + "id": "45259984", + "metadata": {}, + "source": [ + "Since we will be creating a linear controller, we need a linear system model. We obtain that model by linearizing the dynamics around an equilibrium point. This can be done in python-control using the `find_eqpt` function. We fix the output of the system to be zero and find the state and inputs that hold us there." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ea50d7cd", + "metadata": {}, + "outputs": [], + "source": [ + "# Find the equilibrium point corresponding to hover\n", + "xeq, ueq = ct.find_eqpt(pvtol, np.zeros(6), np.zeros(2), y0=np.zeros(6), iy=[0, 1])\n", + "\n", + "print(\"xeq = \", xeq)\n", + "print(\"ueq = \", ueq)\n", + "\n", + "# Get the linearized dynamics\n", + "linsys = pvtol.linearize(xeq, ueq)\n", + "print(linsys)" + ] + }, + { + "cell_type": "markdown", + "id": "7cb8840b", + "metadata": {}, + "source": [ + "## Linear quadratic regulator (LQR) design\n", + "\n", + "Now that we have a linearized model of the system, we can compute a controller using linear quadratic regulator theory. We seek to find the control law that minimizes the function\n", + "\n", + "$$\n", + "J(x(\\cdot), u(\\cdot)) = \\int_0^\\infty x^T(\\tau) Q_x x(\\tau) + u^T(\\tau) Q_u u(\\tau)\\, d\\tau\n", + "$$\n", + "\n", + "The weighting matrices $Q_x \\in \\mathbb{R}^{n \\times n}$ and $Q_u \\in \\mathbb{R}^{m \\times m}$ should be chosen based on the desired performance of the system (tradeoffs in state errors and input magnitudes). See Example 3.5 in OBC for a discussion of how to choose these weights. For now, we just choose identity weights for all states and inputs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5cfa1ba7", + "metadata": {}, + "outputs": [], + "source": [ + "# Start with a diagonal weighting\n", + "Qx1 = np.diag([1, 1, 1, 1, 1, 1])\n", + "Qu1 = np.diag([1, 1])\n", + "K, X, E = ct.lqr(linsys, Qx1, Qu1)" + ] + }, + { + "cell_type": "markdown", + "id": "863d07de", + "metadata": {}, + "source": [ + "To create a controller for the system, we need to create an I/O system that takes in the desired trajectory $(x_\\text{d}, u_\\text{d})$ and the current state $x$ and generates the control law\n", + "\n", + "$$\n", + "u = u_\\text{d} - K (x - x_\\text{d})\n", + "$$\n", + "\n", + "The function `create_statefbk_iosystem()` does this (see [documentation](https://python-control.readthedocs.io/en/0.9.3.post2/generated/control.create_statefbk_iosystem.html) for details)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5db704e6", + "metadata": {}, + "outputs": [], + "source": [ + "control, pvtol_closed = ct.create_statefbk_iosystem(pvtol, K)\n", + "print(control, \"\\n\")\n", + "print(pvtol_closed)" + ] + }, + { + "cell_type": "markdown", + "id": "bedcb0c0", + "metadata": {}, + "source": [ + "## Closed loop system simulation\n", + "\n", + "We now generate a trajectory for the system and track that trajectory.\n", + "\n", + "For this simple example, we take the system input to be a \"step\" input that moves the system 1 meter to the right. More complex trajectories (eg, using the results from HW #3) could also be used." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a497aa2c", + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a step response by setting xd, ud\n", + "Tf = 15\n", + "T = np.linspace(0, Tf, 100)\n", + "xd = np.outer(np.array([1, 0, 0, 0, 0, 0]), np.ones_like(T))\n", + "ud = np.outer(ueq, np.ones_like(T))\n", + "ref = np.vstack([xd, ud])\n", + "\n", + "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", + "plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "id": "f014e660", + "metadata": {}, + "source": [ + "The limitations of the linear controlller can be seen if we take a larger step, say 10 meters." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a141f100", + "metadata": {}, + "outputs": [], + "source": [ + "xd = np.outer(np.array([10, 0, 0, 0, 0, 0]), np.ones_like(T))\n", + "ref = np.vstack([xd, ud])\n", + "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n", + "plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "id": "8adb6ff4", + "metadata": {}, + "source": [ + "We see that the large initial error causes the vehicle to rotate to a very high role angle (almost 1 radian $\\approx 60^\\circ$), at which point the linear model is not very accurate and the controller errors in the $y$ direction get very large.\n", + "\n", + "One way to fix this problem is to change the gains on the controller so that we penalize the $y$ error more and try to keep that error from building up. However, given the fact that we are trying to stabilize a point that is fairly far from our initial condition, it can be difficult to manage the tradesoffs to get good performance.\n", + "\n", + "An alterntaive approach is is to stabilize the system around a trajectory that moves from the initial to final condition. As a very simple approach, we start by using a _nonfeasible_ trajectory that goes from 0 to 10 in 10 seconds." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a075a0a7", + "metadata": {}, + "outputs": [], + "source": [ + "timepts = np.linspace(0, 15, 100)\n", + "xf = np.array([10, 0, 0, 0, 0, 0])\n", + "xd = np.array([xf/10 * t if t < 10 else xf for t in timepts]).T\n", + "ud = np.outer(ueq, np.ones_like(timepts))\n", + "ref = np.vstack([xd, ud])\n", + "response = ct.input_output_response(pvtol_closed, timepts, ref, xeq)\n", + "plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "id": "73d74c23", + "metadata": {}, + "source": [ + "Note that even though the trajectory was not feasible (it asked the system to move sideways while remaining pointed in the vertical ($\\theta = 0$) direction, the controller has very good performance." + ] + }, + { + "cell_type": "markdown", + "id": "b7539806", + "metadata": {}, + "source": [ + "## Gain scheduled controller design" + ] + }, + { + "cell_type": "markdown", + "id": "23d7e21c", + "metadata": {}, + "source": [ + "Another challenge in using linearized models is that they are only accurate near the point in which they were computed. For the PVTOL system, this can be a problem if the roll angle $\\theta$ gets large, since in this case the linearization changes significantly (the forces $F_1$ and $F_2$ are no longer aligned with the horizontal and vertical axes).\n", + "\n", + "One approach to solving this problem is to compute different gains at different points in the operating envelope of the system. The code below illustrates the use of gain scheduling by modifying the system drag to a very high value (so that the vehicle must roll to a large angle in order to move sideways against the high drag) and then demonstrates the difficulty in obtaining good performance while trying to track the (still infeasible) trajectory." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4590b138", + "metadata": {}, + "outputs": [], + "source": [ + "# Increase the viscous drag to force larger angles\n", + "linsys = pvtol.linearize(xeq, ueq, params={'c': 20})\n", + "\n", + "# Change to physically motivated gains\n", + "Qx3 = np.diag([10, 100, (180/np.pi) / 5, 0, 0, 0])\n", + "Qu3 = np.diag([10, 1])\n", + "\n", + "# Compute a single gain around hover\n", + "K, X, E = ct.lqr(linsys, Qx3, Qu3)\n", + "control, pvtol_closed = ct.create_statefbk_iosystem(pvtol, K)\n", + "\n", + "# Simulate the response trying to track horizontal trajectory\n", + "response = ct.input_output_response(pvtol_closed, T, ref, xeq, params={'c': 20})\n", + "plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "id": "9e01104a", + "metadata": {}, + "source": [ + "Note that the angle $\\theta$ is quite large (-0.5 rad) during the initla portion of the trajectory, and at this angle (~30$^\\circ$) it is difficult to maintain our altitude while moving sideways. This happens in large part becuase the system model that we used was linearized about the $\\theta = 0$ configuration.\n", + "\n", + "This problem can be addressed by designing a gain scheduled controller in which we compute different system gains at different roll angles. We carry out those computations below, using the `create_statefbk_iosystem` function, but now passing a set of gains and points instead of just a single gain.\n", + "\n", + "(Note: there is a bug in control-0.9.3 that requires gain scheduling to be done on two or more variables, so we also schedule on the horizontal velocity $\\dot x$, even though that doesn't matter that much here.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e427459f", + "metadata": {}, + "outputs": [], + "source": [ + "import itertools\n", + "import math\n", + "\n", + "# Set up points around which to linearize (control-0.9.3: must be 2D or greater)\n", + "angles = np.linspace(-math.pi/3, math.pi/3, 10)\n", + "speeds = np.linspace(-10, 10, 3)\n", + "points = list(itertools.product(angles, speeds))\n", + "\n", + "# Compute the gains at each design point\n", + "gains = []\n", + "for point in points:\n", + " # Compute the state that we want to linearize about\n", + " xgs = xeq.copy()\n", + " xgs[2], xgs[3] = point[0], point[1]\n", + " \n", + " # Linearize the system and compute the LQR gains\n", + " linsys = pvtol.linearize(xgs, ueq, params={'c': 20})\n", + " K, X, E = ct.lqr(linsys, Qx3, Qu3)\n", + " gains.append(K)\n", + " \n", + "# Create a gain scheduled controller off of the current state\n", + "control, pvtol_closed = ct.create_statefbk_iosystem(\n", + " pvtol, (gains, points), gainsched_indices=['x2', 'x3'])\n", + "\n", + "# Simulate the response\n", + "response = ct.input_output_response(pvtol_closed, T, ref, xeq, params={'c': 20})\n", + "plot_results(response.time, response.states, response.outputs[6:])" + ] + }, + { + "cell_type": "markdown", + "id": "7399db70", + "metadata": {}, + "source": [ + "We see that the response is much better, with about 10X less error in the $y$ coordinate." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c8021347", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L5_rhc-doubleint.ipynb b/examples/cds112-L5_rhc-doubleint.ipynb new file mode 100644 index 000000000..2a311fc46 --- /dev/null +++ b/examples/cds112-L5_rhc-doubleint.ipynb @@ -0,0 +1,616 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "9d41c333", + "metadata": {}, + "source": [ + "# RHC Example: Double integrator with bounded input\n", + "\n", + "Richard M. Murray, 3 Feb 2022 (updated 29 Jan 2023)\n", + "\n", + "To illustrate the implementation of a receding horizon controller, we\n", + "consider a linear system corresponding to a double integrator with\n", + "bounded input:\n", + "\n", + "$$\n", + " \\dot x = \\begin{bmatrix} 0 & 1 \\\\ 0 & 0 \\end{bmatrix} x + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} \\text{clip}(u)\n", + " \\qquad\\text{where}\\qquad\n", + " \\text{clip}(u) = \\begin{cases}\n", + " -1 & u < -1, \\\\\n", + " u & -1 \\leq u \\leq 1, \\\\\n", + " 1 & u > 1.\n", + " \\end{cases}\n", + "$$\n", + "\n", + "We implement a model predictive controller by choosing\n", + "\n", + "$$\n", + " Q_x = \\begin{bmatrix} 1 & 0 \\\\ 0 & 0 \\end{bmatrix}, \\qquad\n", + " Q_u = \\begin{bmatrix} 1 \\end{bmatrix}, \\qquad\n", + " P_1 = \\begin{bmatrix} 0.1 & 0 \\\\ 0 & 0.1 \\end{bmatrix}.\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4fe0af7f", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs\n", + "import time" + ] + }, + { + "cell_type": "markdown", + "id": "4c695f81", + "metadata": {}, + "source": [ + "## System definition\n", + "\n", + "The system is defined as a double integrator with bounded input." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5c01f571", + "metadata": {}, + "outputs": [], + "source": [ + "def doubleint_update(t, x, u, params):\n", + " # Get the parameters\n", + " lb = params.get('lb', -1)\n", + " ub = params.get('ub', 1)\n", + " assert lb < ub\n", + "\n", + " # bound the input\n", + " u_clip = np.clip(u, lb, ub)\n", + "\n", + " return np.array([x[1], u_clip[0]])\n", + "\n", + "proc = ct.NonlinearIOSystem(\n", + " doubleint_update, None, name=\"double integrator\",\n", + " inputs = ['u'], outputs=['x[0]', 'x[1]'], states=2)" + ] + }, + { + "cell_type": "markdown", + "id": "6c2f0d00", + "metadata": {}, + "source": [ + "## Receding horizon controller\n", + "\n", + "To define a receding horizon controller, we create an optimal control problem (using the `OptimalControlProblem` class) and then use the `compute_trajectory` method to solve for the trajectory from the current state.\n", + "\n", + "We start by defining the cost functions, which consists of a trajectory cost and a terminal cost:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a501efef", + "metadata": {}, + "outputs": [], + "source": [ + "Qx = np.diag([1, 0]) # state cost\n", + "Qu = np.diag([1]) # input cost\n", + "traj_cost=opt.quadratic_cost(proc, Qx, Qu)\n", + "\n", + "P1 = np.diag([0.1, 0.1]) # terminal cost\n", + "term_cost = opt.quadratic_cost(proc, P1, None)" + ] + }, + { + "cell_type": "markdown", + "id": "c5470629", + "metadata": {}, + "source": [ + "We also set up a set of constraints the correspond to the fact that the input should have magnitude 1. This can be done using either the [`input_range_constraint`](https://python-control.readthedocs.io/en/0.9.3.post2/generated/control.optimal.input_range_constraint.html) function or the [`input_poly_constraint`](https://python-control.readthedocs.io/en/0.9.3.post2/generated/control.optimal.input_poly_constraint.html) function." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cb4c511a", + "metadata": {}, + "outputs": [], + "source": [ + "traj_constraints = opt.input_range_constraint(proc, -1, 1)\n", + "# traj_constraints = opt.input_poly_constraint(\n", + "# proc, np.array([[1], [-1]]), np.array([1, 1]))" + ] + }, + { + "cell_type": "markdown", + "id": "a5568374", + "metadata": {}, + "source": [ + "We define the horizon for evaluating finite-time, optimal control by setting up a set of time points across the designed horizon. The input will be computed at each time point." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9edec673", + "metadata": {}, + "outputs": [], + "source": [ + "Th = 5\n", + "timepts = np.linspace(0, Th, 11, endpoint=True)\n", + "print(timepts)" + ] + }, + { + "cell_type": "markdown", + "id": "cb8fcecc", + "metadata": {}, + "source": [ + "Finally, we define the optimal control problem that we want to solve (without actually solving it)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e9f31be6", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up the optimal control problem\n", + "ocp = opt.OptimalControlProblem(\n", + " proc, timepts, traj_cost,\n", + " terminal_cost=term_cost,\n", + " trajectory_constraints=traj_constraints,\n", + " # terminal_constraints=term_constraints,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "ee9a39dd", + "metadata": {}, + "source": [ + "To make sure that the problem is properly defined, we solve the problem for a specific initial condition. We also compare the amount of time required to solve the problem from a \"cold start\" (no initial guess) versus a \"warm start\" (use the previous solution, shifted forward on point in time)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "887295eb", + "metadata": {}, + "outputs": [], + "source": [ + "X0 = np.array([1, 1])\n", + "\n", + "start_time = time.process_time()\n", + "res = ocp.compute_trajectory(X0, initial_guess=0, return_states=True)\n", + "stop_time = time.process_time()\n", + "print(f'* Cold start: {stop_time-start_time:.3} sec')\n", + "\n", + "# Resolve using previous solution (shifted forward) as initial guess to compare timing\n", + "start_time = time.process_time()\n", + "u = res.inputs\n", + "u_shift = np.hstack([u[:, 1:], u[:, -1:]])\n", + "ocp.compute_trajectory(X0, initial_guess=u_shift, print_summary=False)\n", + "stop_time = time.process_time()\n", + "print(f'* Warm start: {stop_time-start_time:.3} sec')" + ] + }, + { + "cell_type": "markdown", + "id": "115dec26", + "metadata": {}, + "source": [ + "(In this case the timing is not that different since the system is very simple.)\n", + "\n", + "Plotting the result, we see that the solution is properly computed." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4b98e773", + "metadata": {}, + "outputs": [], + "source": [ + "plt.plot(res.time, res.states[0], 'k-', label='$x_1$')\n", + "plt.plot(res.time, res.inputs[0], 'b-', label='u')\n", + "plt.xlabel('Time [s]')\n", + "plt.ylabel('$x_1$, $u$')\n", + "plt.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "0e85981a", + "metadata": {}, + "source": [ + "We implement the receding horicon controller using a function that we can with different versions of the problem." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "eb2e8126", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a figure to use for plotting\n", + "def run_rhc_and_plot(\n", + " proc, ocp, X0, Tf, print_summary=False, verbose=False, ax=None, plot=True): \n", + " # Start at the initial point\n", + " x = X0\n", + " \n", + " # Initialize the axes\n", + " if plot and ax is None:\n", + " ax = plt.axes()\n", + " \n", + " # Initialize arrays to store the final trajectory\n", + " time_, inputs_, outputs_, states_ = [], [], [], []\n", + " \n", + " # Generate the individual traces for the receding horizon control\n", + " for t in ocp.timepts:\n", + " # Compute the optimal trajectory over the horizon\n", + " start_time = time.process_time()\n", + " res = ocp.compute_trajectory(x, print_summary=print_summary)\n", + " if verbose:\n", + " print(f\"{t=}: comp time = {time.process_time() - start_time:0.3}\")\n", + "\n", + " # Simulate the system for the update time, with higher res for plotting\n", + " tvec = np.linspace(0, res.time[1], 20)\n", + " inputs = res.inputs[:, 0] + np.outer(\n", + " (res.inputs[:, 1] - res.inputs[:, 0]) / (tvec[-1] - tvec[0]), tvec)\n", + " soln = ct.input_output_response(proc, tvec, inputs, x)\n", + " \n", + " # Save this segment for later use (final point will appear in next segment)\n", + " time_.append(t + soln.time[:-1])\n", + " inputs_.append(soln.inputs[:, :-1])\n", + " outputs_.append(soln.outputs[:, :-1])\n", + " states_.append(soln.states[:, :-1])\n", + "\n", + " if plot:\n", + " # Plot the results over the full horizon\n", + " h3, = ax.plot(t + res.time, res.states[0], 'k--', linewidth=0.5)\n", + " ax.plot(t + res.time, res.inputs[0], 'b--', linewidth=0.5)\n", + "\n", + " # Plot the results for this time segment\n", + " h1, = ax.plot(t + soln.time, soln.states[0], 'k-')\n", + " h2, = ax.plot(t + soln.time, soln.inputs[0], 'b-')\n", + " \n", + " # Update the state to use for the next time point\n", + " x = soln.states[:, -1]\n", + " \n", + " # Append the final point to the response\n", + " time_.append(t + soln.time[-1:])\n", + " inputs_.append(soln.inputs[:, -1:])\n", + " outputs_.append(soln.outputs[:, -1:])\n", + " states_.append(soln.states[:, -1:])\n", + "\n", + " # Label the plot\n", + " if plot:\n", + " # Adjust the limits for consistency\n", + " ax.set_ylim([-4, 3.5])\n", + "\n", + " # Add reference line for input lower bound\n", + " ax.plot([0, 7], [-1, -1], 'k--', linewidth=0.666)\n", + "\n", + " # Label the results\n", + " ax.set_xlabel(\"Time $t$ [sec]\")\n", + " ax.set_ylabel(\"State $x_1$, input $u$\")\n", + " ax.legend(\n", + " [h1, h2, h3], ['$x_1$', '$u$', 'prediction'],\n", + " loc='lower right', labelspacing=0)\n", + " plt.tight_layout()\n", + " \n", + " # Append\n", + " return ct.TimeResponseData(\n", + " np.hstack(time_), np.hstack(outputs_), np.hstack(states_), np.hstack(inputs_))" + ] + }, + { + "cell_type": "markdown", + "id": "be13e00a", + "metadata": {}, + "source": [ + "Finally, we call the controller and plot the response. The solid lines show the portions of the trajectory that we follow. The dashed lines are the trajectory over the full horizon, but which are not followed since we update the computation at each time step. (To get rid of the statistics of each optimization call, use `print_summary=False`.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "305a1127", + "metadata": {}, + "outputs": [], + "source": [ + "Tf = 10\n", + "rhc_resp = run_rhc_and_plot(proc, ocp, X0, Tf, verbose=True, print_summary=False)\n", + "print(f\"xf = {rhc_resp.states[:, -1]}\")" + ] + }, + { + "cell_type": "markdown", + "id": "6005bfb3", + "metadata": {}, + "source": [ + "## RHC vs LQR vs LQR terminal cost\n", + "\n", + "In the example above, we used a receding horizon controller with the terminal cost as $P_1 = \\text{diag}(0.1, 0.1)$. An alternative is to set the terminal cost to be the LQR terminal cost that goes along with the trajectory cost, which then provides a \"cost to go\" that matches the LQR \"cost to go\" (but keeping in mind that the LQR controller does not necessarily respect the constraints).\n", + "\n", + "The following code compares the original RHC formulation with a receding horizon controller using an LQR terminal cost versus an LQR controller." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ea2de1f3", + "metadata": {}, + "outputs": [], + "source": [ + "# Get the LQR solution\n", + "K, P_lqr, E = ct.lqr(proc.linearize(0, 0), Qx, Qu)\n", + "print(f\"P_lqr = \\n{P_lqr}\")\n", + "\n", + "# Create an LQR controller (and run it)\n", + "lqr_ctrl, lqr_clsys = ct.create_statefbk_iosystem(proc, K)\n", + "lqr_resp = ct.input_output_response(lqr_clsys, rhc_resp.time, 0, X0)\n", + "\n", + "# Create a new optimal control problem using the LQR terminal cost\n", + "# (need use more refined time grid as well, to approximate LQR rate)\n", + "lqr_timepts = np.linspace(0, Th, 25, endpoint=True)\n", + "lqr_term_cost=opt.quadratic_cost(proc, P_lqr, None)\n", + "ocp_lqr = opt.OptimalControlProblem(\n", + " proc, lqr_timepts, traj_cost, terminal_cost=lqr_term_cost,\n", + " trajectory_constraints=traj_constraints,\n", + ")\n", + "\n", + "# Create the response for the new controller\n", + "rhc_lqr_resp = run_rhc_and_plot(\n", + " proc, ocp_lqr, X0, 10, plot=False, print_summary=False)\n", + "\n", + "# Plot the different responses to compare them\n", + "fig, ax = plt.subplots(2, 1)\n", + "ax[0].plot(rhc_resp.time, rhc_resp.states[0], label='RHC + P_1')\n", + "ax[0].plot(rhc_lqr_resp.time, rhc_lqr_resp.states[0], '--', label='RHC + P_lqr')\n", + "ax[0].plot(lqr_resp.time, lqr_resp.outputs[0], ':', label='LQR')\n", + "ax[0].legend()\n", + "\n", + "ax[1].plot(rhc_resp.time, rhc_resp.inputs[0], label='RHC + P_1')\n", + "ax[1].plot(rhc_lqr_resp.time, rhc_lqr_resp.inputs[0], '--', label='RHC + P_lqr')\n", + "ax[1].plot(lqr_resp.time, lqr_resp.outputs[2], ':', label='LQR')" + ] + }, + { + "cell_type": "markdown", + "id": "9497530b", + "metadata": {}, + "source": [ + "## Discrete time RHC\n", + "\n", + "Many receding horizon control problems are solved based on a discrete time model. We show here how to implement this for a \"double integrator\" system, which in discrete time has the form\n", + "\n", + "$$\n", + " x[k+1] = \\begin{bmatrix} 1 & 1 \\\\ 0 & 1 \\end{bmatrix} x[k] + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} \\text{clip}(u[k])\n", + "$$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ae7cefa5", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# System definition\n", + "#\n", + "\n", + "def doubleint_update(t, x, u, params):\n", + " # Get the parameters\n", + " lb = params.get('lb', -1)\n", + " ub = params.get('ub', 1)\n", + " assert lb < ub\n", + "\n", + " # Get the sampling time\n", + " dt = params.get('dt', 1)\n", + "\n", + " # bound the input\n", + " u_clip = np.clip(u, lb, ub)\n", + "\n", + " return np.array([x[0] + dt * x[1], x[1] + dt * u_clip[0]])\n", + "\n", + "proc = ct.NonlinearIOSystem(\n", + " doubleint_update, None, name=\"double integrator\",\n", + " inputs = ['u'], outputs=['x[0]', 'x[1]'], states=2,\n", + " params={'dt': 1}, dt=1)\n", + "\n", + "#\n", + "# Linear quadratic regulator\n", + "#\n", + "\n", + "# Define the cost functions to use\n", + "Qx = np.diag([1, 0]) # state cost\n", + "Qu = np.diag([1]) # input cost\n", + "P1 = np.diag([0.1, 0.1]) # terminal cost\n", + "\n", + "# Get the LQR solution\n", + "K, P, E = ct.dlqr(proc.linearize(0, 0), Qx, Qu)\n", + "\n", + "# Test out the LQR controller, with no constraints\n", + "linsys = proc.linearize(0, 0)\n", + "clsys_lin = ct.ss(linsys.A - linsys.B @ K, linsys.B, linsys.C, 0, dt=proc.dt)\n", + "\n", + "X0 = np.array([2, 1]) # initial conditions\n", + "Tf = 10 # simulation time\n", + "res = ct.initial_response(clsys_lin, Tf, X0=X0)\n", + "\n", + "# Plot the results\n", + "plt.figure(1); plt.clf(); ax = plt.axes()\n", + "ax.plot(res.time, res.states[0], 'k-', label='$x_1$')\n", + "ax.plot(res.time, (-K @ res.states)[0], 'b-', label='$u$')\n", + "\n", + "# Test out the LQR controller with constraints\n", + "clsys_lqr = ct.feedback(proc, -K, 1)\n", + "tvec = np.arange(0, Tf, proc.dt)\n", + "res_lqr_const = ct.input_output_response(clsys_lqr, tvec, 0, X0)\n", + "\n", + "# Plot the results\n", + "ax.plot(res_lqr_const.time, res_lqr_const.states[0], 'k--', label='constrained')\n", + "ax.plot(res_lqr_const.time, (-K @ res_lqr_const.states)[0], 'b--')\n", + "ax.plot([0, 7], [-1, -1], 'k--', linewidth=0.75)\n", + "\n", + "# Adjust the limits for consistency\n", + "ax.set_ylim([-4, 3.5])\n", + "\n", + "# Label the results\n", + "ax.set_xlabel(\"Time $t$ [sec]\")\n", + "ax.set_ylabel(\"State $x_1$, input $u$\")\n", + "ax.legend(loc='lower right', labelspacing=0)\n", + "plt.title(\"Linearized LQR response from x0\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "13cfc5d8", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# Receding horizon controller\n", + "#\n", + "\n", + "# Create the constraints\n", + "traj_constraints = opt.input_range_constraint(proc, -1, 1)\n", + "term_constraints = opt.state_range_constraint(proc, [0, 0], [0, 0])\n", + "\n", + "# Define the optimal control problem we want to solve\n", + "T = 5\n", + "timepts = np.arange(0, T * proc.dt, proc.dt)\n", + "\n", + "# Set up the optimal control problems\n", + "ocp_orig = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P1, None),\n", + ")\n", + "\n", + "ocp_lqr = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P, None),\n", + ")\n", + "\n", + "ocp_low = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P/10, None),\n", + ")\n", + "\n", + "ocp_high = opt.OptimalControlProblem(\n", + " proc, timepts,\n", + " opt.quadratic_cost(proc, Qx, Qu),\n", + " trajectory_constraints=traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P*10, None),\n", + ")\n", + "weight_list = [P1, P, P/10, P*10]\n", + "ocp_list = [ocp_orig, ocp_lqr, ocp_low, ocp_high]\n", + "\n", + "# Do a test run to figure out how long computation takes\n", + "start_time = time.process_time()\n", + "ocp_lqr.compute_trajectory(X0)\n", + "stop_time = time.process_time()\n", + "print(\"* Process time: %0.2g s\\n\" % (stop_time - start_time))\n", + "\n", + "# Create a figure to use for plotting\n", + "fig, [[ax_orig, ax_lqr], [ax_low, ax_high]] = plt.subplots(2, 2)\n", + "ax_list = [ax_orig, ax_lqr, ax_low, ax_high]\n", + "ax_name = ['orig', 'lqr', 'low', 'high']\n", + "\n", + "# Generate the individual traces for the receding horizon control\n", + "for ocp, ax, name, Pf in zip(ocp_list, ax_list, ax_name, weight_list):\n", + " x, t = X0, 0\n", + " for i in np.arange(0, Tf, proc.dt):\n", + " # Calculate the optimal trajectory\n", + " res = ocp.compute_trajectory(x, print_summary=False)\n", + " soln = ct.input_output_response(proc, res.time, res.inputs, x)\n", + "\n", + " # Plot the results for this time instant\n", + " ax.plot(res.time[:2] + t, res.inputs[0, :2], 'b-', linewidth=1)\n", + " ax.plot(res.time[:2] + t, soln.outputs[0, :2], 'k-', linewidth=1)\n", + " \n", + " # Plot the results projected forward\n", + " ax.plot(res.time[1:] + t, res.inputs[0, 1:], 'b--', linewidth=0.75)\n", + " ax.plot(res.time[1:] + t, soln.outputs[0, 1:], 'k--', linewidth=0.75)\n", + " \n", + " # Update the state to use for the next time point\n", + " x = soln.states[:, 1]\n", + " t += proc.dt\n", + "\n", + " # Adjust the limits for consistency\n", + " ax.set_ylim([-1.5, 3.5])\n", + "\n", + " # Label the results\n", + " ax.set_xlabel(\"Time $t$ [sec]\")\n", + " ax.set_ylabel(\"State $x_1$, input $u$\")\n", + " ax.set_title(f\"MPC response for {name}\")\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "015dc953", + "metadata": {}, + "source": [ + "We can also implement a receding horizon controller for a discrete time system using `opt.create_mpc_iosystem`. This creates a controller that accepts the current state as the input and generates the control to apply from that state." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4f8bb594", + "metadata": {}, + "outputs": [], + "source": [ + "# Construct using create_mpc_iosystem\n", + "clsys = opt.create_mpc_iosystem(\n", + " proc, timepts, opt.quadratic_cost(proc, Qx, Qu), traj_constraints,\n", + " terminal_cost=opt.quadratic_cost(proc, P1, None), \n", + ")\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "id": "f1b08fb4", + "metadata": {}, + "source": [ + "(This function needs some work to be more user-friendly, e.g. renaming of the inputs and outputs.)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d2afd287", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L6_stochastic-linsys.ipynb b/examples/cds112-L6_stochastic-linsys.ipynb new file mode 100644 index 000000000..3efc158cb --- /dev/null +++ b/examples/cds112-L6_stochastic-linsys.ipynb @@ -0,0 +1,328 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "03aa22e7", + "metadata": {}, + "source": [ + "# Stochastic Response\n", + "Richard M. Murray, 6 Feb 2022 (updated 9 Feb 2023)\n", + "\n", + "This notebook illustrates the implementation of random processes and stochastic response. We focus on a system of the form\n", + "$$\n", + " \\dot X = A X + F V \\qquad X \\in {\\mathbb R}^n\n", + "$$\n", + "\n", + "where $V$ is a white noise process and the system is a first order linear system." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "902af902", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "from math import sqrt, exp" + ] + }, + { + "cell_type": "markdown", + "id": "77d58303", + "metadata": {}, + "source": [ + "## First order linear system\n", + "\n", + "We start by looking at the stochastic response for a first order linear system\n", + "\n", + "$$\n", + "\\begin{gathered}\n", + " \\dot X = -a X + V, \\qquad Y = C X \\\\\n", + " \\mathbb{E}(V) = 0, \\quad \\mathbb{E}(V^\\mathsf{T}(t_1) V(t_2)) = 0.1\\, \\delta(t_1 - t_2)\n", + "\\end{gathered}\n", + "$$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "60192a8c", + "metadata": {}, + "outputs": [], + "source": [ + "# First order system\n", + "a = 1\n", + "c = 1\n", + "sys = ct.tf(c, [1, a])\n", + "\n", + "# Create the time vector that we want to use\n", + "Tf = 5\n", + "T = np.linspace(0, Tf, 1000)\n", + "dt = T[1] - T[0]\n", + "\n", + "# Create the basis for a white noise signal\n", + "# Note: use sqrt(Q/dt) for desired covariance\n", + "Q = np.array([[0.1]])\n", + "# V = np.random.normal(0, sqrt(Q[0,0]/dt), T.shape)\n", + "V = ct.white_noise(T, Q)\n", + "\n", + "plt.plot(T, V[0])\n", + "plt.xlabel('Time [s]')\n", + "plt.ylabel('$V$');" + ] + }, + { + "cell_type": "markdown", + "id": "b4629e2c", + "metadata": {}, + "source": [ + "Note that the magnitude of the signal seems to be much larger than $Q$. This is because we have a Guassian process $\\implies$ the signal consists of a sequence of \"impulse-like\" functions that have magnitude that increases with the time step $dt$ as $1/\\sqrt{dt}$ (this gives covariance $\\mathbb{E}(V(t_1) V^T(t_2)) = Q \\delta(t_2 - t_1)$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "23319dc6", + "metadata": {}, + "outputs": [], + "source": [ + "# Calculate the sample properties and make sure they match\n", + "print(\"mean(V) [0.0] = \", np.mean(V))\n", + "print(\"cov(V) * dt [%0.3g] = \" % Q, np.round(np.cov(V), decimals=3) * dt)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2bdaaccf", + "metadata": {}, + "outputs": [], + "source": [ + "# Response of the first order system\n", + "# Scale white noise by sqrt(dt) to account for impulse\n", + "T, Y = ct.forced_response(sys, T, V)\n", + "plt.plot(T, Y)\n", + "plt.xlabel('Time [s]')\n", + "plt.ylabel('$Y$');" + ] + }, + { + "cell_type": "markdown", + "id": "ead0232e", + "metadata": {}, + "source": [ + "This is a first order system, and so we can use the calculation from the course\n", + "notes to compute the analytical correlation function and compare this to the \n", + "sampled data:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d31ce324", + "metadata": {}, + "outputs": [], + "source": [ + "# Compare static properties to what we expect analytically\n", + "def r(tau):\n", + " return c**2 * Q / (2 * a) * exp(-a * abs(tau))\n", + " \n", + "print(\"* mean(Y) [%0.3g] = %0.3g\" % (0, np.mean(Y).item()))\n", + "print(\"* cov(Y) [%0.3g] = %0.3g\" % (r(0).item(), np.cov(Y).item()))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1cf5a4b1", + "metadata": {}, + "outputs": [], + "source": [ + "# Correlation function for the input\n", + "# Scale by dt to take time step into account\n", + "# r_V = sp.signal.correlate(V, V) * dt / Tf\n", + "# tau = sp.signal.correlation_lags(len(V), len(V)) * dt\n", + "tau, r_V = ct.correlation(T, V)\n", + "\n", + "plt.plot(tau, r_V, 'r-')\n", + "plt.xlabel(r'$\\tau$')\n", + "plt.ylabel(r'$r_V(\\tau)$');" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "62af90a4", + "metadata": {}, + "outputs": [], + "source": [ + "# Correlation function for the output\n", + "# r_Y = sp.signal.correlate(Y, Y) * dt / Tf\n", + "# tau = sp.signal.correlation_lags(len(Y), len(Y)) * dt\n", + "tau, r_Y = ct.correlation(T, Y)\n", + "plt.plot(tau, r_Y)\n", + "plt.xlabel(r'$\\tau$')\n", + "plt.ylabel(r'$r_Y(\\tau)$')\n", + "\n", + "# Compare to the analytical answer\n", + "plt.plot(tau, [r(t)[0, 0] for t in tau], 'k--');" + ] + }, + { + "cell_type": "markdown", + "id": "2a2785e9", + "metadata": {}, + "source": [ + "The analytical curve may or may not line up that well with the correlation function based on the sample. Try running the code again from the top to see how things change based on the specific random sequence chosen at the start.\n", + "\n", + "Note: the _right_ way to compute the correlation function would be to run a lot of different samples of white noise filtered through the system dynamics and compute $R(t_1, t_2)$ across those samples." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "bd5dfc75", + "metadata": {}, + "outputs": [], + "source": [ + "# As a crude approximation, compute the average correlation\n", + "r_avg = np.zeros_like(r_Y)\n", + "for i in range(100):\n", + " V = ct.white_noise(T, Q)\n", + " _, Y = ct.forced_response(sys, T, V)\n", + " tau, r_Y = ct.correlation(T, Y)\n", + " r_avg = r_avg + r_Y\n", + "r_avg = r_avg / i\n", + "plt.plot(tau, r_avg)\n", + "plt.xlabel(r'$\\tau$')\n", + "plt.ylabel(r'$r_Y(\\tau)$')\n", + "\n", + "# Compare to the analytical answer\n", + "plt.plot(tau, [r(t)[0, 0] for t in tau], 'k--');" + ] + }, + { + "cell_type": "markdown", + "id": "f07ec584", + "metadata": {}, + "source": [ + "## Dryden gust model\n", + "\n", + "Friedland, _Control Systems Design_, Example 10B\n", + "\n", + "Based on experimental data, the power spectral density for the vertical component of random wind velocity in turbulent air can be modeled as\n", + "$$\n", + "S(\\omega) = \\sigma_\\text{z}^2 T \\frac{1 + 3 (\\omega T)^2}{[1 + (\\omega T)^2]^2},\n", + "$$\n", + "where $\\sigma_\\text{z}$ and $T$ are parameters that depend on the wind characteristics.\n", + "\n", + "This power spectral density can be modeled using white noise by running it through a linear system with transfer fucntion\n", + "$$\n", + "H(s) = \\frac{1 + \\sqrt{3} T}{(1 + T s)^2}.\n", + "$$\n", + "A state space realization for this transfer function is given by\n", + "$$\n", + "\\begin{aligned}\n", + " \\dot X &= \\begin{bmatrix} 0 & 1 \\\\ -\\frac{1}{T^2} & -\\frac{2}{T} \\end{bmatrix} X \n", + " + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} V \\\\\n", + " Y &= \\begin{bmatrix} \\frac{1}{T^2} & \\frac{\\sqrt{3}}{T} \\end{bmatrix}\n", + " \\end{aligned}\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "id": "d09fc03a", + "metadata": {}, + "source": [ + "To create a disturbance signal with the characteristics of the Dryden gust model, we create a linear system with the given parameters and computing the input/output response to white noise:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8df16a23", + "metadata": {}, + "outputs": [], + "source": [ + "sigma_z = 1\n", + "T = 1\n", + "filter = ct.ss([[0, 1], [-1/T**2, -2/T]], [[0], [1]], [[1/T**2, sqrt(3)/T]], 0)\n", + "\n", + "timepts = np.linspace(0, 10, 1000)\n", + "V = ct.white_noise(timepts, sigma_z**2)\n", + "resp = ct.input_output_response(filter, timepts, V)\n", + "\n", + "plt.plot(resp.time, resp.outputs);" + ] + }, + { + "cell_type": "markdown", + "id": "4d6604ee", + "metadata": {}, + "source": [ + "We can compute the correlation function and power spectral density to confirm that we match the desired characteristics:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "febc8b80", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the correlation function\n", + "tau, R = ct.correlation(resp.time, resp.outputs)\n", + "\n", + "# Analytical expression for the correlation function (see Friedland)\n", + "def dryden_corrfcn(tau, sigma_z=1, T=1):\n", + " return sigma_z**2 * np.exp(-np.abs(tau)/T) * (1- np.abs(tau)/(2*T))\n", + "\n", + "# Plot the correlation function\n", + "fig, axs = plt.subplots(1, 2)\n", + "axs[0].plot(tau, R)\n", + "axs[0].plot(tau, dryden_corrfcn(tau))\n", + "axs[0].set_xlabel(r\"$\\tau$\")\n", + "axs[0].set_ylabel(r\"$r(\\tau)$\")\n", + "axs[0].set_title(\"Correlation function\")\n", + "\n", + "# Compute the power spectral density\n", + "dt = timepts[1] - timepts[0]\n", + "S = sp.fft.rfft(R) * dt * 2 # rfft returns omega >= 0 => muliple mag by 2\n", + "omega = sp.fft.rfftfreq(R.size, dt)\n", + "\n", + "# Analytical expression for the correlation function (see Friedland)\n", + "def dryden_psd(omega, sigma_z=1., T=1.):\n", + " return sigma_z**2 * T * (1 + 3 * (omega * T)**2) / (1 + (omega * T)**2)**2\n", + "\n", + "# Plot the power spectral density\n", + "axs[1].loglog(omega[1:], np.abs(S[1:]))\n", + "axs[1].loglog(omega[1:], dryden_psd(omega[1:]))\n", + "axs[1].set_xlabel(r\"$\\omega$ [rad/sec]\")\n", + "axs[1].set_ylabel(r\"$S(\\omega)$\")\n", + "axs[1].set_title(\"Power spectral density\")\n", + "\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1516ff6a", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L7_kalman-pvtol.ipynb b/examples/cds112-L7_kalman-pvtol.ipynb new file mode 100644 index 000000000..435c7fce6 --- /dev/null +++ b/examples/cds112-L7_kalman-pvtol.ipynb @@ -0,0 +1,425 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "c017196f", + "metadata": {}, + "source": [ + "# PVTOL LQR + EQF example\n", + "RMM, 14 Feb 2022\n", + "\n", + "This notebook illustrates the implementation of an extended Kalman filter and the use of the estimated state for LQR feedback." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "544525ab", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import matplotlib.patches as patches\n", + "import control as ct" + ] + }, + { + "cell_type": "markdown", + "id": "859834cf", + "metadata": {}, + "source": [ + "## System definition\n", + "The dynamics of the system\n", + "with disturbances on the $x$ and $y$ variables is given by\n", + "$$\n", + " \\begin{aligned}\n", + " m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x + d_x, \\\\\n", + " m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - c \\dot y - m g + d_y, \\\\\n", + " J \\ddot \\theta &= r F_1.\n", + " \\end{aligned}\n", + "$$\n", + "The measured values of the system are the position and orientation,\n", + "with added noise $n_x$, $n_y$, and $n_\\theta$:\n", + "$$\n", + " \\vec y = \\begin{bmatrix} x \\\\ y \\\\ \\theta \\end{bmatrix} + \n", + " \\begin{bmatrix} n_x \\\\ n_y \\\\ n_z \\end{bmatrix}.\n", + "$$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ffafed74", + "metadata": {}, + "outputs": [], + "source": [ + "# pvtol = nominal system (no disturbances or noise)\n", + "# noisy_pvtol = pvtol w/ process disturbances and sensor noise\n", + "from pvtol import pvtol, pvtol_noisy, plot_results\n", + "\n", + "# Find the equilibrium point corresponding to the origin\n", + "xe, ue = ct.find_eqpt(\n", + " pvtol, np.zeros(pvtol.nstates),\n", + " np.zeros(pvtol.ninputs), [0, 0, 0, 0, 0, 0],\n", + " iu=range(2, pvtol.ninputs), iy=[0, 1])\n", + "\n", + "x0, u0 = ct.find_eqpt(\n", + " pvtol, np.zeros(pvtol.nstates),\n", + " np.zeros(pvtol.ninputs), np.array([2, 1, 0, 0, 0, 0]),\n", + " iu=range(2, pvtol.ninputs), iy=[0, 1])\n", + "\n", + "# Extract the linearization for use in LQR design\n", + "pvtol_lin = pvtol.linearize(xe, ue)\n", + "A, B = pvtol_lin.A, pvtol_lin.B\n", + "\n", + "print(pvtol, \"\\n\")\n", + "print(pvtol_noisy)" + ] + }, + { + "cell_type": "markdown", + "id": "2b63bf5b", + "metadata": {}, + "source": [ + "We now define the properties of the noise and disturbances. To make things (a bit more) interesting, we include some cross terms between the noise in $\\theta$ and the noise in $x$ and $y$:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1e1ee7c9", + "metadata": {}, + "outputs": [], + "source": [ + "# Disturbance and noise intensities\n", + "Qv = np.diag([1e-2, 1e-2])\n", + "Qw = np.array([[2e-4, 0, 1e-5], [0, 2e-4, 1e-5], [1e-5, 1e-5, 1e-4]])\n", + "Qwinv = np.linalg.inv(Qw)\n", + "\n", + "# Initial state covariance\n", + "P0 = np.eye(pvtol.nstates)" + ] + }, + { + "cell_type": "markdown", + "id": "e4c52c73", + "metadata": {}, + "source": [ + "## Control system design\n", + "\n", + "To design the control system, we first construct an estimator for the state (given the commanded inputs and measured outputs. Since this is a nonlinear system, we use the update law for the nominal system to compute the state update. We also make use of the linearization around the current state for the covariance update (using the function `pvtol.A(x, u)`, which is defined in `pvtol.py`, making this an extended Kalman filter)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3647bf15", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the disturbance input and measured output matrices\n", + "F = np.array([[0, 0], [0, 0], [0, 0], [1/pvtol.params['m'], 0], [0, 1/pvtol.params['m']], [0, 0]])\n", + "C = np.eye(3, 6)\n", + "\n", + "# Estimator update law\n", + "def estimator_update(t, x, u, params):\n", + " # Extract the states of the estimator\n", + " xhat = x[0:pvtol.nstates]\n", + " P = x[pvtol.nstates:].reshape(pvtol.nstates, pvtol.nstates)\n", + "\n", + " # Extract the inputs to the estimator\n", + " y = u[0:3] # just grab the first three outputs\n", + " u = u[6:8] # get the inputs that were applied as well\n", + "\n", + " # Compute the linearization at the current state\n", + " A = pvtol.A(xhat, u) # A matrix depends on current state\n", + " # A = pvtol.A(xe, ue) # Fixed A matrix (for testing/comparison)\n", + " \n", + " # Compute the optimal again\n", + " L = P @ C.T @ Qwinv\n", + "\n", + " # Update the state estimate\n", + " xhatdot = pvtol.updfcn(t, xhat, u, params) - L @ (C @ xhat - y)\n", + "\n", + " # Update the covariance\n", + " Pdot = A @ P + P @ A.T - P @ C.T @ Qwinv @ C @ P + F @ Qv @ F.T\n", + "\n", + " # Return the derivative\n", + " return np.hstack([xhatdot, Pdot.reshape(-1)])\n", + "\n", + "def estimator_output(t, x, u, params):\n", + " # Return the estimator states\n", + " return x[0:pvtol.nstates]\n", + "\n", + "estimator = ct.NonlinearIOSystem(\n", + " estimator_update, estimator_output,\n", + " states=pvtol.nstates + pvtol.nstates**2,\n", + " inputs= pvtol_noisy.output_labels \\\n", + " + pvtol_noisy.input_labels[0:pvtol.ninputs],\n", + " outputs=[f'xh{i}' for i in range(pvtol.nstates)],\n", + ")\n", + "print(estimator)" + ] + }, + { + "cell_type": "markdown", + "id": "ba3d2640", + "metadata": {}, + "source": [ + "For the controller, we will use an LQR feedback with physically motivated weights (see OBC, Example 3.5):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9787db61", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# LQR design w/ physically motivated weighting\n", + "#\n", + "# Shoot for 1 cm error in x, 10 cm error in y. Try to keep the angle\n", + "# less than 5 degrees in making the adjustments. Penalize side forces\n", + "# due to loss in efficiency.\n", + "#\n", + "\n", + "Qx = np.diag([100, 10, (180/np.pi) / 5, 0, 0, 0])\n", + "Qu = np.diag([10, 1])\n", + "K, _, _ = ct.lqr(A, B, Qx, Qu)\n", + "\n", + "#\n", + "# Control system construction: combine LQR w/ EKF\n", + "#\n", + "# Use the linearization around the origin to design the optimal gains\n", + "# to see how they compare to the final value of P for the EKF\n", + "#\n", + "\n", + "# Construct the state feedback controller with estimated state as input\n", + "statefbk, _ = ct.create_statefbk_iosystem(pvtol, K, estimator=estimator)\n", + "print(statefbk, \"\\n\")\n", + "\n", + "# Reconstruct the control system with the noisy version of the process\n", + "# Create a closed loop system around the controller\n", + "clsys = ct.interconnect(\n", + " [pvtol_noisy, statefbk, estimator],\n", + " inplist = statefbk.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", + " pvtol_noisy.input_labels[pvtol.ninputs:],\n", + " inputs = statefbk.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", + " pvtol_noisy.input_labels[pvtol.ninputs:],\n", + " outlist = pvtol.output_labels + statefbk.output_labels + estimator.output_labels,\n", + " outputs = pvtol.output_labels + statefbk.output_labels + estimator.output_labels\n", + ")\n", + "print(clsys)" + ] + }, + { + "cell_type": "markdown", + "id": "5f527f16", + "metadata": {}, + "source": [ + "Note that we have to construct the closed loop system manually since we need to allow the disturbance and noise inputs to be sent to the closed loop system and `create_statefbk_iosystem` does not support this (to be fixed in an upcoming release)." + ] + }, + { + "cell_type": "markdown", + "id": "7bf558a0", + "metadata": {}, + "source": [ + "## Simulations\n", + "\n", + "Finally, we can simulate the system to see how it all works. We start by creating the noise for the system:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c2583a0e", + "metadata": {}, + "outputs": [], + "source": [ + "# Create the time vector for the simulation\n", + "Tf = 10\n", + "timepts = np.linspace(0, Tf, 1000)\n", + "\n", + "# Create representative process disturbance and sensor noise vectors\n", + "np.random.seed(117) # avoid figures changing from run to run\n", + "V = ct.white_noise(timepts, Qv) # smaller disturbances and noise then design\n", + "W = ct.white_noise(timepts, Qw)\n", + "plt.plot(timepts, V[0], label=\"V[0]\")\n", + "plt.plot(timepts, W[0], label=\"W[0]\")\n", + "plt.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "4d944709", + "metadata": {}, + "source": [ + "### LQR with EKF\n", + "\n", + "We can now feed the desired trajectory plus the noise and disturbances into the system and see how well the controller with a state estimator does in holding the system at an equilibrium point:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ad7a9750", + "metadata": {}, + "outputs": [], + "source": [ + "# Put together the input for the system\n", + "U = [xe, ue, V, W]\n", + "X0 = [x0, xe, P0.reshape(-1)]\n", + "\n", + "# Initial condition response\n", + "resp = ct.input_output_response(clsys, timepts, U, X0)\n", + "\n", + "# Plot the response\n", + "plot_results(timepts, resp.states, resp.outputs[pvtol.nstates:])" + ] + }, + { + "cell_type": "markdown", + "id": "86f10064", + "metadata": {}, + "source": [ + "To see how well the estimtator did, we can compare the estimated position with the actual position:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c5f24119", + "metadata": {}, + "outputs": [], + "source": [ + "# Response of the first two states, including internal estimates\n", + "h1, = plt.plot(resp.time, resp.outputs[0], 'b-', linewidth=0.75)\n", + "h2, = plt.plot(resp.time, resp.outputs[1], 'r-', linewidth=0.75)\n", + "\n", + "# Add on the internal estimator states\n", + "xh0 = clsys.find_output('xh0')\n", + "xh1 = clsys.find_output('xh1')\n", + "h3, = plt.plot(resp.time, resp.outputs[xh0], 'k--')\n", + "h4, = plt.plot(resp.time, resp.outputs[xh1], 'k--')\n", + "\n", + "plt.plot([0, 10], [0, 0], 'k--', linewidth=0.5)\n", + "plt.ylabel(r\"Position $x$, $y$ [m]\")\n", + "plt.xlabel(r\"Time $t$ [s]\")\n", + "plt.legend(\n", + " [h1, h2, h3, h4], ['$x$', '$y$', r'$\\hat{x}$', r'$\\hat{y}$'], \n", + " loc='upper right', frameon=False, ncol=2);" + ] + }, + { + "cell_type": "markdown", + "id": "7139202f", + "metadata": {}, + "source": [ + "Note the rapid convergence of the estimate to the proper value, since we are directly measuring the position variables. If we look at the full set of states, we see that other variables have different convergence properties:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "78a61e74", + "metadata": {}, + "outputs": [], + "source": [ + "fig, axs = plt.subplots(2, 3)\n", + "var = ['x', 'y', r'\\theta', r'\\dot x', r'\\dot y', r'\\dot \\theta']\n", + "for i in [0, 1]:\n", + " for j in [0, 1, 2]:\n", + " k = i * 3 + j\n", + " axs[i, j].plot(resp.time, resp.outputs[k], label=f'${var[k]}$')\n", + " axs[i, j].plot(resp.time, resp.outputs[xh0+k], label=f'$\\\\hat {var[k]}$')\n", + " axs[i, j].legend()\n", + " if i == 1:\n", + " axs[i, j].set_xlabel(\"Time $t$ [s]\")\n", + " if j == 0:\n", + " axs[i, j].set_ylabel(\"State\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "2039578e", + "metadata": {}, + "source": [ + "Note the lag in tracking changes in the $\\dot x$ and $\\dot y$ states (varies from simulation to simulation, depending on the specific noise signal)." + ] + }, + { + "cell_type": "markdown", + "id": "0c0d5c99", + "metadata": {}, + "source": [ + "### Full state feedback\n", + "\n", + "To see how the inclusion of the estimator affects the system performance, we compare it with the case where we are able to directly measure the state of the system." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3b6a1f1c", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the full state feedback solution\n", + "lqr_ctrl, _ = ct.create_statefbk_iosystem(pvtol, K)\n", + "\n", + "lqr_clsys = ct.interconnect(\n", + " [pvtol_noisy, lqr_ctrl],\n", + " inplist = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", + " pvtol_noisy.input_labels[pvtol.ninputs:],\n", + " inputs = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", + " pvtol_noisy.input_labels[pvtol.ninputs:],\n", + " outlist = pvtol.output_labels + lqr_ctrl.output_labels,\n", + " outputs = pvtol.output_labels + lqr_ctrl.output_labels\n", + ")\n", + "\n", + "# Put together the input for the system (turn off sensor noise)\n", + "U = [xe, ue, V, W*0]\n", + "\n", + "# Run a simulation with full state feedback\n", + "lqr_resp = ct.input_output_response(lqr_clsys, timepts, U, x0)\n", + "\n", + "# Compare the results\n", + "plt.plot(resp.states[0], resp.states[1], 'b-', label=\"Extended KF\")\n", + "plt.plot(lqr_resp.states[0], lqr_resp.states[1], 'r-', label=\"Full state\")\n", + "\n", + "plt.xlabel('$x$ [m]')\n", + "plt.ylabel('$y$ [m]')\n", + "plt.axis('equal')\n", + "plt.legend(frameon=False);" + ] + }, + { + "cell_type": "markdown", + "id": "8c0083cb", + "metadata": {}, + "source": [ + "Things to try:\n", + "* Compute a feasable trajectory and stabilize around that instead of the origin" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "777053a4", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L8_fusion-kincar.ipynb b/examples/cds112-L8_fusion-kincar.ipynb new file mode 100644 index 000000000..20bc26c93 --- /dev/null +++ b/examples/cds112-L8_fusion-kincar.ipynb @@ -0,0 +1,476 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "eec23018", + "metadata": {}, + "source": [ + "# Kinematic car sensor fusion example\n", + "RMM, 24 Feb 2022 (updated 23 Feb 2023)\n", + "\n", + "In this example we work through estimation of the state of a car changing\n", + "lanes with two different sensors available: one with good longitudinal accuracy\n", + "and the other with good lateral accuracy.\n", + "\n", + "All calculations are done in discrete time, using both the form of the Kalman\n", + "filter in Theorem 7.2 and the predictor corrector form." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "107a6613", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "import control.optimal as opt\n", + "import control.flatsys as fs\n", + "\n", + "# Define some line styles for later use\n", + "ebarstyle = {'elinewidth': 0.5, 'capsize': 2}\n", + "xdstyle = {'color': 'k', 'linestyle': '--', 'linewidth': 0.5, \n", + " 'marker': '+', 'markersize': 4}" + ] + }, + { + "cell_type": "markdown", + "id": "ea8807a4", + "metadata": {}, + "source": [ + "## System definition\n", + "\n", + "We make use of a simple model for a vehicle navigating in the plane, known as the \"bicycle model\". The kinematics of this vehicle can be written in terms of the contact point $(x, y)$ and the angle $\\theta$ of the vehicle with respect to the horizontal axis:\n", + "\n", + "\n", + "\n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " \\dot x &= \\cos\\theta\\, v \\\\\n", + " \\dot y &= \\sin\\theta\\, v \\\\\n", + " \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "The input $v$ represents the velocity of the vehicle and the input $\\delta$ represents the turning rate. The parameter $l$ is the wheelbase." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a04106f8", + "metadata": {}, + "outputs": [], + "source": [ + "# Vehicle steering dynamics\n", + "#\n", + "# System state: x, y, theta\n", + "# System input: v, phi\n", + "# System output: x, y\n", + "# System parameters: wheelbase, maxsteer\n", + "#\n", + "from kincar import kincar, plot_lanechange\n", + "print(kincar)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "69c048ed", + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a trajectory for the vehicle\n", + "# Define the endpoints of the trajectory\n", + "x0 = [0., -2., 0.]; u0 = [10., 0.]\n", + "xf = [40., 2., 0.]; uf = [10., 0.]\n", + "Tf = 4\n", + "\n", + "# Find a trajectory between the initial condition and the final condition\n", + "traj = fs.point_to_point(kincar, Tf, x0, u0, xf, uf, basis=fs.PolyFamily(6))\n", + "\n", + "# Create the desired trajectory between the initial and final condition\n", + "Ts = 0.1\n", + "# Ts = 0.5\n", + "timepts = np.arange(0, Tf + Ts, Ts)\n", + "xd, ud = traj.eval(timepts)\n", + "\n", + "plot_lanechange(timepts, xd, ud)" + ] + }, + { + "cell_type": "markdown", + "id": "aeeaa39e", + "metadata": {}, + "source": [ + "### Discrete time system model\n", + "\n", + "For the model that we use for the Kalman filter, we take a simple discretization using the approximation that $\\dot x = (x[k+1] - x[k])/T_s$ where $T_s$ is the sampling time." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2469c60e", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# Create a discrete time, linear model\n", + "#\n", + "\n", + "# Linearize about the starting point\n", + "linsys = ct.linearize(kincar, x0, u0)\n", + "\n", + "# Create a discrete time model by hand\n", + "Ad = np.eye(linsys.nstates) + linsys.A * Ts\n", + "Bd = linsys.B * Ts\n", + "discsys = ct.ss(Ad, Bd, np.eye(linsys.nstates), 0, dt=Ts)\n", + "print(discsys);" + ] + }, + { + "cell_type": "markdown", + "id": "084c5ae8", + "metadata": {}, + "source": [ + "### Sensor model\n", + "\n", + "We assume that we have two sensors: one with good longitudinal accuracy and the other with good lateral accuracy. For each sensor we define the map from the state space to the sensor outputs, the covariance matrix for the measurements, and a white noise signal (now in discrete time).\n", + "\n", + "Note: we pass the keyword `dt` to the `white_noise` function so that the white noise is consistent with a discrete time model (so the covariance is _not_ rescaled by $\\sqrt{dt}$)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0a19d109", + "metadata": {}, + "outputs": [], + "source": [ + "# Sensor #1: longitudinal\n", + "C_lon = np.eye(2, discsys.nstates)\n", + "Rw_lon = np.diag([0.1 ** 2, 1 ** 2])\n", + "W_lon = ct.white_noise(timepts, Rw_lon, dt=Ts)\n", + "\n", + "# Sensor #2: lateral\n", + "C_lat = np.eye(2, discsys.nstates)\n", + "Rw_lat = np.diag([1 ** 2, 0.1 ** 2])\n", + "W_lat = ct.white_noise(timepts, Rw_lat, dt=Ts)\n", + "\n", + "# Plot the noisy signals\n", + "plt.subplot(2, 1, 1)\n", + "Y = xd[0:2] + W_lon\n", + "plt.plot(Y[0], Y[1])\n", + "plt.plot(xd[0], xd[1], **xdstyle)\n", + "plt.xlabel(\"$x$ position [m]\")\n", + "plt.ylabel(\"$y$ position [m]\")\n", + "plt.title(\"Sensor #1 (longitudinal)\")\n", + " \n", + "plt.subplot(2, 1, 2)\n", + "Y = xd[0:2] + W_lat\n", + "plt.plot(Y[0], Y[1])\n", + "plt.plot(xd[0], xd[1], **xdstyle)\n", + "plt.xlabel(\"$x$ position [m]\")\n", + "plt.ylabel(\"$y$ position [m]\")\n", + "plt.title(\"Sensor #2 (lateral)\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "c3fa1a3d", + "metadata": {}, + "source": [ + "## Linear Quadratic Estimator\n", + "\n", + "We now construct a linear quadratic estimator for the system usign the Kalman filter form. This is idone using the [`create_estimator_iosystem`](https://github.com/python-control/python-control/blob/main/control/stochsys.py#L310-L517) function in python-control." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "993601a2", + "metadata": {}, + "outputs": [], + "source": [ + "# Disturbance and initial condition model\n", + "# Note: multiple by sampling time since we discretized the dynamics\n", + "Rv = np.diag([0.1, 0.01]) * Ts\n", + "# Rv = np.diag([10, 1]) * Ts # Variant: no input information\n", + "P0 = np.diag([1, 1, 0.1])\n", + "\n", + "# Combine the sensors\n", + "# Note: no sampling time here because we are doing discrete-time KF\n", + "C = np.vstack([C_lon, C_lat])\n", + "Rw = sp.linalg.block_diag(Rw_lon, Rw_lat)\n", + "\n", + "estim = ct.create_estimator_iosystem(discsys, Rv, Rw, C=C, P0=P0)\n", + "print(estim)" + ] + }, + { + "cell_type": "markdown", + "id": "0c2e8ab0", + "metadata": {}, + "source": [ + "We can now run the estimator on the noisy signals to see how well it works." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3d02ec33", + "metadata": {}, + "outputs": [], + "source": [ + "# Compute the inputs to the estimator\n", + "Y = np.vstack([xd[0:2] + W_lon, xd[0:2] + W_lat])\n", + "U = np.vstack([Y, ud]) # add input to the Kalman filter\n", + "# U = np.vstack([Y, ud * 0]) # variant: no input information\n", + "X0 = np.hstack([xd[:, 0], P0.reshape(-1)])\n", + "\n", + "# Run the estimator on the trajectory\n", + "estim_resp = ct.input_output_response(estim, timepts, U, X0)\n", + "\n", + "# Run a prediction to see what happens next\n", + "T_predict = np.arange(timepts[-1], timepts[-1] + 4 + Ts, Ts)\n", + "U_predict = np.outer(U[:, -1], np.ones_like(T_predict))\n", + "predict_resp = ct.input_output_response(\n", + " estim, T_predict, U_predict, estim_resp.states[:, -1],\n", + " params={'correct': False})\n", + "\n", + "# Plot the estimated trajectory versus the actual trajectory\n", + "plt.subplot(2, 1, 1)\n", + "plt.errorbar(\n", + " estim_resp.time, estim_resp.outputs[0], \n", + " estim_resp.states[estim.find_state('P[0,0]')], fmt='b-', **ebarstyle)\n", + "plt.errorbar(\n", + " predict_resp.time, predict_resp.outputs[0], \n", + " predict_resp.states[estim.find_state('P[0,0]')], fmt='r-', **ebarstyle)\n", + "plt.plot(timepts, xd[0], 'k--')\n", + "plt.ylabel(\"$x$ position [m]\")\n", + "\n", + "plt.subplot(2, 1, 2)\n", + "plt.errorbar(\n", + " estim_resp.time, estim_resp.outputs[1], \n", + " estim_resp.states[estim.find_state('P[1,1]')], fmt='b-', **ebarstyle)\n", + "plt.errorbar(\n", + " predict_resp.time, predict_resp.outputs[1], \n", + " predict_resp.states[estim.find_state('P[1,1]')], fmt='r-', **ebarstyle)\n", + "# lims = plt.axis(); plt.axis([lims[0], lims[1], -5, 5])\n", + "plt.plot(timepts, xd[1], 'k--');\n", + "plt.ylabel(\"$y$ position [m]\")\n", + "plt.xlabel(\"Time $t$ [s]\");" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "44f69f79", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the estimated errors\n", + "plt.subplot(2, 1, 1)\n", + "plt.errorbar(\n", + " estim_resp.time, estim_resp.outputs[0] - xd[0], \n", + " estim_resp.states[estim.find_state('P[0,0]')], fmt='b-', **ebarstyle)\n", + "plt.errorbar(\n", + " predict_resp.time, predict_resp.outputs[0] - (xd[0] + xd[0, -1]), \n", + " predict_resp.states[estim.find_state('P[0,0]')], fmt='r-', **ebarstyle)\n", + "lims = plt.axis(); plt.axis([lims[0], lims[1], -0.2, 0.2])\n", + "# lims = plt.axis(); plt.axis([lims[0], lims[1], -2, 0.2])\n", + "\n", + "plt.subplot(2, 1, 2)\n", + "plt.errorbar(\n", + " estim_resp.time, estim_resp.outputs[1] - xd[1], \n", + " estim_resp.states[estim.find_state('P[1,1]')], fmt='b-', **ebarstyle)\n", + "plt.errorbar(\n", + " predict_resp.time, predict_resp.outputs[1] - xd[1, -1], \n", + " predict_resp.states[estim.find_state('P[1,1]')], fmt='r-', **ebarstyle)\n", + "lims = plt.axis(); plt.axis([lims[0], lims[1], -0.2, 0.2]);" + ] + }, + { + "cell_type": "markdown", + "id": "6f6c1b6f", + "metadata": {}, + "source": [ + "## Things to try\n", + "* Remove the input (and update P0 and Rv)\n", + "* Change the sampling rate" + ] + }, + { + "cell_type": "markdown", + "id": "8f680b92", + "metadata": {}, + "source": [ + "## Predictor-corrector form\n", + "\n", + "Instead of using create_estimator_iosystem, we can also compute out the estimate in a more manual fashion, done here using the predictor-corrector form." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "fa488d51", + "metadata": {}, + "outputs": [], + "source": [ + "# System matrices\n", + "A, B, F = discsys.A, discsys.B, discsys.B\n", + "\n", + "# Create an array to store the results\n", + "xhat = np.zeros((discsys.nstates, timepts.size))\n", + "P = np.zeros((discsys.nstates, discsys.nstates, timepts.size))\n", + "\n", + "# Update the estimates at each time\n", + "for i, t in enumerate(timepts):\n", + " # Prediction step\n", + " if i == 0:\n", + " # Use the initial condition\n", + " xkkm1 = xd[:, 0]\n", + " Pkkm1 = P0\n", + " else:\n", + " xkkm1 = A @ xkk + B @ ud[:, i-1]\n", + " Pkkm1 = A @ Pkk @ A.T + F @ Rv @ F.T\n", + " \n", + " # Correction step (variant: apply only when sensor data is available)\n", + " L = Pkkm1 @ C.T @ np.linalg.inv(Rw + C @ Pkkm1 @ C.T)\n", + " xkk = xkkm1 - L @ (C @ xkkm1 - Y[:, i])\n", + " Pkk = Pkkm1 - L @ C @ Pkkm1\n", + "\n", + " # Save the state estimate and covariance for later plotting\n", + " xhat[:, i], P[:, :, i] = xkkm1, Pkkm1 # For comparison to Kalman form\n", + " # xhat[:, i], P[:, :, i] = xkk, Pkk # variant: \n", + " \n", + "plt.subplot(2, 1, 1)\n", + "plt.errorbar(timepts, xhat[0], P[0, 0], fmt='b-', **ebarstyle)\n", + "plt.plot(timepts, xd[0], 'k--')\n", + "plt.ylabel(\"$x$ position [m]\")\n", + "\n", + "plt.subplot(2, 1, 2)\n", + "plt.errorbar(timepts, xhat[1], P[1, 1], fmt='b-', **ebarstyle)\n", + "plt.plot(timepts, xd[1], 'k--')\n", + "plt.ylabel(\"$x$ position [m]\")\n", + "plt.xlabel(\"Time $t$ [s]\");" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4eda4729", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the estimated errors (and compare to Kalman form)\n", + "plt.subplot(2, 1, 1)\n", + "plt.errorbar(timepts, xhat[0] - xd[0], P[0, 0], fmt='b-', **ebarstyle)\n", + "plt.plot(estim_resp.time, estim_resp.outputs[0] - xd[0], 'r--', linewidth=3)\n", + "lims = plt.axis(); plt.axis([lims[0], lims[1], -0.2, 0.2])\n", + "plt.ylabel(\"x error [m]\")\n", + "\n", + "plt.subplot(2, 1, 2)\n", + "plt.errorbar(timepts, xhat[1] - xd[1], P[1, 1], fmt='b-', **ebarstyle,\n", + " label='predictor/corrector')\n", + "plt.plot(estim_resp.time, estim_resp.outputs[1] - xd[1], 'r--', linewidth=3,\n", + " label='Kalman form')\n", + "lims = plt.axis(); plt.axis([lims[0], lims[1], -0.2, 0.2])\n", + "plt.ylabel(\"y error [m]\")\n", + "plt.xlabel(\"Time $t$ [s]\")\n", + "plt.legend(loc='lower right');" + ] + }, + { + "cell_type": "markdown", + "id": "19a673a1", + "metadata": {}, + "source": [ + "## Information filter\n", + "\n", + "An alternative way to implement the computation is using the information filter formulation." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "36111bc2", + "metadata": {}, + "outputs": [], + "source": [ + "from numpy.linalg import inv\n", + "\n", + "# Update the estimates at each time\n", + "for i, t in enumerate(timepts):\n", + " # Prediction step\n", + " if i == 0:\n", + " # Use the initial condition\n", + " xkkm1 = xd[:, 0]\n", + " Pkkm1 = P0\n", + " else:\n", + " xkkm1 = A @ xkk + B @ ud[:, i-1]\n", + " Pkkm1 = A @ Pkk @ A.T + F @ Rv @ F.T\n", + " \n", + " # Correction step (variant: apply only when sensor data is available)\n", + " Ikk, Zkk = inv(Pkkm1), inv(Pkkm1) @ xkkm1\n", + " \n", + " # Longitudinal sensor update\n", + " Ikk += C_lon.T @ inv(Rw_lon) @ C_lon # Omega_lon\n", + " Zkk += C_lon.T @ inv(Rw_lon) @ Y[:2, i] # Psi_lon\n", + "\n", + " # Lateral sensor update\n", + " Ikk += C_lat.T @ inv(Rw_lat) @ C_lat # Omega_lat\n", + " Zkk += C_lat.T @ inv(Rw_lat) @ Y[2:, i] # Psi_lat\n", + " \n", + " # Compute the updated state and covariance \n", + " Pkk = inv(Ikk)\n", + " xkk = Pkk @ Zkk\n", + "\n", + " # Save the state estimate and covariance for later plotting\n", + " xhat[:, i], P[:, :, i] = xkkm1, Pkkm1\n", + "\n", + "# Plot the estimated errors (and compare to Kalman form)\n", + "plt.subplot(2, 1, 1)\n", + "plt.errorbar(timepts, xhat[0] - xd[0], P[0, 0], fmt='b-', **ebarstyle)\n", + "plt.plot(estim_resp.time, estim_resp.outputs[0] - xd[0], 'r--', linewidth=3)\n", + "lims = plt.axis(); plt.axis([lims[0], lims[1], -0.2, 0.2])\n", + "plt.ylabel(\"x error [m]\")\n", + "\n", + "plt.subplot(2, 1, 2)\n", + "plt.errorbar(timepts, xhat[1] - xd[1], P[1, 1], fmt='b-', **ebarstyle,\n", + " label='information filter')\n", + "plt.plot(estim_resp.time, estim_resp.outputs[1] - xd[1], 'r--', linewidth=3,\n", + " label='Kalman form')\n", + "lims = plt.axis(); plt.axis([lims[0], lims[1], -0.2, 0.2])\n", + "plt.ylabel(\"y error [m]\")\n", + "plt.xlabel(\"Time $t$ [s]\")\n", + "plt.legend(loc='lower right');" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ad5cf57f", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/cds112-L9_mhe-pvtol.ipynb b/examples/cds112-L9_mhe-pvtol.ipynb new file mode 100644 index 000000000..12793df7b --- /dev/null +++ b/examples/cds112-L9_mhe-pvtol.ipynb @@ -0,0 +1,758 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "baba5fab", + "metadata": {}, + "source": [ + "# Moving Horizon Estimation\n", + "\n", + "Richard M. Murray, 24 Feb 2023\n", + "\n", + "In this notebook we illustrate the implementation of moving horizon estimation (MHE)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "36715c5f", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "import control as ct\n", + "\n", + "import control.optimal as opt\n", + "import control.flatsys as fs" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1a4e4084", + "metadata": {}, + "outputs": [], + "source": [ + "# Import the new MHE routines (to be moved to python-control)\n", + "# import ctrlutil as opt_" + ] + }, + { + "cell_type": "markdown", + "id": "d72a155b", + "metadata": {}, + "source": [ + "## System Description\n", + "\n", + "We use the PVTOL dynamics from the textbook, which are contained in the `pvtol` module:\n", + "\n", + "\n", + "\n", + " \n", + " \n", + " \n", + "\n", + "
\n", + "$$\n", + "\\begin{aligned}\n", + " m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x, \\\\\n", + " m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - m g - c \\dot y, \\\\\n", + " J \\ddot \\theta &= r F_1.\n", + "\\end{aligned}\n", + "$$\n", + "
\n", + "\n", + "The measured values of the system are the position and orientation,\n", + "with added noise $n_x$, $n_y$, and $n_\\theta$:\n", + "\n", + "$$\n", + " \\vec y = \\begin{bmatrix} x \\\\ y \\\\ \\theta \\end{bmatrix} + \n", + " \\begin{bmatrix} n_x \\\\ n_y \\\\ n_z \\end{bmatrix}.\n", + "$$\n", + "\n", + "The parameter values for the PVTOL system come from the Caltech ducted fan experiment, described in more detail in [Lecture 4b](cds112-L4b_pvtol-lqr.ipynb)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "08919988", + "metadata": {}, + "outputs": [], + "source": [ + "# pvtol = nominal system (no disturbances or noise)\n", + "# noisy_pvtol = pvtol w/ process disturbances and sensor noise\n", + "from pvtol import pvtol, pvtol_noisy, plot_results\n", + "import pvtol as pvt\n", + "\n", + "# Find the equiblirum point corresponding to the origin\n", + "xe, ue = ct.find_eqpt(\n", + " pvtol, np.zeros(pvtol.nstates),\n", + " np.zeros(pvtol.ninputs), [0, 0, 0, 0, 0, 0],\n", + " iu=range(2, pvtol.ninputs), iy=[0, 1])\n", + "\n", + "# Initial condition = 2 meters right, 1 meter up\n", + "x0, u0 = ct.find_eqpt(\n", + " pvtol, np.zeros(pvtol.nstates),\n", + " np.zeros(pvtol.ninputs), np.array([2, 1, 0, 0, 0, 0]),\n", + " iu=range(2, pvtol.ninputs), iy=[0, 1])\n", + "\n", + "# Extract the linearization for use in LQR design\n", + "pvtol_lin = pvtol.linearize(xe, ue)\n", + "A, B = pvtol_lin.A, pvtol_lin.B\n", + "\n", + "print(pvtol, \"\\n\")\n", + "print(pvtol_noisy)" + ] + }, + { + "cell_type": "markdown", + "id": "5771ab93", + "metadata": {}, + "source": [ + "### Control Design\n", + "\n", + "We begin by designing an LQR conroller than can be used for trajectory tracking, which is described in more detail in [Lecture 4b](cds112-L4b_pvtol-lqr.ipynb):" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d2e88938", + "metadata": {}, + "outputs": [], + "source": [ + "#\n", + "# LQR design w/ physically motivated weighting\n", + "#\n", + "# Shoot for 10 cm error in x, 10 cm error in y. Try to keep the angle\n", + "# less than 5 degrees in making the adjustments. Penalize side forces\n", + "# due to loss in efficiency.\n", + "#\n", + "\n", + "Qx = np.diag([100, 10, (180/np.pi) / 5, 0, 0, 0])\n", + "Qu = np.diag([10, 1])\n", + "K, _, _ = ct.lqr(A, B, Qx, Qu)\n", + "\n", + "# Compute the full state feedback solution\n", + "lqr_ctrl, _ = ct.create_statefbk_iosystem(pvtol, K)\n", + "\n", + "# Define the closed loop system that will be used to generate trajectories\n", + "lqr_clsys = ct.interconnect(\n", + " [pvtol_noisy, lqr_ctrl],\n", + " inplist = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", + " pvtol_noisy.input_labels[pvtol.ninputs:],\n", + " inputs = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", + " pvtol_noisy.input_labels[pvtol.ninputs:],\n", + " outlist = pvtol.output_labels + lqr_ctrl.output_labels,\n", + " outputs = pvtol.output_labels + lqr_ctrl.output_labels\n", + ")\n", + "print(lqr_clsys)" + ] + }, + { + "cell_type": "markdown", + "id": "29f55c0a-8c17-4347-aa46-b1944e700b32", + "metadata": {}, + "source": [ + "(The warning message can be ignored; it is generated because we implement this system as a differentially flat system and hence we require that an output function be explicitly given, rather than using `None`.)" + ] + }, + { + "cell_type": "markdown", + "id": "e9bc481f-7b2f-4b40-89b7-1ef5a35251b7", + "metadata": {}, + "source": [ + "We next define the characteristics of the uncertainty in the system: the disturbance and noise covariances (intensities) as well as the initial condition covariance:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "78853391", + "metadata": {}, + "outputs": [], + "source": [ + "# Disturbance and noise intensities\n", + "Qv = np.diag([1e-2, 1e-2])\n", + "Qw = np.array([[1e-4, 0, 1e-5], [0, 1e-4, 1e-5], [1e-5, 1e-5, 1e-4]])\n", + "\n", + "# Initial state covariance\n", + "P0 = np.eye(pvtol.nstates)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c590fd88", + "metadata": {}, + "outputs": [], + "source": [ + "# Create the time vector for the simulation\n", + "Tf = 6\n", + "timepts = np.linspace(0, Tf, 20)\n", + "\n", + "# Create representative process disturbance and sensor noise vectors\n", + "# np.random.seed(117) # uncomment to avoid figures changing from run to run\n", + "V = ct.white_noise(timepts, Qv)\n", + "W = ct.white_noise(timepts, Qw)\n", + "plt.plot(timepts, V[0], label=\"V[0]\")\n", + "plt.plot(timepts, W[0], label=\"W[0]\")\n", + "plt.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "7db5188e-03c7-439c-8cf2-47681d3feccf", + "metadata": {}, + "source": [ + "To get a better sense of the size of the disturbances and noise, we simulate the noise-free system with the applied disturbances, and then add in the noise. Note that in this simulation we are still assuming that the controller has access to the noise-free state (not realistic, but used here just to show that the disturbances and noise do not cause large perturbations)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c35fd695", + "metadata": {}, + "outputs": [], + "source": [ + "# Desired trajectory\n", + "xd, ud = xe, ue\n", + "# xd = np.vstack([\n", + "# np.sin(2 * np.pi * timepts / timepts[-1]), \n", + "# np.zeros((5, timepts.size))])\n", + "# ud = np.outer(ue, np.ones_like(timepts))\n", + "\n", + "# Run a simulation with full state feedback (no noise) to generate a trajectory\n", + "uvec = [xd, ud, V, W*0]\n", + "lqr_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", + "U = lqr_resp.outputs[6:8] # controller input signals\n", + "Y = lqr_resp.outputs[0:3] + W # noisy output signals (noise in pvtol_noisy)\n", + "\n", + "# Compare to the no noise case\n", + "uvec = [xd, ud, V*0, W*0]\n", + "lqr0_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", + "lqr0_fine = ct.input_output_response(lqr_clsys, timepts, uvec, x0, \n", + " t_eval=np.linspace(timepts[0], timepts[-1], 100))\n", + "U0 = lqr0_resp.outputs[6:8]\n", + "Y0 = lqr0_resp.outputs[0:3]\n", + "\n", + "# Compare the results\n", + "# plt.plot(Y0[0], Y0[1], 'k--', linewidth=2, label=\"No disturbances\")\n", + "plt.plot(lqr0_fine.states[0], lqr0_fine.states[1], 'r-', label=\"Actual\")\n", + "plt.plot(Y[0], Y[1], 'b-', label=\"Noisy\")\n", + "\n", + "plt.xlabel('$x$ [m]')\n", + "plt.ylabel('$y$ [m]')\n", + "plt.axis('equal')\n", + "plt.legend(frameon=False)\n", + "\n", + "plt.figure()\n", + "plot_results(timepts, lqr_resp.states, lqr_resp.outputs[6:8])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a7f1dec6", + "metadata": {}, + "outputs": [], + "source": [ + "# Utility functions for making plots\n", + "def plot_state_comparison(\n", + " timepts, est_states, act_states=None, estimated_label='$\\\\hat x_{i}$', actual_label='$x_{i}$',\n", + " start=0):\n", + " for i in range(sys.nstates):\n", + " plt.subplot(2, 3, i+1)\n", + " if act_states is not None:\n", + " plt.plot(timepts[start:], act_states[i, start:], 'r--', \n", + " label=actual_label.format(i=i))\n", + " plt.plot(timepts[start:], est_states[i, start:], 'b', \n", + " label=estimated_label.format(i=i))\n", + " plt.legend()\n", + " plt.tight_layout()\n", + " \n", + "# Define a function to plot out all of the relevant signals\n", + "def plot_estimator_response(timepts, estimated, U, V, Y, W, start=0):\n", + " # Plot the input signal and disturbance\n", + " for i in [0, 1]:\n", + " # Input signal (the same across all)\n", + " plt.subplot(4, 3, i+1)\n", + " plt.plot(timepts[start:], U[i, start:], 'k')\n", + " plt.ylabel(f'U[{i}]')\n", + "\n", + " # Plot the estimated disturbance signal\n", + " plt.subplot(4, 3, 4+i)\n", + " plt.plot(timepts[start:], estimated.inputs[i, start:], 'b-', label=\"est\")\n", + " plt.plot(timepts[start:], V[i, start:], 'k', label=\"actual\")\n", + " plt.ylabel(f'V[{i}]')\n", + "\n", + " plt.subplot(4, 3, 6)\n", + " plt.plot(0, 0, 'b', label=\"estimated\")\n", + " plt.plot(0, 0, 'k', label=\"actual\")\n", + " plt.plot(0, 0, 'r', label=\"measured\")\n", + " plt.legend(frameon=False)\n", + " plt.grid(False)\n", + " plt.axis('off')\n", + " \n", + " # Plot the output (measured and estimated) \n", + " for i in [0, 1, 2]:\n", + " plt.subplot(4, 3, 7+i)\n", + " plt.plot(timepts[start:], Y[i, start:], 'r', label=\"measured\")\n", + " plt.plot(timepts[start:], estimated.states[i, start:], 'b', label=\"measured\")\n", + " plt.plot(timepts[start:], Y[i, start:] - W[i, start:], 'k', label=\"actual\")\n", + " plt.ylabel(f'Y[{i}]')\n", + " \n", + " for i in [0, 1, 2]:\n", + " plt.subplot(4, 3, 10+i)\n", + " plt.plot(timepts[start:], estimated.outputs[i, start:], 'b', label=\"estimated\")\n", + " plt.plot(timepts[start:], W[i, start:], 'k', label=\"actual\")\n", + " plt.ylabel(f'W[{i}]')\n", + " plt.xlabel('Time [s]')\n", + "\n", + " plt.tight_layout()" + ] + }, + { + "cell_type": "markdown", + "id": "73dd9be3", + "metadata": {}, + "source": [ + "## State Estimation\n", + "\n", + "We next consider the problem of only measuring the (noisy) outputs of the system and designing a controller that uses the estimated state as the input to the LQR controller that we designed previously.\n", + "\n", + "We start by using a standard Kalman filter." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5a1f32da", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a new system with only x, y, theta as outputs\n", + "sys = ct.nlsys(\n", + " pvt._noisy_update, lambda t, x, u, params: x[0:3], name=\"pvtol_noisy\",\n", + " states = [f'x{i}' for i in range(6)],\n", + " inputs = ['F1', 'F2'] + ['Dx', 'Dy'],\n", + " outputs = ['x', 'y', 'theta']\n", + ")\n", + "print(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3a0679f4", + "metadata": {}, + "outputs": [], + "source": [ + "# Standard Kalman filter\n", + "linsys = sys.linearize(xe, [ue, V[:, 0] * 0])\n", + "# print(linsys)\n", + "B = linsys.B[:, 0:2]\n", + "G = linsys.B[:, 2:4]\n", + "linsys = ct.ss(\n", + " linsys.A, B, linsys.C, 0,\n", + " states=sys.state_labels, inputs=sys.input_labels[0:2], outputs=sys.output_labels)\n", + "# print(linsys)\n", + "\n", + "estim = ct.create_estimator_iosystem(linsys, Qv, Qw, G=G, P0=P0)\n", + "print(estim)\n", + "print(f'{xe=}, {P0=}')\n", + "\n", + "kf_resp = ct.input_output_response(\n", + " estim, timepts, [Y, U], X0 = [xe, P0.reshape(-1)])\n", + "plot_state_comparison(timepts, kf_resp.outputs, lqr_resp.states)" + ] + }, + { + "cell_type": "markdown", + "id": "654dde1b", + "metadata": {}, + "source": [ + "### Extended Kalman filter\n", + "\n", + "We see that the standard Kalman filter does not do a good job in estimating the $y$ position (state $x_2$) nor the $y$ velocity (state $x_4$).\n", + "\n", + "A better estimate can be obtained using an extended Kalman filter, which uses the linearization of the system around the current state, rather than a fixed linearization." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1f83a335", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the disturbance input and measured output matrices\n", + "F = np.array([[0, 0], [0, 0], [0, 0], [1/pvtol.params['m'], 0], [0, 1/pvtol.params['m']], [0, 0]])\n", + "C = np.eye(3, 6)\n", + "\n", + "Qwinv = np.linalg.inv(Qw)\n", + "\n", + "# Estimator update law\n", + "def estimator_update(t, x, u, params):\n", + " # Extract the states of the estimator\n", + " xhat = x[0:pvtol.nstates]\n", + " P = x[pvtol.nstates:].reshape(pvtol.nstates, pvtol.nstates)\n", + "\n", + " # Extract the inputs to the estimator\n", + " y = u[0:3] # just grab the first three outputs\n", + " u = u[6:8] # get the inputs that were applied as well\n", + "\n", + " # Compute the linearization at the current state\n", + " A = pvtol.A(xhat, u) # A matrix depends on current state\n", + " # A = pvtol.A(xe, ue) # Fixed A matrix (for testing/comparison)\n", + " \n", + " # Compute the optimal \"gain\n", + " L = P @ C.T @ Qwinv\n", + "\n", + " # Update the state estimate\n", + " xhatdot = pvtol.updfcn(t, xhat, u, params) - L @ (C @ xhat - y)\n", + "\n", + " # Update the covariance\n", + " Pdot = A @ P + P @ A.T - P @ C.T @ Qwinv @ C @ P + F @ Qv @ F.T\n", + "\n", + " # Return the derivative\n", + " return np.hstack([xhatdot, Pdot.reshape(-1)])\n", + "\n", + "def estimator_output(t, x, u, params):\n", + " # Return the estimator states\n", + " return x[0:pvtol.nstates]\n", + "\n", + "ekf = ct.NonlinearIOSystem(\n", + " estimator_update, estimator_output,\n", + " states=pvtol.nstates + pvtol.nstates**2,\n", + " inputs= pvtol_noisy.output_labels \\\n", + " + pvtol_noisy.input_labels[0:pvtol.ninputs],\n", + " outputs=[f'xh{i}' for i in range(pvtol.nstates)]\n", + ")\n", + "print(ekf)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a4caf69b", + "metadata": {}, + "outputs": [], + "source": [ + "ekf_resp = ct.input_output_response(\n", + " ekf, timepts, [lqr_resp.states, lqr_resp.outputs[6:8]],\n", + " X0=[xe, P0.reshape(-1)])\n", + "plot_state_comparison(timepts, ekf_resp.outputs, lqr_resp.states)" + ] + }, + { + "cell_type": "markdown", + "id": "10163c6c-5634-4dbb-ba11-e20fb1e065ed", + "metadata": {}, + "source": [ + "## Maximum Likelihood Estimation\n", + "\n", + "Finally, we illustrate how to set up the problem as maximum likelihood problem, which is described in more detail in the [Optimization-Based Control](https://fbswiki.org/wiki/index.php/Supplement:_Optimization-Based_Control) (OBC) course notes, in Section 7.6.\n", + "\n", + "The basic idea in maximum likelihood estimation is to set up the estimation problem as an optimization problem where we define the likelihood of a given estimate (and the resulting noise and disturbances predicted by the\n", + "model) as a cost function." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1074908c", + "metadata": {}, + "outputs": [], + "source": [ + "# Define the optimal estimation problem\n", + "traj_cost = opt.gaussian_likelihood_cost(sys, Qv, Qw)\n", + "init_cost = lambda xhat, x: (xhat - x) @ P0 @ (xhat - x)\n", + "oep = opt.OptimalEstimationProblem(\n", + " sys, timepts, traj_cost, terminal_cost=init_cost)\n", + "\n", + "# Compute the estimate from the noisy signals\n", + "est = oep.compute_estimate(Y, U, X0=lqr_resp.states[:, 0])\n", + "plot_state_comparison(timepts, est.states, lqr_resp.states)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0c6981b9", + "metadata": {}, + "outputs": [], + "source": [ + "# Plot the response of the estimator\n", + "plot_estimator_response(timepts, est, U, V, Y, W)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "25b8aa85", + "metadata": {}, + "outputs": [], + "source": [ + "# Noise free and disturbance free => estimation should be near perfect\n", + "noisefree_cost = opt.gaussian_likelihood_cost(sys, Qv, Qw*1e-6)\n", + "oep0 = opt.OptimalEstimationProblem(\n", + " sys, timepts, noisefree_cost, terminal_cost=init_cost)\n", + "est0 = oep0.compute_estimate(Y0, U0, X0=lqr0_resp.states[:, 0],\n", + " initial_guess=(lqr0_resp.states, V * 0))\n", + "plot_state_comparison(\n", + " timepts, est0.states, lqr0_resp.states, estimated_label='$\\\\bar x_{i}$')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7a76821f", + "metadata": {}, + "outputs": [], + "source": [ + "plot_estimator_response(timepts, est0, U0, V*0, Y0, W*0)" + ] + }, + { + "cell_type": "markdown", + "id": "6b9031cf", + "metadata": {}, + "source": [ + "### Bounded disturbances\n", + "\n", + "Another situation that the maximum likelihood framework can handle is when input distributions that are bounded. We implement that here by carrying out the optimal estimation problem with constraints." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "93482470", + "metadata": {}, + "outputs": [], + "source": [ + "V_clipped = np.clip(V, -0.05, 0.05) \n", + "\n", + "plt.plot(timepts, V[0], label=\"V[0]\")\n", + "plt.plot(timepts, V_clipped[0], label=\"V[0] clipped\")\n", + "plt.plot(timepts, W[0], label=\"W[0]\")\n", + "plt.legend();" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "56e186f1", + "metadata": {}, + "outputs": [], + "source": [ + "uvec = [xe, ue, V_clipped, W]\n", + "clipped_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", + "U_clipped = clipped_resp.outputs[6:8] # controller input signals\n", + "Y_clipped = clipped_resp.outputs[0:3] + W # noisy output signals\n", + "\n", + "traj_constraint = opt.disturbance_range_constraint(\n", + " sys, [-0.05, -0.05], [0.05, 0.05])\n", + "oep_clipped = opt.OptimalEstimationProblem(\n", + " sys, timepts, traj_cost, terminal_cost=init_cost,\n", + " trajectory_constraints=traj_constraint)\n", + "\n", + "est_clipped = oep_clipped.compute_estimate(\n", + " Y_clipped, U_clipped, X0=lqr0_resp.states[:, 0])\n", + "plot_state_comparison(timepts, est_clipped.states, lqr_resp.states)\n", + "plt.suptitle(\"MHE with constraints\")\n", + "plt.tight_layout()\n", + "\n", + "plt.figure()\n", + "ekf_unclipped = ct.input_output_response(\n", + " ekf, timepts, [clipped_resp.states, clipped_resp.outputs[6:8]],\n", + " X0=[xe, P0.reshape(-1)])\n", + "\n", + "plot_state_comparison(timepts, ekf_unclipped.outputs, lqr_resp.states)\n", + "plt.suptitle(\"EKF w/out constraints\")\n", + "plt.tight_layout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "108c341a", + "metadata": {}, + "outputs": [], + "source": [ + "plot_estimator_response(timepts, est_clipped, U, V_clipped, Y, W)" + ] + }, + { + "cell_type": "markdown", + "id": "430117ce", + "metadata": {}, + "source": [ + "## Moving Horizon Estimation (MHE)\n", + "\n", + "Finally, we can now move to the implementation of a moving horizon estimator, using our fixed horizon, maximum likelihood, optimal estimator. The details of this implementation are described in more detail in the [Optimization-Based Control](https://fbswiki.org/wiki/index.php/Supplement:_Optimization-Based_Control) (OBC) course notes, in Section 7.6." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "121d67ba", + "metadata": {}, + "outputs": [], + "source": [ + "# Use a shorter horizon\n", + "mhe_timepts = timepts[0:5]\n", + "oep = opt.OptimalEstimationProblem(\n", + " sys, mhe_timepts, traj_cost, terminal_cost=init_cost)\n", + "\n", + "try:\n", + " mhe = oep.create_mhe_iosystem(2)\n", + " \n", + " est_mhe = ct.input_output_response(\n", + " mhe, timepts, [Y, U], X0=resp.states[:, 0], \n", + " params={'verbose': True}\n", + " )\n", + " plot_state_comparison(timepts, est_mhe.states, lqr_resp.states)\n", + "except:\n", + " print(\"MHE for continuous time systems not implemented\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1914ad96", + "metadata": {}, + "outputs": [], + "source": [ + "# Create discrete time version of PVTOL\n", + "Ts = 0.1\n", + "print(f\"Sample time: {Ts=}\")\n", + "dsys = ct.nlsys(\n", + " lambda t, x, u, params: x + Ts * sys.updfcn(t, x, u, params),\n", + " sys.outfcn, dt=Ts, states=sys.state_labels,\n", + " inputs=sys.input_labels, outputs=sys.output_labels,\n", + ")\n", + "print(dsys)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "11162130", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a new list of time points\n", + "timepts = np.arange(0, Tf, Ts)\n", + "\n", + "# Create representative process disturbance and sensor noise vectors\n", + "# np.random.seed(117) # avoid figures changing from run to run\n", + "V = ct.white_noise(timepts, Qv)\n", + "# V = np.clip(V0, -0.1, 0.1) # Hold for later\n", + "W = ct.white_noise(timepts, Qw, dt=Ts)\n", + "# plt.plot(timepts, V0[0], 'b--', label=\"V[0]\")\n", + "plt.plot(timepts, V[0], label=\"V[0]\")\n", + "plt.plot(timepts, W[0], label=\"W[0]\")\n", + "plt.legend();" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c8a6a693", + "metadata": {}, + "outputs": [], + "source": [ + "# Generate a new trajectory over the longer time vector\n", + "uvec = [xd, ud, V, W*0]\n", + "lqr_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", + "U = lqr_resp.outputs[6:8] # controller input signals\n", + "Y = lqr_resp.outputs[0:3] + W # noisy output signals" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d683767f", + "metadata": {}, + "outputs": [], + "source": [ + "mhe_timepts = timepts[0:10]\n", + "oep = opt.OptimalEstimationProblem(\n", + " dsys, mhe_timepts, traj_cost, terminal_cost=init_cost,\n", + " disturbance_indices=[2, 3])\n", + "mhe = oep.create_mhe_iosystem()\n", + " \n", + "mhe_resp = ct.input_output_response(\n", + " mhe, timepts, [Y, U], X0=x0, \n", + " params={'verbose': True}\n", + ")\n", + "plot_state_comparison(timepts, mhe_resp.states, lqr_resp.states)" + ] + }, + { + "cell_type": "markdown", + "id": "ad6aac39-5b55-4ffd-ab21-44385dc11ff5", + "metadata": {}, + "source": [ + "Although this estimator eventually converges to the underlying tate of the system, the initial transient response is quite poor.\n", + "\n", + "One possible explanation is that we are not starting the system at the origin, even though we are penalizing the initial state if it is away from the origin.\n", + "\n", + "To see if this matters, we shift the problem to one in which the system's initial condition is at the origin:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "bfc68072", + "metadata": {}, + "outputs": [], + "source": [ + "# Resimulate starting at the origin and moving to the \"initial\" condition\n", + "uvec = [x0, ue, V, W*0]\n", + "lqr_resp = ct.input_output_response(lqr_clsys, timepts, uvec, xe)\n", + "U = lqr_resp.outputs[6:8] # controller input signals\n", + "Y = lqr_resp.outputs[0:3] + W # noisy output signals" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "49213d04", + "metadata": {}, + "outputs": [], + "source": [ + "mhe_timepts = timepts[0:8]\n", + "oep = opt.OptimalEstimationProblem(\n", + " dsys, mhe_timepts, traj_cost, terminal_cost=init_cost,\n", + " disturbance_indices=[2, 3])\n", + "mhe = oep.create_mhe_iosystem()\n", + " \n", + "mhe_resp = ct.input_output_response(\n", + " mhe, timepts, [Y, U],\n", + " params={'verbose': True}\n", + ")\n", + "plot_state_comparison(timepts, mhe_resp.outputs, lqr_resp.states)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "650a559a", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/examples/describing_functions.ipynb b/examples/describing_functions.ipynb index fc7185901..5d433c4c6 100644 --- a/examples/describing_functions.ipynb +++ b/examples/describing_functions.ipynb @@ -232,7 +232,7 @@ "$$\n", "H(j\\omega) N(a) = -1$$\n", "\n", - "The `describing_function_plot` function plots $H(j\\omega)$ and $-1/N(a)$ and prints out the the amplitudes and frequencies corresponding to intersections of these curves. " + "The `describing_function_plot` function plots $H(j\\omega)$ and $-1/N(a)$ and prints out the amplitudes and frequencies corresponding to intersections of these curves. " ] }, { diff --git a/examples/era_msd.py b/examples/era_msd.py new file mode 100644 index 000000000..101933435 --- /dev/null +++ b/examples/era_msd.py @@ -0,0 +1,63 @@ +# era_msd.py +# Johannes Kaisinger, 4 July 2024 +# +# Demonstrate estimation of State Space model from impulse response. +# SISO, SIMO, MISO, MIMO case + +import numpy as np +import matplotlib.pyplot as plt +import os + +import control as ct + +# set up a mass spring damper system (2dof, MIMO case) +# Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. +# Figure 6.5 / Example 6.7 +# m q_dd + c q_d + k q = f +m1, k1, c1 = 1., 4., 1. +m2, k2, c2 = 2., 2., 1. +k3, c3 = 6., 2. + +A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] +]) +B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) +C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) +D = np.zeros((2,2)) + +xixo_list = ["SISO","SIMO","MISO","MIMO"] +xixo = xixo_list[3] # choose a system for estimation +match xixo: + case "SISO": + sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0]) + case "SIMO": + sys = ct.StateSpace(A, B[:,:1], C, D[:,:1]) + case "MISO": + sys = ct.StateSpace(A, B, C[:1,:], D[:1,:]) + case "MIMO": + sys = ct.StateSpace(A, B, C, D) + + +dt = 0.1 +sysd = sys.sample(dt, method='zoh') +response = ct.impulse_response(sysd) +response.plot() +plt.show() + +sysd_est, _ = ct.eigensys_realization(response,r=4,dt=dt) + +step_true = ct.step_response(sysd) +step_true.sysname="H_true" +step_est = ct.step_response(sysd_est) +step_est.sysname="H_est" + +step_true.plot(title=xixo) +step_est.plot(color='orange',linestyle='dashed') + +plt.show() + +if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + plt.show() \ No newline at end of file diff --git a/examples/kincar.py b/examples/kincar.py new file mode 100644 index 000000000..a12cdc774 --- /dev/null +++ b/examples/kincar.py @@ -0,0 +1,113 @@ +# kincar.py - planar vehicle model (with flatness) +# RMM, 16 Jan 2022 + +import numpy as np +import matplotlib.pyplot as plt +import control as ct +import control.flatsys as fs + +# +# Vehicle dynamics (bicycle model) +# + +# Function to take states, inputs and return the flat flag +def _kincar_flat_forward(x, u, params={}): + # Get the parameter values + b = params.get('wheelbase', 3.) + #! TODO: add dir processing + + # Create a list of arrays to store the flat output and its derivatives + zflag = [np.zeros(3), np.zeros(3)] + + # Flat output is the x, y position of the rear wheels + zflag[0][0] = x[0] + zflag[1][0] = x[1] + + # First derivatives of the flat output + zflag[0][1] = u[0] * np.cos(x[2]) # dx/dt + zflag[1][1] = u[0] * np.sin(x[2]) # dy/dt + + # First derivative of the angle + thdot = (u[0]/b) * np.tan(u[1]) + + # Second derivatives of the flat output (setting vdot = 0) + zflag[0][2] = -u[0] * thdot * np.sin(x[2]) + zflag[1][2] = u[0] * thdot * np.cos(x[2]) + + return zflag + +# Function to take the flat flag and return states, inputs +def _kincar_flat_reverse(zflag, params={}): + # Get the parameter values + b = params.get('wheelbase', 3.) + dir = params.get('dir', 'f') + + # Create a vector to store the state and inputs + x = np.zeros(3) + u = np.zeros(2) + + # Given the flat variables, solve for the state + x[0] = zflag[0][0] # x position + x[1] = zflag[1][0] # y position + if dir == 'f': + x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot + elif dir == 'r': + # Angle is flipped by 180 degrees (since v < 0) + x[2] = np.arctan2(-zflag[1][1], -zflag[0][1]) + else: + raise ValueError("unknown direction:", dir) + + # And next solve for the inputs + u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2]) + thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2]) + u[1] = np.arctan2(thdot_v, u[0]**2 / b) + + return x, u + +# Function to compute the RHS of the system dynamics +def _kincar_update(t, x, u, params): + b = params.get('wheelbase', 3.) # get parameter values + #! TODO: add dir processing + dx = np.array([ + np.cos(x[2]) * u[0], + np.sin(x[2]) * u[0], + (u[0]/b) * np.tan(u[1]) + ]) + return dx + +def _kincar_output(t, x, u, params): + return x # return x, y, theta (full state) + +# Create differentially flat input/output system +kincar = fs.FlatSystem( + _kincar_flat_forward, _kincar_flat_reverse, name="kincar", + updfcn=_kincar_update, outfcn=_kincar_output, + inputs=('v', 'delta'), outputs=('x', 'y', 'theta'), + states=('x', 'y', 'theta')) + +# +# Utility function to plot lane change maneuver +# + +def plot_lanechange(t, y, u, figure=None, yf=None): + # Plot the xy trajectory + plt.subplot(3, 1, 1, label='xy') + plt.plot(y[0], y[1]) + plt.xlabel("x [m]") + plt.ylabel("y [m]") + if yf is not None: + plt.plot(yf[0], yf[1], 'ro') + + # Plot the inputs as a function of time + plt.subplot(3, 1, 2, label='v') + plt.plot(t, u[0]) + plt.xlabel("Time $t$ [sec]") + plt.ylabel("$v$ [m/s]") + + plt.subplot(3, 1, 3, label='delta') + plt.plot(t, u[1]) + plt.xlabel("Time $t$ [sec]") + plt.ylabel("$\\delta$ [rad]") + + plt.suptitle("Lane change maneuver") + plt.tight_layout() diff --git a/examples/markov.py b/examples/markov.py new file mode 100644 index 000000000..6c02499bd --- /dev/null +++ b/examples/markov.py @@ -0,0 +1,108 @@ +# markov.py +# Johannes Kaisinger, 4 July 2024 +# +# Demonstrate estimation of markov parameters. +# SISO, SIMO, MISO, MIMO case + +import numpy as np +import matplotlib.pyplot as plt +import os + +import control as ct + +def create_impulse_response(H, time, transpose, dt): + """Helper function to use TimeResponseData type for plotting""" + + H = np.array(H, ndmin=3) + + if transpose: + H = np.transpose(H) + + q, p, m = H.shape + inputs = np.zeros((p,p,m)) + + issiso = True if (q == 1 and p == 1) else False + + input_labels = [] + trace_labels, trace_types = [], [] + for i in range(p): + inputs[i,i,0] = 1/dt # unit area impulse + input_labels.append(f"u{[i]}") + trace_labels.append(f"From u{[i]}") + trace_types.append('impulse') + + output_labels = [] + for i in range(q): + output_labels.append(f"y{[i]}") + + return ct.TimeResponseData(time=time[:m], + outputs=H, + output_labels=output_labels, + inputs=inputs, + input_labels=input_labels, + trace_labels=trace_labels, + trace_types=trace_types, + sysname="H_est", + transpose=transpose, + plot_inputs=False, + issiso=issiso) + +# set up a mass spring damper system (2dof, MIMO case) +# Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. +# Figure 6.5 / Example 6.7 +# m q_dd + c q_d + k q = f +m1, k1, c1 = 1., 4., 1. +m2, k2, c2 = 2., 2., 1. +k3, c3 = 6., 2. + +A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] +]) +B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) +C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) +D = np.zeros((2,2)) + + +xixo_list = ["SISO","SIMO","MISO","MIMO"] +xixo = xixo_list[3] # choose a system for estimation +match xixo: + case "SISO": + sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0]) + case "SIMO": + sys = ct.StateSpace(A, B[:,:1], C, D[:,:1]) + case "MISO": + sys = ct.StateSpace(A, B, C[:1,:], D[:1,:]) + case "MIMO": + sys = ct.StateSpace(A, B, C, D) + +dt = 0.25 +sysd = sys.sample(dt, method='zoh') +sysd.name = "H_true" + + # random forcing input +t = np.arange(0,100,dt) +u = np.random.randn(sysd.B.shape[-1], len(t)) + +response = ct.forced_response(sysd, U=u) +response.plot() +plt.show() + +m = 50 +ir_true = ct.impulse_response(sysd, T=dt*m) + +H_est = ct.markov(response, m, dt=dt) +# Helper function for plotting only +ir_est = create_impulse_response(H_est, + ir_true.time, + ir_true.transpose, + dt) + +ir_true.plot(title=xixo) +ir_est.plot(color='orange',linestyle='dashed') +plt.show() + +if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + plt.show() \ No newline at end of file diff --git a/examples/plot_gallery.py b/examples/plot_gallery.py new file mode 100644 index 000000000..d7a700c91 --- /dev/null +++ b/examples/plot_gallery.py @@ -0,0 +1,163 @@ +# plot_gallery.py - different types of plots for comparing versions +# RMM, 19 Jun 2024 +# +# This file collects together some of the more interesting plots that can +# be generated by python-control and puts them into a PDF file that can be +# used to compare what things look like between different versions of the +# library. It is mainly intended for uses by developers to make sure there +# are no unexpected changes in plot formats, but also has some interest +# examples of things you can plot. + +import os +import sys +from math import pi + +import matplotlib.pyplot as plt +import numpy as np + +import control as ct + +# Don't save figures if we are running CI tests +savefigs = 'PYCONTROL_TEST_EXAMPLES' not in os.environ +if savefigs: + # Create a pdf file for storing the results + import subprocess + from matplotlib.backends.backend_pdf import PdfPages + from datetime import date + git_info = subprocess.check_output(['git', 'describe'], text=True).strip() + pdf = PdfPages( + f'plot_gallery-{git_info}-{date.today().isoformat()}.pdf') + +# Context manager to handle plotting +class create_figure(object): + def __init__(self, name): + self.name = name + def __enter__(self): + self.fig = plt.figure() + print(f"Generating {self.name} as Figure {self.fig.number}") + return self.fig + def __exit__(self, type, value, traceback): + if type is not None: + print(f"Exception: {type=}, {value=}, {traceback=}") + if savefigs: + pdf.savefig() + if hasattr(sys, 'ps1'): + # Show the figures on the screen + plt.show(block=False) + else: + plt.close() + +# Define systems to use throughout +sys1 = ct.tf([1], [1, 2, 1], name='sys1') +sys2 = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys2') +sys_mimo1 = ct.tf2ss( + [[[1], [0.1]], [[0.2], [1]]], + [[[1, 0.6, 1], [1, 1, 1]], [[1, 0.4, 1], [1, 2, 1]]], name="sys_mimo1") +sys_mimo2 = ct.tf2ss( + [[[1], [0.1]], [[0.2], [1]]], + [[[1, 0.2, 1], [1, 24, 22, 5]], [[1, 4, 16, 21], [1, 0.1]]], + name="sys_mimo2") +sys_frd = ct.frd( + [[np.array([10 + 0j, 5 - 5j, 1 - 1j, 0.5 - 1j, -.1j]), + np.array([1j, 0.5 - 0.5j, -0.5, 0.1 - 0.1j, -.05j]) * 0.1], + [np.array([10 + 0j, -20j, -10, 2j, 1]), + np.array([10 + 0j, 5 - 5j, 1 - 1j, 0.5 - 1j, -.1j]) * 0.01]], + np.logspace(-2, 2, 5)) +sys_frd.name = 'frd' # For backward compatibility + +# Close all existing figures +plt.close('all') + +# bode +with create_figure("Bode plot"): + try: + ct.bode_plot([sys_mimo1, sys_mimo2]) + except AttributeError: + print(" - falling back to earlier method") + plt.clf() + ct.bode_plot(sys_mimo1) + ct.bode_plot(sys_mimo2) + +# describing function +with create_figure("Describing function plot"): + H = ct.tf([1], [1, 2, 2, 1]) * 8 + F = ct.descfcn.saturation_nonlinearity(1) + amp = np.linspace(1, 4, 10) + ct.describing_function_response(H, F, amp).plot() + +# nichols +with create_figure("Nichols chart"): + response = ct.frequency_response([sys1, sys2]) + ct.nichols_plot(response) + +# nyquist +with create_figure("Nyquist plot"): + ct.nyquist_plot([sys1, sys2]) + +# phase plane +with create_figure("Phase plane plot"): + def invpend_update(t, x, u, params): + m, l, b, g = params['m'], params['l'], params['b'], params['g'] + return [x[1], -b/m * x[1] + (g * l / m) * np.sin(x[0]) + u[0]/m] + invpend = ct.nlsys(invpend_update, states=2, inputs=1, name='invpend') + ct.phase_plane_plot( + invpend, [-2*pi, 2*pi, -2, 2], 5, + gridtype='meshgrid', gridspec=[5, 8], arrows=3, + plot_separatrices={'gridspec': [12, 9]}, + params={'m': 1, 'l': 1, 'b': 0.2, 'g': 1}) + +# pole zero map +with create_figure("Pole/zero map"): + T = ct.tf( + [-9.0250000e-01, -4.7200750e+01, -8.6812900e+02, + +5.6261850e+03, +2.1258472e+05, +8.4724600e+05, + +1.0192000e+06, +2.3520000e+05], + [9.02500000e-03, 9.92862812e-01, 4.96974094e+01, + 1.35705659e+03, 2.09294163e+04, 1.64898435e+05, + 6.54572220e+05, 1.25274600e+06, 1.02420000e+06, + 2.35200000e+05], name='T') + ct.pole_zero_plot([T, sys2]) + +# root locus +with create_figure("Root locus plot") as fig: + ax_array = ct.pole_zero_subplots(2, 1, grid=[True, False]) + ax1, ax2 = ax_array[:, 0] + sys1 = ct.tf([1, 2], [1, 2, 3], name='sys1') + sys2 = ct.tf([1, 0.2], [1, 1, 3, 1, 1], name='sys2') + ct.root_locus_plot([sys1, sys2], ax=ax1) + ct.root_locus_plot([sys1, sys2], ax=ax2) + plt.suptitle("Root locus plots (w/ specified axes)", fontsize='medium') + +# sisotool +with create_figure("sisotool"): + s = ct.tf('s') + H = (s+0.3)/(s**4 + 4*s**3 + 6.25*s**2) + ct.sisotool(H) + +# step response +with create_figure("step response") as fig: + try: + ct.step_response([sys_mimo1, sys_mimo2]).plot() + except ValueError: + print(" - falling back to earlier method") + fig.clf() + ct.step_response(sys_mimo1).plot() + ct.step_response(sys_mimo2).plot() + +# time response +with create_figure("time response"): + timepts = np.linspace(0, 10) + + U = np.vstack([np.sin(timepts), np.cos(2*timepts)]) + resp1 = ct.input_output_response(sys_mimo1, timepts, U) + + U = np.vstack([np.cos(2*timepts), np.sin(timepts)]) + resp2 = ct.input_output_response(sys_mimo1, timepts, U) + + resp = ct.combine_time_responses( + [resp1, resp2], trace_labels=["resp1", "resp2"]) + resp.plot(transpose=True) + +# Show the figures if running in interactive mode +if savefigs: + pdf.close() diff --git a/examples/pvtol-nested.py b/examples/pvtol-nested.py index 040b4a1f4..eeb46d075 100644 --- a/examples/pvtol-nested.py +++ b/examples/pvtol-nested.py @@ -61,8 +61,6 @@ Hi = ct.parallel(ct.feedback(Ci, Pi), -m * g *ct.feedback(Ci * Pi, 1)) plt.figure(4) -plt.clf() -plt.subplot(221) ct.bode_plot(Hi) # Now design the lateral control system @@ -129,7 +127,7 @@ # plt.figure(7) plt.clf() -ct.nyquist_plot(L, (0.0001, 1000)) +ct.nyquist_plot(L) # Add a box in the region we are going to expand plt.plot([-2, -2, 1, 1, -2], [-4, 4, 4, -4, -4], 'r-') diff --git a/examples/springmass-coupled.png b/examples/springmass-coupled.png new file mode 100644 index 000000000..bc898da09 Binary files /dev/null and b/examples/springmass-coupled.png differ diff --git a/examples/steering-gainsched.py b/examples/steering-gainsched.py index 88eed9a95..85e8d8bda 100644 --- a/examples/steering-gainsched.py +++ b/examples/steering-gainsched.py @@ -222,7 +222,7 @@ def trajgen_output(t, x, u, params): vehicle, (gains, points), name='controller', ud_labels=['vd', 'phid'], gainsched_indices=['vd', 'theta'], gainsched_method='linear') -# Connect everything together (note that controller inputs are different +# Connect everything together (note that controller inputs are different) steering = ct.interconnect( # List of subsystems (trajgen, controller, vehicle), name='steering', @@ -235,7 +235,7 @@ def trajgen_output(t, x, u, params): ['controller.x', 'vehicle.x'], ['controller.y', 'vehicle.y'], ['controller.theta', 'vehicle.theta'], - ['controller.vd', ('trajgen', 'vd', 0.2)], # create error + ['controller.vd', ('trajgen', 'vd', 0.2)], # create some error ['controller.phid', 'trajgen.phid'], ['vehicle.v', 'controller.v'], ['vehicle.phi', 'controller.phi'] diff --git a/examples/steering-optimal.py b/examples/steering-optimal.py index d9bad608e..80ad671f6 100644 --- a/examples/steering-optimal.py +++ b/examples/steering-optimal.py @@ -79,14 +79,14 @@ def plot_lanechange(t, y, u, yf=None, figure=None): plt.xlabel("t [sec]") plt.ylabel("steering [rad/s]") - plt.suptitle("Lane change manuever") + plt.suptitle("Lane change maneuver") plt.tight_layout() plt.show(block=False) # # Optimal control problem # -# Perform a "lane change" manuever over the course of 10 seconds. +# Perform a "lane change" maneuver over the course of 10 seconds. # # Initial and final conditions diff --git a/examples/steering.ipynb b/examples/steering.ipynb index 217e3b2db..ebad51185 100644 --- a/examples/steering.ipynb +++ b/examples/steering.ipynb @@ -90,9 +90,7 @@ { "cell_type": "code", "execution_count": 3, - "metadata": { - "scrolled": false - }, + "metadata": {}, "outputs": [ { "data": { @@ -452,9 +450,7 @@ { "cell_type": "code", "execution_count": 8, - "metadata": { - "scrolled": false - }, + "metadata": {}, "outputs": [ { "name": "stdout", @@ -1067,7 +1063,7 @@ ], "metadata": { "kernelspec": { - "display_name": "Python 3", + "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, @@ -1081,9 +1077,9 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.9.1" + "version": "3.12.2" } }, "nbformat": 4, - "nbformat_minor": 2 + "nbformat_minor": 4 } diff --git a/examples/tfvis.py b/examples/tfvis.py index 0cb789db4..c9e9872de 100644 --- a/examples/tfvis.py +++ b/examples/tfvis.py @@ -45,13 +45,8 @@ import Pmw import matplotlib.pyplot as plt from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg -from numpy.lib.polynomial import polymul -from numpy.lib.type_check import real -from numpy.core.multiarray import array -from numpy.core.fromnumeric import size -# from numpy.lib.function_base import logspace from control.matlab import logspace -from numpy import conj +from numpy import array, conj, polymul, real, size def make_poly(facts): diff --git a/examples/vehicle.py b/examples/vehicle.py index b316ceced..07af35c9f 100644 --- a/examples/vehicle.py +++ b/examples/vehicle.py @@ -84,7 +84,7 @@ def _vehicle_output(t, x, u, params): states=('x', 'y', 'theta')) # -# Utility function to plot lane change manuever +# Utility function to plot lane change maneuver # def plot_lanechange(t, y, u, figure=None, yf=None): @@ -107,5 +107,5 @@ def plot_lanechange(t, y, u, figure=None, yf=None): plt.xlabel("t [sec]") plt.ylabel("steering [rad/s]") - plt.suptitle("Lane change manuever") + plt.suptitle("Lane change maneuver") plt.tight_layout() diff --git a/pyproject.toml b/pyproject.toml index f3df75f1d..649dcad5d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -40,7 +40,7 @@ dynamic = ["version"] packages = ["control"] [project.optional-dependencies] -test = ["pytest", "pytest-timeout"] +test = ["pytest", "pytest-timeout", "ruff"] slycot = [ "slycot>=0.4.0" ] cvxopt = [ "cvxopt>=1.2.0" ] @@ -56,3 +56,9 @@ addopts = "-ra" filterwarnings = [ "error:.*matrix subclass:PendingDeprecationWarning", ] + +[tool.ruff.lint] +select = ['D', 'E', 'W', 'DOC'] + +[tool.ruff.lint.pydocstyle] +convention = 'numpy'