fixed that full 2x2 system wasn't being passed around correctly, giving wrong step response

sawyerbfuller · sawyerbfuller · commit 77bc5c57ef4a · 2021-02-04T14:03:44.000-08:00
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
@@ -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,7 +576,9 @@ 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()
@@ -584,10 +589,10 @@ def _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=False):
     # 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
@@ -68,13 +68,10 @@ def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
     from .rlocus import root_locus
 
     # sys as loop transfer function if SISO
-    if sys.issiso():
-        sys_loop = sys
-    else:
+    if not sys.issiso():
         if not sys.ninputs == 2 and sys.noutputs == 2:
             raise ControlMIMONotImplemented(
                 'sys must be SISO or 2-input, 2-output')
-        sys_loop = sys[0,0]
 
     # Setup sisotool figure or superimpose if one is already present
     fig = plt.gcf()
@@ -101,7 +98,7 @@ def sisotool(sys, kvect = None, xlim_rlocus = None, ylim_rlocus = None,
         1 if kvect is None else kvect[0], bode_plot_params)
 
     # Setup the root-locus plot window
-    root_locus(sys_loop, kvect=kvect, xlim=xlim_rlocus,
+    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)
 
@@ -159,10 +156,10 @@ def _SisotoolUpdate(sys, fig, K, bode_plot_params, tvect=None):
     if sys.issiso():
         sys_closed = (K*sys).feedback(1)
     else:
-        sys_closed = append(sys, K)
+        sys_closed = append(sys, -K)
         connects = [[1, 3],
                     [3, 1]]
-        sys_closed = connect(sys_closed, connects, (2,), (2,))
+        sys_closed = connect(sys_closed, connects, 2, 2)
     if tvect is None:
         tvect, yout = step_response(sys_closed, T_num=100)
     else:
