Merge pull request #531 from sawyerbfuller/sisotooling

bnavigator · web-flow · commit 3adc610a1ad7 · 2021-02-07T14:25:08.000+01:00
sisotool small visual cleanup, new feature to show step response of different input-output than loop
diff --git a/control/bdalg.py b/control/bdalg.py
@@ -334,7 +334,8 @@ def connect(sys, Q, inputv, outputv):
     interconnecting multiple systems.
 
     """
-    inputv, outputv, Q = np.asarray(inputv), np.asarray(outputv), np.asarray(Q)
+    inputv, outputv, Q = \
+        np.atleast_1d(inputv), np.atleast_1d(outputv), np.atleast_1d(Q)
     # check indices
     index_errors = (inputv - 1 > sys.ninputs) | (inputv < 1)
     if np.any(index_errors):
diff --git a/control/rlocus.py b/control/rlocus.py
@@ -137,8 +137,10 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None,
     print_gain = config._get_param(
         'rlocus', 'print_gain', print_gain, _rlocus_defaults)
 
+    sys_loop = sys if sys.issiso() else sys[0,0]
+
     # Convert numerator and denominator to polynomials if they aren't
-    (nump, denp) = _systopoly1d(sys)
+    (nump, denp) = _systopoly1d(sys_loop)
 
     # if discrete-time system and if xlim and ylim are not given,
     #  that we a view of the unit circle
@@ -241,12 +243,12 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None,
             else:
                 _sgrid_func()
         else:
-            ax.axhline(0., linestyle=':', color='k', zorder=-20)
-            ax.axvline(0., linestyle=':', color='k', zorder=-20)
+            ax.axhline(0., linestyle=':', color='k', linewidth=.75, zorder=-20)
+            ax.axvline(0., linestyle=':', color='k', linewidth=.75, zorder=-20)
             if isdtime(sys, strict=True):
                 ax.add_patch(plt.Circle(
                     (0, 0), radius=1.0, linestyle=':', edgecolor='k',
-                    linewidth=1.5, fill=False, zorder=-20))
+                    linewidth=0.75, fill=False, zorder=-20))
 
     return mymat, kvect
 
@@ -540,8 +542,9 @@ def _RLSortRoots(mymat):
 
 def _RLZoomDispatcher(event, sys, ax_rlocus, plotstr):
     """Rootlocus plot zoom dispatcher"""
+    sys_loop = sys if sys.issiso() else sys[0,0]
 
-    nump, denp = _systopoly1d(sys)
+    nump, denp = _systopoly1d(sys_loop)
     xlim, ylim = ax_rlocus.get_xlim(), ax_rlocus.get_ylim()
 
     kvect, mymat, xlim, ylim = _default_gains(
@@ -573,21 +576,23 @@ def _RLClickDispatcher(event, sys, fig, ax_rlocus, plotstr, sisotool=False,
 
 def _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=False):
     """Display root-locus gain feedback point for clicks on root-locus plot"""
-    (nump, denp) = _systopoly1d(sys)
+    sys_loop = sys if sys.issiso() else sys[0,0]
+
+    (nump, denp) = _systopoly1d(sys_loop)
 
     xlim = ax_rlocus.get_xlim()
     ylim = ax_rlocus.get_ylim()
-    x_tolerance = 0.05 * abs((xlim[1] - xlim[0]))
-    y_tolerance = 0.05 * abs((ylim[1] - ylim[0]))
+    x_tolerance = 0.1 * abs((xlim[1] - xlim[0]))
+    y_tolerance = 0.1 * abs((ylim[1] - ylim[0]))
     gain_tolerance = np.mean([x_tolerance, y_tolerance])*0.1
 
     # Catch type error when event click is in the figure but not in an axis
     try:
         s = complex(event.xdata, event.ydata)
-        K = -1. / sys(s)
-        K_xlim = -1. / sys(
+        K = -1. / sys_loop(s)
+        K_xlim = -1. / sys_loop(
             complex(event.xdata + 0.05 * abs(xlim[1] - xlim[0]), event.ydata))
-        K_ylim = -1. / sys(
+        K_ylim = -1. / sys_loop(
             complex(event.xdata, event.ydata + 0.05 * abs(ylim[1] - ylim[0])))
 
     except TypeError:
diff --git a/control/sisotool.py b/control/sisotool.py
@@ -1,17 +1,19 @@
 __all__ = ['sisotool']
 
+from control.exception import ControlMIMONotImplemented
 from .freqplot import bode_plot
 from .timeresp import step_response
 from .lti import issiso, isdtime
+from .xferfcn import TransferFunction
+from .bdalg import append, connect
 import matplotlib
 import matplotlib.pyplot as plt
 import warnings
 
-def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
-             plotstr_rlocus = 'b' if int(matplotlib.__version__[0]) == 1 else 'C0',
-             rlocus_grid = False, omega = None, dB = None, Hz = None,
-             deg = None, omega_limits = None, omega_num = None,
-             margins_bode = True, tvect=None):
+def sisotool(sys, kvect=None, xlim_rlocus=None, ylim_rlocus=None,
+             plotstr_rlocus='C0', rlocus_grid=False, omega=None, dB=None,
+             Hz=None, deg=None, omega_limits=None, omega_num=None,
+             margins_bode=True, tvect=None):
     """
     Sisotool style collection of plots inspired by MATLAB's sisotool.
     The left two plots contain the bode magnitude and phase diagrams.
