new Pid design function built on sisotool

control/rlocus.py

@@ -168,8 +168,8 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None,
         else:
             if ax is None:
                 ax = plt.gca()
-                fig = ax.figure
-                ax.set_title('Root Locus')
+            fig = ax.figure
+            ax.set_title('Root Locus')
 
         if print_gain and not sisotool:
             fig.canvas.mpl_connect(
@@ -180,15 +180,15 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None,
             fig.axes[1].plot(
                 [root.real for root in start_mat],
                 [root.imag for root in start_mat],
-                marker='s', markersize=8, zorder=20, label='gain_point')
+                marker='s', markersize=6, zorder=20, color='k', label='gain_point')
             s = start_mat[0][0]
             if isdtime(sys, strict=True):
                 zeta = -np.cos(np.angle(np.log(s)))
             else:
                 zeta = -1 * s.real / abs(s)
             fig.suptitle(
                 "Clicked at: %10.4g%+10.4gj  gain: %10.4g  damp: %10.4g" %
-                (s.real, s.imag, 1, zeta),
+                (s.real, s.imag, kvect[0], zeta),
                 fontsize=12 if int(mpl.__version__[0]) == 1 else 10)
             fig.canvas.mpl_connect(
                 'button_release_event',
@@ -623,7 +623,7 @@ def _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=False):
             ax_rlocus.plot(
                 [root.real for root in mymat],
                 [root.imag for root in mymat],
-                marker='s', markersize=8, zorder=20, label='gain_point')
+                marker='s', markersize=6, zorder=20, label='gain_point', color='k')
         else:
             ax_rlocus.plot(s.real, s.imag, 'k.', marker='s', markersize=8,
                            zorder=20, label='gain_point')