@@ -22,7 +24,16 @@ def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
     Parameters
     ----------
     sys : LTI object
-        Linear input/output systems (SISO only)
+        Linear input/output systems. If sys is SISO, use the same
+        system for the root locus and step response. If it is desired to
+        see a different step response than feedback(K*loop,1), sys can be
+        provided as a two-input, two-output system (e.g. by using
+        :func:`bdgalg.connect' or :func:`iosys.interconnect`). Sisotool
+        inserts the negative of the selected gain K between the first output
+        and first input and uses the second input and output for computing
+        the step response. This allows you to see the step responses of more
+        complex systems, for example, systems with a feedforward path into the
+        plant or in which the gain appears in the feedback path.
     kvect : list or ndarray, optional
         List of gains to use for plotting root locus
     xlim_rlocus : tuple or list, optional
@@ -32,21 +43,23 @@ def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
         control of y-axis range
     plotstr_rlocus : :func:`matplotlib.pyplot.plot` format string, optional
         plotting style for the root locus plot(color, linestyle, etc)
-    rlocus_grid: boolean (default = False)
-        If True plot s-plane grid.
-    omega : freq_range
-        Range of frequencies in rad/sec for the bode plot
+    rlocus_grid : boolean (default = False)
+        If True plot s- or z-plane grid.
+    omega : array_like
+        List of frequencies in rad/sec to be used for bode plot
     dB : boolean
         If True, plot result in dB for the bode plot
     Hz : boolean
         If True, plot frequency in Hz for the bode plot (omega must be provided in rad/sec)
     deg : boolean
         If True, plot phase in degrees for the bode plot (else radians)
-    omega_limits: tuple, list, ... of two values
+    omega_limits : array_like of two values
         Limits of the to generate frequency vector.
-        If Hz=True the limits are in Hz otherwise in rad/s.
-    omega_num: int
-        number of samples
+        If Hz=True the limits are in Hz otherwise in rad/s. Ignored if omega
+        is provided, and auto-generated if omitted.
+    omega_num : int
+        Number of samples to plot.  Defaults to
+        config.defaults['freqplot.number_of_samples'].
     margins_bode : boolean
         If True, plot gain and phase margin in the bode plot
     tvect : list or ndarray, optional
@@ -60,8 +73,11 @@ def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
     """
     from .rlocus import root_locus
 
-    # Check if it is a single SISO system
-    issiso(sys,strict=True)
+    # sys as loop transfer function if SISO
+    if not sys.issiso():
+        if not (sys.ninputs == 2 and sys.noutputs == 2):
+            raise ControlMIMONotImplemented(
+                'sys must be SISO or 2-input, 2-output')
 
     # Setup sisotool figure or superimpose if one is already present
     fig = plt.gcf()
@@ -84,64 +100,74 @@ def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
     }
 
     # First time call to setup the bode and step response plots
-    _SisotoolUpdate(sys, fig,1 if kvect is None else kvect[0],bode_plot_params)
+    _SisotoolUpdate(sys, fig,
+        1 if kvect is None else kvect[0], bode_plot_params)
 
     # Setup the root-locus plot window
-    root_locus(sys,kvect=kvect,xlim=xlim_rlocus,ylim = ylim_rlocus,plotstr=plotstr_rlocus,grid = rlocus_grid,fig=fig,bode_plot_params=bode_plot_params,tvect=tvect,sisotool=True)
+    root_locus(sys, kvect=kvect, xlim=xlim_rlocus,
+        ylim=ylim_rlocus, plotstr=plotstr_rlocus, grid=rlocus_grid,
+        fig=fig, bode_plot_params=bode_plot_params, tvect=tvect, sisotool=True)
 
-def _SisotoolUpdate(sys,fig,K,bode_plot_params,tvect=None):
+def _SisotoolUpdate(sys, fig, K, bode_plot_params, tvect=None):
 
-    if int(matplotlib.__version__[0]) == 1:
-        title_font_size = 12
-        label_font_size = 10
-    else:
-        title_font_size = 10
-        label_font_size = 8
+    title_font_size = 10
+    label_font_size = 8
 
     # Get the subaxes and clear them
-    ax_mag,ax_rlocus,ax_phase,ax_step = fig.axes[0],fig.axes[1],fig.axes[2],fig.axes[3]
+    ax_mag, ax_rlocus, ax_phase, ax_step = \
+        fig.axes[0], fig.axes[1], fig.axes[2], fig.axes[3]
 
     # Catch matplotlib 2.1.x and higher userwarnings when clearing a log axis
     with warnings.catch_warnings():
         warnings.simplefilter("ignore")
         ax_step.clear(), ax_mag.clear(), ax_phase.clear()
 
+    sys_loop = sys if sys.issiso() else sys[0,0]
+
     # Update the bodeplot
-    bode_plot_params['syslist'] = sys*K.real
+    bode_plot_params['syslist'] = sys_loop*K.real
     bode_plot(**bode_plot_params)
 
     # Set the titles and labels
     ax_mag.set_title('Bode magnitude',fontsize = title_font_size)
     ax_mag.set_ylabel(ax_mag.get_ylabel(), fontsize=label_font_size)
+    ax_mag.tick_params(axis='both', which='major', labelsize=label_font_size)
 
     ax_phase.set_title('Bode phase',fontsize=title_font_size)
     ax_phase.set_xlabel(ax_phase.get_xlabel(),fontsize=label_font_size)
     ax_phase.set_ylabel(ax_phase.get_ylabel(),fontsize=label_font_size)
     ax_phase.get_xaxis().set_label_coords(0.5, -0.15)
     ax_phase.get_shared_x_axes().join(ax_phase, ax_mag)
+    ax_phase.tick_params(axis='both', which='major', labelsize=label_font_size)
 
     ax_step.set_title('Step response',fontsize = title_font_size)
     ax_step.set_xlabel('Time (seconds)',fontsize=label_font_size)
-    ax_step.set_ylabel('Amplitude',fontsize=label_font_size)
+    ax_step.set_ylabel('Output',fontsize=label_font_size)
     ax_step.get_xaxis().set_label_coords(0.5, -0.15)
     ax_step.get_yaxis().set_label_coords(-0.15, 0.5)
+    ax_step.tick_params(axis='both', which='major', labelsize=label_font_size)
 
     ax_rlocus.set_title('Root locus',fontsize = title_font_size)
     ax_rlocus.set_ylabel('Imag', fontsize=label_font_size)
     ax_rlocus.set_xlabel('Real', fontsize=label_font_size)
     ax_rlocus.get_xaxis().set_label_coords(0.5, -0.15)
     ax_rlocus.get_yaxis().set_label_coords(-0.15, 0.5)
-
-
+    ax_rlocus.tick_params(axis='both', which='major',labelsize=label_font_size)
 
     # Generate the step response and plot it
-    sys_closed = (K*sys).feedback(1)
+    if sys.issiso():
+        sys_closed = (K*sys).feedback(1)
+    else:
+        sys_closed = append(sys, -K)
+        connects = [[1, 3],
+                    [3, 1]]
+        sys_closed = connect(sys_closed, connects, 2, 2)
     if tvect is None:
         tvect, yout = step_response(sys_closed, T_num=100)
     else:
-        tvect, yout = step_response(sys_closed,tvect)
+        tvect, yout = step_response(sys_closed, tvect)
     if isdtime(sys_closed, strict=True):
-        ax_step.plot(tvect, yout, 'o')
+        ax_step.plot(tvect, yout, '.')
     else:
         ax_step.plot(tvect, yout)
     ax_step.axhline(1.,linestyle=':',color='k',zorder=-20)
diff --git a/control/tests/sisotool_test.py b/control/tests/sisotool_test.py
@@ -1,5 +1,6 @@
 """sisotool_test.py"""
 
+from control.exception import ControlMIMONotImplemented
 import matplotlib.pyplot as plt
 import numpy as np
 from numpy.testing import assert_array_almost_equal
@@ -8,6 +9,7 @@
 from control.sisotool import sisotool
 from control.rlocus import _RLClickDispatcher
 from control.xferfcn import TransferFunction
+from control.statesp import StateSpace
 
 
 @pytest.mark.usefixtures("mplcleanup")
@@ -19,6 +21,35 @@ def sys(self):
         """Return a generic SISO transfer function"""
         return TransferFunction([1000], [1, 25, 100, 0])
 
+    @pytest.fixture
+    def sysdt(self):
+        """Return a generic SISO transfer function"""
+        return TransferFunction([1000], [1, 25, 100, 0], True)
+
+    @pytest.fixture
+    def sys222(self):
+        """2-states square system (2 inputs x 2 outputs)"""
+        A222 = [[4., 1.],
+                [2., -3]]
+        B222 = [[5., 2.],
+                [-3., -3.]]
+        C222 = [[2., -4],
+                [0., 1.]]
+        D222 = [[3., 2.],
+                [1., -1.]]
+        return StateSpace(A222, B222, C222, D222)
+
+    @pytest.fixture
+    def sys221(self):
+        """2-states, 2 inputs x 1 output"""
+        A222 = [[4., 1.],
+                [2., -3]]
+        B222 = [[5., 2.],
+                [-3., -3.]]
+        C221 = [[0., 1.]]
+        D221 = [[1., -1.]]
+        return StateSpace(A222, B222, C221, D221)
+
     def test_sisotool(self, sys):
         sisotool(sys, Hz=False)
         fig = plt.gcf()
@@ -27,7 +58,7 @@ def test_sisotool(self, sys):
         # Check the initial root locus plot points
         initial_point_0 = (np.array([-22.53155977]), np.array([0.]))
         initial_point_1 = (np.array([-1.23422011]), np.array([-6.54667031]))
-        initial_point_2 = (np.array([-1.23422011]), np.array([06.54667031]))
+        initial_point_2 = (np.array([-1.23422011]), np.array([6.54667031]))
         assert_array_almost_equal(ax_rlocus.lines[0].get_data(),
                                   initial_point_0, 4)
         assert_array_almost_equal(ax_rlocus.lines[1].get_data(),
@@ -88,7 +119,49 @@ def test_sisotool(self, sys):
         step_response_moved = np.array(
             [0., 0.0072, 0.0516, 0.1554, 0.3281, 0.5681, 0.8646, 1.1987,
              1.5452, 1.875])
-        # old: array([0., 0.0239, 0.161 , 0.4547, 0.8903, 1.407,
-        #             1.9121, 2.2989, 2.4686, 2.353])
         assert_array_almost_equal(
             ax_step.lines[0].get_data()[1][:10], step_response_moved, 4)
+
+    def test_sisotool_tvect(self, sys):
+        # test supply tvect
+        tvect = np.linspace(0, 1, 10)
+        sisotool(sys, tvect=tvect)
+        fig = plt.gcf()
+        ax_rlocus, ax_step = fig.axes[1], fig.axes[3]
+
+        # Move the rootlocus to another point and confirm same tvect
+        event = type('test', (object,), {'xdata': 2.31206868287,
+                                         'ydata': 15.5983051046,
+                                         'inaxes': ax_rlocus.axes})()
+        _RLClickDispatcher(event=event, sys=sys, fig=fig,
+                           ax_rlocus=ax_rlocus, sisotool=True, plotstr='-',
+                           bode_plot_params=dict(), tvect=tvect)
+        assert_array_almost_equal(tvect, ax_step.lines[0].get_data()[0])
+
+    def test_sisotool_tvect_dt(self, sysdt):
+        # test supply tvect
+        tvect = np.linspace(0, 1, 10)
+        sisotool(sysdt, tvect=tvect)
+        fig = plt.gcf()
+        ax_rlocus, ax_step = fig.axes[1], fig.axes[3]
+
+        # Move the rootlocus to another point and confirm same tvect
+        event = type('test', (object,), {'xdata': 2.31206868287,
+                                         'ydata': 15.5983051046,
+                                         'inaxes': ax_rlocus.axes})()
+        _RLClickDispatcher(event=event, sys=sysdt, fig=fig,
+                           ax_rlocus=ax_rlocus, sisotool=True, plotstr='-',
+                           bode_plot_params=dict(), tvect=tvect)
+        assert_array_almost_equal(tvect, ax_step.lines[0].get_data()[0])
+
+    def test_sisotool_mimo(self,  sys222, sys221):
+        # a 2x2 should not raise an error:
+        sisotool(sys222)
+
+        # but 2 input, 1 output should
+        with pytest.raises(ControlMIMONotImplemented):
+            sisotool(sys221)
+
+
+
+