@@ -769,7 +769,7 @@ def _default_wn(xloc, yloc, max_lines=7):
 
     """
     sep = xloc[1]-xloc[0]       # separation between x-ticks
-    
+
     # Decide whether to use the x or y axis for determining wn
     if yloc[-1] / sep > max_lines*10:
         # y-axis scale >> x-axis scale

control/sisotool.py

@@ -1,11 +1,15 @@
-__all__ = ['sisotool']
+__all__ = ['sisotool', 'rootlocus_pid_designer']
 
 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 .xferfcn import tf
+from .statesp import ss
 from .bdalg import append, connect
+from .iosys import tf2io, ss2io, summing_junction, interconnect
+from control.statesp import _convert_to_statespace, StateSpace
+from control.lti import common_timebase, isctime
 import matplotlib
 import matplotlib.pyplot as plt
 import warnings
@@ -176,3 +180,156 @@ def _SisotoolUpdate(sys, fig, K, bode_plot_params, tvect=None):
     fig.subplots_adjust(top=0.9,wspace = 0.3,hspace=0.35)
     fig.canvas.draw()
 
+# contributed by Sawyer Fuller, minster@uw.edu 2021.11.02, based on
+# an implementation in Matlab by Martin Berg.
+def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r',
+                           Kp0=0, Ki0=0, Kd0=0, tau=0.01,
+                           C_ff=0, derivative_in_feedback_path=False,
+                           plot=True):
+    """Manual PID controller design based on root locus using Sisotool
+
+    Uses `Sisotool` to investigate the effect of adding or subtracting an
+    amount `deltaK` to the proportional, integral, or derivative (PID) gains of
+    a controller. One of the PID gains, `Kp`, `Ki`, or `Kd`, respectively, can
+    be modified at a time. `Sisotool` plots the step response, frequency
+    response, and root locus.
+
+    When first run, `deltaK` is set to 0; click on a branch of the root locus
+    plot to try a different value. Each click updates plots and prints
+    the corresponding `deltaK`. To tune all three PID gains, repeatedly call
+    `rootlocus_pid_designer`, and select a different `gain` each time (`'P'`,
+    `'I'`, or `'D'`). Make sure to add the resulting `deltaK` to your chosen
+    initial gain on the next iteration.
+
+    Example: to examine the effect of varying `Kp` starting from an intial
+    value of 10, use the arguments `gain='P', Kp0=10`. Suppose a `deltaK`
+    value of 5 gives satisfactory performance. Then on the next iteration,
+    to tune the derivative gain, use the arguments `gain='D', Kp0=15`.
+
+    By default, all three PID terms are in the forward path C_f in the diagram
+    shown below, that is,
+
+    C_f = Kp + Ki/s + Kd*s/(tau*s + 1).
+
+    If `plant` is a discrete-time system, then the proportional, integral, and
+    derivative terms are given instead by Kp, Ki*dt/2*(z+1)/(z-1), and
+    Kd/dt*(z-1)/z, respectively.
+
+        ------> C_ff ------    d
+        |                 |    |
+    r   |     e           V    V  u         y
+    ------->O---> C_f --->O--->O---> plant --->
+            ^-            ^-                |
+            |             |                 |
+            |             ----- C_b <-------|
+            ---------------------------------
+
+    It is also possible to move the derivative term into the feedback path
+    `C_b` using `derivative_in_feedback_path=True`. This may be desired to
+    avoid that the plant is subject to an impulse function when the reference
+    `r` is a step input. `C_b` is otherwise set to zero.
+
+    If `plant` is a 2-input system, the disturbance `d` is fed directly into
+    its second input rather than being added to `u`.
+
+    Remark: It may be helpful to zoom in using the magnifying glass on the
+    plot. Just ake sure to deactivate magnification mode when you are done by
+    clicking the magnifying glass. Otherwise you will not be able to be able to choose
+    a gain on the root locus plot.
+
+    Parameters
+    ----------
+    plant : :class:`LTI` (:class:`TransferFunction` or :class:`StateSpace` system)
+        The dynamical system to be controlled
+    gain : string (optional)
+        Which gain to vary by `deltaK`. Must be one of `'P'`, `'I'`, or `'D'`
+        (proportional, integral, or derative)
+    sign : int (optional)
+        The sign of deltaK gain perturbation
+    input : string (optional)
+        The input used for the step response; must be `'r'` (reference) or
+        `'d'` (disturbance) (see figure above)
+    Kp0, Ki0, Kd0 : float (optional)
+        Initial values for proportional, integral, and derivative gains,
+        respectively
+    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)
+        Feedforward controller. If :class:`LTI`, must have timebase that is
+        compatible with plant.
+    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)
+        Whether to create Sisotool interactive plot.
+
+    Returns
+    ----------
+    closedloop : class:`StateSpace` system
+        The closed-loop system using initial gains.
+    """
+
+    plant = _convert_to_statespace(plant)
+    if plant.ninputs == 1:
+        plant = ss2io(plant, inputs='u', outputs='y')
+    elif plant.ninputs == 2:
+        plant = ss2io(plant, inputs=['u', 'd'], outputs='y')
+    else:
+        raise ValueError("plant must have one or two inputs")
+    C_ff = ss2io(_convert_to_statespace(C_ff),   inputs='r', outputs='uff')
+    dt = common_timebase(plant, C_ff)
+
+    # create systems used for interconnections
+    e_summer = summing_junction(['r', '-y'], 'e')
+    if plant.ninputs == 2:
+        u_summer = summing_junction(['ufb', 'uff'], 'u')
+    else:
+        u_summer = summing_junction(['ufb', 'uff', 'd'], 'u')
+
+    if isctime(plant):
+        prop  = tf(1, 1)
+        integ = tf(1, [1, 0])
+        deriv = tf([1, 0], [tau, 1])
+    else: # discrete-time
+        prop  = tf(1, 1, dt)
+        integ = tf([dt/2, dt/2], [1, -1], dt)
+        deriv = tf([1, -1], [dt, 0], dt)
+
+    # add signal names by turning into iosystems
+    prop  = tf2io(prop,        inputs='e', outputs='prop_e')
+    integ = tf2io(integ,       inputs='e', outputs='int_e')
+    if derivative_in_feedback_path:
+        deriv = tf2io(-deriv,  inputs='y', outputs='deriv')
+    else:
+        deriv = tf2io(deriv,   inputs='e', outputs='deriv')
+
+    # create gain blocks
+    Kpgain = tf2io(tf(Kp0, 1),            inputs='prop_e',  outputs='ufb')
+    Kigain = tf2io(tf(Ki0, 1),            inputs='int_e',   outputs='ufb')
+    Kdgain = tf2io(tf(Kd0, 1),            inputs='deriv',  outputs='ufb')
+
+    # for the gain that is varied, replace gain block with a special block
+    # that has an 'input' and an 'output' that creates loop transfer function
+    if gain in ('P', 'p'):
+        Kpgain = ss2io(ss([],[],[],[[0, 1], [-sign, Kp0]]),
+            inputs=['input', 'prop_e'], outputs=['output', 'ufb'])
+    elif gain in ('I', 'i'):
+        Kigain = ss2io(ss([],[],[],[[0, 1], [-sign, Ki0]]),
+            inputs=['input', 'int_e'],  outputs=['output', 'ufb'])
+    elif gain in ('D', 'd'):
+        Kdgain = ss2io(ss([],[],[],[[0, 1], [-sign, Kd0]]),
+            inputs=['input', 'deriv'], outputs=['output', 'ufb'])
+    else:
+        raise ValueError(gain + ' gain not recognized.')
+
+    # the second input and output are used by sisotool to plot step response
+    loop = interconnect((plant, Kpgain, Kigain, Kdgain, prop, integ, deriv,
+                            C_ff, e_summer, u_summer),
+                            inplist=['input', input_signal],
+                            outlist=['output', 'y'])
+    if plot:
+        sisotool(loop, kvect=(0.,))
+    cl = loop[1, 1] # closed loop transfer function with initial gains
+    return StateSpace(cl.A, cl.B, cl.C, cl.D, cl.dt)

control/tests/lti_test.py

@@ -77,7 +77,7 @@ def test_damp(self):
         p2_splane = -wn2 * zeta2 + 1j * wn2 * np.sqrt(1 - zeta2**2)
         p2_zplane = np.exp(p2_splane * dt)
         np.testing.assert_almost_equal(p2, p2_zplane)
-        
+
     def test_dcgain(self):
         sys = tf(84, [1, 2])
         np.testing.assert_allclose(sys.dcgain(), 42)
@@ -136,7 +136,7 @@ def test_common_timebase(self, dt1, dt2, expected, sys1, sys2):
                               (0, 1),
                               (1, 2)])
     def test_common_timebase_errors(self, i1, i2):
-        """Test that common_timbase throws errors on invalid combinations"""
+        """Test that common_timbase raises errors on invalid combinations"""
         with pytest.raises(ValueError):
             common_timebase(i1, i2)
         # Make sure behaviour is symmetric

control/tests/sisotool_test.py

@@ -6,11 +6,11 @@
 from numpy.testing import assert_array_almost_equal
 import pytest
 
-from control.sisotool import sisotool
+from control.sisotool import sisotool, rootlocus_pid_designer
 from control.rlocus import _RLClickDispatcher
 from control.xferfcn import TransferFunction
 from control.statesp import StateSpace
-
+from control import c2d
 
 @pytest.mark.usefixtures("mplcleanup")
 class TestSisotool:
@@ -140,3 +140,40 @@ def test_sisotool_mimo(self,  sys222, sys221):
         # but 2 input, 1 output should
         with pytest.raises(ControlMIMONotImplemented):
             sisotool(sys221)
+
+@pytest.mark.usefixtures("mplcleanup")
+class TestPidDesigner:
+    @pytest.fixture
+    def plant(self, request):
+        plants = {
+            'syscont':TransferFunction(1,[1, 3, 0]),
+            'sysdisc1':c2d(TransferFunction(1,[1, 3, 0]), .1),
+            'syscont221':StateSpace([[-.3, 0],[1,0]],[[-1,],[.1,]], [0, -.3], 0)}
+        return plants[request.param]
+
+    # test permutations of system construction without plotting
+    @pytest.mark.parametrize('plant', ('syscont', 'sysdisc1', 'syscont221'), indirect=True)
+    @pytest.mark.parametrize('gain', ('P', 'I', 'D'))
+    @pytest.mark.parametrize('sign', (1,))
+    @pytest.mark.parametrize('input_signal', ('r', 'd'))
+    @pytest.mark.parametrize('Kp0', (0,))
+    @pytest.mark.parametrize('Ki0', (1.,))
+    @pytest.mark.parametrize('Kd0', (0.1,))
+    @pytest.mark.parametrize('tau', (0.01,))
+    @pytest.mark.parametrize('C_ff', (0, 1,))
+    @pytest.mark.parametrize('derivative_in_feedback_path', (True, False,))
+    @pytest.mark.parametrize("kwargs", [{'plot':False},])
+    def test_pid_designer_1(self, plant, gain, sign, input_signal, Kp0, Ki0, Kd0, tau, C_ff,
+            derivative_in_feedback_path, kwargs):
+        rootlocus_pid_designer(plant, gain, sign, input_signal, Kp0, Ki0, Kd0, tau, C_ff,
+            derivative_in_feedback_path, **kwargs)
+
+    # test creation of sisotool plot
+    # input from reference or disturbance
+    @pytest.mark.parametrize('plant', ('syscont', 'syscont221'), indirect=True)
+    @pytest.mark.parametrize("kwargs", [
+        {'input_signal':'r', 'Kp0':0.01, 'derivative_in_feedback_path':True},
+        {'input_signal':'d', 'Kp0':0.01, 'derivative_in_feedback_path':True},])
+    def test_pid_designer_2(self, plant, kwargs):
+        rootlocus_pid_designer(plant, **kwargs)
+

doc/control.rst

@@ -124,6 +124,7 @@ Control system synthesis
     lqe
     mixsyn
     place
+    rlocus_pid_designer
 
 Model simplification tools
 ==========================