13107996232

Committed 03 Feb 2025 06:53AM UTC coverage: 94.731% (+0.02%) from 94.709%

Build # 13107996232

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #1094 from murrayrm/userguide-22Dec2024

Updated user documentation (User Guide, Reference Manual)

Run Details

9673 of 10211 relevant lines covered (94.73%)

8.28 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

95.6

control/rlocus.py

# rlocus.py - code for computing a root locus plot
#
# Initial author: Ryan Krauss
# Creation date: 2010
#
# RMM, 17 June 2010: modified to be a standalone piece of code
#
# RMM, 2 April 2011: modified to work with new LTI structure
#
# Sawyer B. Fuller (minster@uw.edu) 21 May 2020: added compatibility
# with discrete-time systems.

"""Code for computing a root locus plot."""

import warnings

import numpy as np
import scipy.signal  # signal processing toolbox
from numpy import poly1d, vstack, zeros_like

from . import config
from .ctrlplot import ControlPlot
from .exception import ControlMIMONotImplemented
from .lti import LTI
from .xferfcn import _convert_to_transfer_function

__all__ = ['root_locus_map', 'root_locus_plot', 'root_locus', 'rlocus']

# Default values for module parameters
_rlocus_defaults = {
    'rlocus.grid': True,
}


# Root locus map
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 and k varies over a range of gains.

    Parameters
    ----------
    sysdata : LTI system or list of LTI systems
        Linear input/output systems (SISO only, for now).
    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
    -------
    rldata : `PoleZeroData` or list of `PoleZeroData`
        Root locus data object(s).  The loci of the root locus diagram are
        available in the array `rldata.loci`, indexed by the gain index and
        the locus index, and the gains are in the array `rldata.gains`.

    Notes
    -----
    For backward compatibility, the `rldata` return object can be
    assigned to the tuple ``(roots, gains)``.

    """
    from .pzmap import PoleZeroData, PoleZeroList

    # Convert the first argument to a list
    syslist = sysdata if isinstance(sysdata, (list, tuple)) else [sysdata]

    responses = []
    for idx, sys in enumerate(syslist):
        if not sys.issiso():
            raise ControlMIMONotImplemented(
                "sys must be single-input single-output (SISO)")

        # Convert numerator and denominator to polynomials if they aren't
        nump, denp = _systopoly1d(sys[0, 0])

        if gains is None:
            kvect, root_array, _, _ = _default_gains(nump, denp, None, None)
        else:
            kvect = np.atleast_1d(gains)
            root_array = _RLFindRoots(nump, denp, kvect)
            root_array = _RLSortRoots(root_array)

        responses.append(PoleZeroData(
            sys.poles(), sys.zeros(), kvect, root_array, sort_loci=False,
            dt=sys.dt, sysname=sys.name, sys=sys))

    if isinstance(sysdata, (list, tuple)):
        return PoleZeroList(responses)
    else:
        return responses[0]


def root_locus_plot(
        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 and k varies over a range of gains.

    Parameters
    ----------
    sysdata : PoleZeroMap or LTI object or list
        Linear input/output systems (SISO only, for now).
    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 (see `matplotlib.axes.Axes.set_xlim`).
    ylim : tuple or list, optional
        Set limits of y axis (see `matplotlib.axes.Axes.set_ylim`).
    plot : bool, optional
        (legacy) If given, `root_locus_plot` returns the legacy return values
        of roots and gains.  If False, just return the values with no plot.
    grid : bool or str, optional
        If True plot omega-damping grid, if False show imaginary axis
        for continuous-time systems, unit circle for discrete-time systems.
        If 'empty', do not draw any additional lines.  Default value is set
        by `config.defaults['rlocus.grid']`.
    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
    -------
    cplt : `ControlPlot` object
        Object containing the data that were plotted.  See `ControlPlot`
        for more detailed information.
    cplt.lines : array of list of `matplotlib.lines.Line2D`
        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 `matplotlib.axes.Axes`
        Axes for each subplot.
    cplt.figure : `matplotlib.figure.Figure`
        Figure containing the plot.
    cplt.legend : 2D array of `matplotlib.legend.Legend`
        Legend object(s) contained in the plot.
    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,
        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
    -----
    The root_locus_plot 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 matplotlib.pyplot.gca().axis('auto') and
    then set the axis limits to the desired values.

    """
    # 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=gains)
    else:
        responses = sysdata

    #
    # Process `plot` keyword
    #
    # See bode_plot for a description of how this keyword is handled to
    # support legacy implementations of root_locus.
    #
    if plot is not None:
        warnings.warn(
            "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
    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 ControlPlot(cplt.lines, cplt.axes, cplt.figure)


def _default_gains(num, den, xlim, ylim):
    """Unsupervised gains calculation for root locus plot.

    References
    ----------
    .. [1] Ogata, K. (2002). Modern control engineering (4th
       ed.). Upper Saddle River, NJ : New Delhi: Prentice Hall..

    """
    # Compute the break points on the real axis for the root locus plot
    k_break, real_break = _break_points(num, den)

    # Decide on the maximum gain to use and create the gain vector
    kmax = _k_max(num, den, real_break, k_break)
    kvect = np.hstack((np.linspace(0, kmax, 50), np.real(k_break)))
    kvect.sort()

    # Find the roots for all of the gains and sort them
    root_array = _RLFindRoots(num, den, kvect)
    root_array = _RLSortRoots(root_array)

    # Keep track of the open loop poles and zeros
    open_loop_poles = den.roots
    open_loop_zeros = num.roots

    # ???
    if open_loop_zeros.size != 0 and \
       open_loop_zeros.size < open_loop_poles.size:
        open_loop_zeros_xl = np.append(
            open_loop_zeros,
            np.ones(open_loop_poles.size - open_loop_zeros.size)
            * open_loop_zeros[-1])
        root_array_xl = np.append(root_array, open_loop_zeros_xl)
    else:
        root_array_xl = root_array
    singular_points = np.concatenate((num.roots, den.roots), axis=0)
    important_points = np.concatenate((singular_points, real_break), axis=0)
    important_points = np.concatenate((important_points, np.zeros(2)), axis=0)
    root_array_xl = np.append(root_array_xl, important_points)

    false_gain = float(den.coeffs[0]) / float(num.coeffs[0])
    if false_gain < 0 and not den.order > num.order:
        # TODO: make error message more understandable
        raise ValueError("Not implemented support for 0 degrees root locus "
                         "with equal order of numerator and denominator.")

    if xlim is None and false_gain > 0:
        x_tolerance = 0.05 * (np.max(np.real(root_array_xl))
                              - np.min(np.real(root_array_xl)))
        xlim = _ax_lim(root_array_xl)
    elif xlim is None and false_gain < 0:
        axmin = np.min(np.real(important_points)) \
            - (np.max(np.real(important_points))
               - np.min(np.real(important_points)))
        axmin = np.min(np.array([axmin, np.min(np.real(root_array_xl))]))
        axmax = np.max(np.real(important_points)) \
            + np.max(np.real(important_points)) \
            - np.min(np.real(important_points))
        axmax = np.max(np.array([axmax, np.max(np.real(root_array_xl))]))
        xlim = [axmin, axmax]
        x_tolerance = 0.05 * (axmax - axmin)
    else:
        x_tolerance = 0.05 * (xlim[1] - xlim[0])

    if ylim is None:
        y_tolerance = 0.05 * (np.max(np.imag(root_array_xl))
                              - np.min(np.imag(root_array_xl)))
        ylim = _ax_lim(root_array_xl * 1j)
    else:
        y_tolerance = 0.05 * (ylim[1] - ylim[0])

    # Figure out which points are spaced too far apart
    if x_tolerance == 0:
        # Root locus is on imaginary axis (rare), use just y distance
        tolerance = y_tolerance
    elif y_tolerance == 0:
        # Root locus is on imaginary axis (common), use just x distance
        tolerance = x_tolerance
    else:
        tolerance = np.min([x_tolerance, y_tolerance])
    indexes_too_far = _indexes_filt(root_array, tolerance)

    # Add more points into the root locus for points that are too far apart
    while len(indexes_too_far) > 0 and kvect.size < 5000:
        for counter, index in enumerate(indexes_too_far):
            index = index + counter*3
            new_gains = np.linspace(kvect[index], kvect[index + 1], 5)
            new_points = _RLFindRoots(num, den, new_gains[1:4])
            kvect = np.insert(kvect, index + 1, new_gains[1:4])
            root_array = np.insert(root_array, index + 1, new_points, axis=0)

        root_array = _RLSortRoots(root_array)
        indexes_too_far = _indexes_filt(root_array, tolerance)

    new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4)))
    new_points = _RLFindRoots(num, den, new_gains[1:4])
    kvect = np.append(kvect, new_gains[1:4])
    root_array = np.concatenate((root_array, new_points), axis=0)
    root_array = _RLSortRoots(root_array)
    return kvect, root_array, xlim, ylim


def _indexes_filt(root_array, tolerance):
    """Calculate the distance between points and return the indices.

    Filter the indexes so only the resolution of points within the xlim and
    ylim is improved when zoom is used.

    """
    distance_points = np.abs(np.diff(root_array, axis=0))
    indexes_too_far = list(np.unique(np.where(distance_points > tolerance)[0]))
    indexes_too_far.sort()
    return indexes_too_far


def _break_points(num, den):
    """Extract break points over real axis and gains given these locations"""
    # type: (np.poly1d, np.poly1d) -> (np.array, np.array)
    dnum = num.deriv(m=1)
    dden = den.deriv(m=1)
    polynom = den * dnum - num * dden
    real_break_pts = polynom.r
    # don't care about infinite break points
    real_break_pts = real_break_pts[num(real_break_pts) != 0]
    k_break = -den(real_break_pts) / num(real_break_pts)
    idx = k_break >= 0   # only positives gains
    k_break = k_break[idx]
    real_break_pts = real_break_pts[idx]
    if len(k_break) == 0:
        k_break = [0]
        real_break_pts = den.roots
    return k_break, real_break_pts


def _ax_lim(root_array):
    """Utility to get the axis limits"""
    axmin = np.min(np.real(root_array))
    axmax = np.max(np.real(root_array))
    if axmax != axmin:
        deltax = (axmax - axmin) * 0.02
    else:
        deltax = np.max([1., axmax / 2])
    axlim = [axmin - deltax, axmax + deltax]
    return axlim


def _k_max(num, den, real_break_points, k_break_points):
    """"Calculate the maximum gain for the root locus shown in the figure."""
    asymp_number = den.order - num.order
    singular_points = np.concatenate((num.roots, den.roots), axis=0)
    important_points = np.concatenate(
        (singular_points, real_break_points), axis=0)
    false_gain = den.coeffs[0] / num.coeffs[0]

    if asymp_number > 0:
        asymp_center = (np.sum(den.roots) - np.sum(num.roots))/asymp_number
        distance_max = 4 * np.max(np.abs(important_points - asymp_center))
        asymp_angles = (2 * np.arange(0, asymp_number) - 1) \
            * np.pi / asymp_number
        if false_gain > 0:
            # farthest points over asymptotes
            farthest_points = asymp_center \
                + distance_max * np.exp(asymp_angles * 1j)
        else:
            asymp_angles = asymp_angles + np.pi
            # farthest points over asymptotes
            farthest_points = asymp_center \
                + distance_max * np.exp(asymp_angles * 1j)
        kmax_asymp = np.real(np.abs(den(farthest_points)
                                    / num(farthest_points)))
    else:
        kmax_asymp = np.abs([np.abs(den.coeffs[0])
                             / np.abs(num.coeffs[0]) * 3])

    kmax = np.max(np.concatenate((np.real(kmax_asymp),
                                  np.real(k_break_points)), axis=0))
    if np.abs(false_gain) > kmax:
        kmax = np.abs(false_gain)
    return kmax


def _systopoly1d(sys):
    """Extract numerator and denominator polynomials for a system"""
    # Allow inputs from the signal processing toolbox
    if (isinstance(sys, scipy.signal.lti)):
        nump = sys.num
        denp = sys.den

    else:
        # Convert to a transfer function, if needed
        sys = _convert_to_transfer_function(sys)

        # Make sure we have a SISO system
        if not sys.issiso():
            raise ControlMIMONotImplemented()

        # Start by extracting the numerator and denominator from system object
        nump = sys.num[0][0]
        denp = sys.den[0][0]

    # Check to see if num, den are already polynomials; otherwise convert
    if (not isinstance(nump, poly1d)):
        nump = poly1d(nump)

    if (not isinstance(denp, poly1d)):
        denp = poly1d(denp)

    return (nump, denp)


def _RLFindRoots(nump, denp, kvect):
    """Find the roots for the root locus."""
    # Convert numerator and denominator to polynomials if they aren't
    roots = []
    for k in np.atleast_1d(kvect):
        curpoly = denp + k * nump
        curroots = curpoly.r
        if len(curroots) < denp.order:
            # if I have fewer poles than open loop, it is because i have
            # one at infinity
            curroots = np.append(curroots, np.inf)

        curroots.sort()
        roots.append(curroots)

    return vstack(roots)


def _RLSortRoots(roots):
    """Sort the roots from _RLFindRoots, so that the root
    locus doesn't show weird pseudo-branches as roots jump from
    one branch to another."""

    sorted = zeros_like(roots)
    sorted[0] = roots[0]
    for n, row in enumerate(roots[1:], start=1):
        # sort the current row by finding the element with the
        # smallest absolute distance to each root in the
        # previous row
        prevrow = sorted[n-1]
        available = list(range(len(prevrow)))
        for elem in row:
            evect = elem - prevrow[available]
            ind1 = abs(evect).argmin()
            ind = available.pop(ind1)
            sorted[n, ind] = elem
    return sorted


# Alternative ways to call these functions
root_locus = root_locus_plot
rlocus = root_locus_plot

1	# rlocus.py - code for computing a root locus plot
2	#
3	# Initial author: Ryan Krauss
4	# Creation date: 2010
5	#
6	# RMM, 17 June 2010: modified to be a standalone piece of code
7	#
8	# RMM, 2 April 2011: modified to work with new LTI structure
9	#
10	# Sawyer B. Fuller (minster@uw.edu) 21 May 2020: added compatibility
11	# with discrete-time systems.
12
13	"""Code for computing a root locus plot."""
14
15	import warnings	9✔
16
17	import numpy as np	9✔
18	import scipy.signal # signal processing toolbox	9✔
19	from numpy import poly1d, vstack, zeros_like	9✔
20
21	from . import config	9✔
22	from .ctrlplot import ControlPlot	9✔
23	from .exception import ControlMIMONotImplemented	9✔
24	from .lti import LTI	9✔
25	from .xferfcn import _convert_to_transfer_function	9✔
26
27	__all__ = ['root_locus_map', 'root_locus_plot', 'root_locus', 'rlocus']	9✔
28
29	# Default values for module parameters
30	_rlocus_defaults = {	9✔
31	'rlocus.grid': True,
32	}
33
34
35	# Root locus map
36	def root_locus_map(sysdata, gains=None):	9✔
37	"""Compute the root locus map for an LTI system.
38
39	Calculate the root locus by finding the roots of 1 + k * G(s) where G
40	is a linear system and k varies over a range of gains.
41
42	Parameters
43	----------
44	sysdata : LTI system or list of LTI systems
45	Linear input/output systems (SISO only, for now).
46	gains : array_like, optional
47	Gains to use in computing plot of closed-loop poles. If not given,
48	gains are chosen to include the main features of the root locus map.
49
50	Returns
51	-------
52	rldata : `PoleZeroData` or list of `PoleZeroData`
53	Root locus data object(s). The loci of the root locus diagram are
54	available in the array `rldata.loci`, indexed by the gain index and
55	the locus index, and the gains are in the array `rldata.gains`.
56
57	Notes
58	-----
59	For backward compatibility, the `rldata` return object can be
60	assigned to the tuple ``(roots, gains)``.
61
62	"""
63	from .pzmap import PoleZeroData, PoleZeroList	9✔
64
65	# Convert the first argument to a list
66	syslist = sysdata if isinstance(sysdata, (list, tuple)) else [sysdata]	9✔
67
68	responses = []	9✔
69	for idx, sys in enumerate(syslist):	9✔
70	if not sys.issiso():	9✔
71	raise ControlMIMONotImplemented(	×
72	"sys must be single-input single-output (SISO)")
73
74	# Convert numerator and denominator to polynomials if they aren't
75	nump, denp = _systopoly1d(sys[0, 0])	9✔
76
77	if gains is None:	9✔
78	kvect, root_array, _, _ = _default_gains(nump, denp, None, None)	9✔
79	else:
80	kvect = np.atleast_1d(gains)	9✔
81	root_array = _RLFindRoots(nump, denp, kvect)	9✔
82	root_array = _RLSortRoots(root_array)	9✔
83
84	responses.append(PoleZeroData(	9✔
85	sys.poles(), sys.zeros(), kvect, root_array, sort_loci=False,
86	dt=sys.dt, sysname=sys.name, sys=sys))
87
88	if isinstance(sysdata, (list, tuple)):	9✔
89	return PoleZeroList(responses)	9✔
90	else:
91	return responses[0]	9✔
92
93
94	def root_locus_plot(	9✔
95	sysdata, gains=None, grid=None, plot=None, **kwargs):
96
97	"""Root locus plot.
98
99	Calculate the root locus by finding the roots of 1 + k * G(s) where G
100	is a linear system and k varies over a range of gains.
101
102	Parameters
103	----------
104	sysdata : PoleZeroMap or LTI object or list
105	Linear input/output systems (SISO only, for now).
106	gains : array_like, optional
107	Gains to use in computing plot of closed-loop poles. If not given,
108	gains are chosen to include the main features of the root locus map.
109	xlim : tuple or list, optional
110	Set limits of x axis (see `matplotlib.axes.Axes.set_xlim`).
111	ylim : tuple or list, optional
112	Set limits of y axis (see `matplotlib.axes.Axes.set_ylim`).
113	plot : bool, optional
114	(legacy) If given, `root_locus_plot` returns the legacy return values
115	of roots and gains. If False, just return the values with no plot.
116	grid : bool or str, optional
117	If True plot omega-damping grid, if False show imaginary axis
118	for continuous-time systems, unit circle for discrete-time systems.
119	If 'empty', do not draw any additional lines. Default value is set
120	by `config.defaults['rlocus.grid']`.
121	initial_gain : float, optional
122	Mark the point on the root locus diagram corresponding to the
123	given gain.
124	color : matplotlib color spec, optional
125	Specify the color of the markers and lines.
126
127	Returns
128	-------
129	cplt : `ControlPlot` object
130	Object containing the data that were plotted. See `ControlPlot`
131	for more detailed information.
132	cplt.lines : array of list of `matplotlib.lines.Line2D`
133	The shape of the array is given by (nsys, 3) where nsys is the number
134	of systems or responses passed to the function. The second index
135	specifies the object type:
136
137	- lines[idx, 0]: poles
138	- lines[idx, 1]: zeros
139	- lines[idx, 2]: loci
140
141	cplt.axes : 2D array of `matplotlib.axes.Axes`
142	Axes for each subplot.
143	cplt.figure : `matplotlib.figure.Figure`
144	Figure containing the plot.
145	cplt.legend : 2D array of `matplotlib.legend.Legend`
146	Legend object(s) contained in the plot.
147	roots, gains : ndarray
148	(legacy) If the `plot` keyword is given, returns the closed-loop
149	root locations, arranged such that each row corresponds to a gain,
150	and the array of gains (same as `gains` keyword argument if provided).
151
152	Other Parameters
153	----------------
154	ax : `matplotlib.axes.Axes`, optional
155	The matplotlib axes to draw the figure on. If not specified and
156	the current figure has a single axes, that axes is used.
157	Otherwise, a new figure is created.
158	label : str or array_like of str, optional
159	If present, replace automatically generated label(s) with the given
160	label(s). If sysdata is a list, strings should be specified for each
161	system.
162	legend_loc : int or str, optional
163	Include a legend in the given location. Default is 'center right',
164	with no legend for a single response. Use False to suppress legend.
165	show_legend : bool, optional
166	Force legend to be shown if True or hidden if False. If
167	None, then show legend when there is more than one line on the
168	plot or `legend_loc` has been specified.
169	title : str, optional
170	Set the title of the plot. Defaults to plot type and system name(s).
171
172	Notes
173	-----
174	The root_locus_plot function calls matplotlib.pyplot.axis('equal'), which
175	means that trying to reset the axis limits may not behave as expected.
176	To change the axis limits, use matplotlib.pyplot.gca().axis('auto') and
177	then set the axis limits to the desired values.
178
179	"""
180	# Legacy parameters
181	for oldkey in ['kvect', 'k']:	9✔
182	gains = config._process_legacy_keyword(kwargs, oldkey, 'gains', gains)	9✔
183
184	if isinstance(sysdata, list) and all(	9✔
185	[isinstance(sys, LTI) for sys in sysdata]) or \
186	isinstance(sysdata, LTI):
187	responses = root_locus_map(sysdata, gains=gains)	9✔
188	else:
189	responses = sysdata	9✔
190
191	#
192	# Process `plot` keyword
193	#
194	# See bode_plot for a description of how this keyword is handled to
195	# support legacy implementations of root_locus.
196	#
197	if plot is not None:	9✔
198	warnings.warn(	9✔
199	"root_locus() return value of roots, gains is deprecated; "
200	"use root_locus_map()", FutureWarning)
201
202	if plot is False:	9✔
203	return responses.loci, responses.gains	9✔
204
205	# Plot the root loci
206	cplt = responses.plot(grid=grid, **kwargs)	9✔
207
208	# Legacy processing: return locations of poles and zeros as a tuple
209	if plot is True:	9✔
210	return responses.loci, responses.gains	9✔
211
212	return ControlPlot(cplt.lines, cplt.axes, cplt.figure)	9✔
213
214
215	def _default_gains(num, den, xlim, ylim):	9✔
216	"""Unsupervised gains calculation for root locus plot.
217
218	References
219	----------
220	.. [1] Ogata, K. (2002). Modern control engineering (4th
221	ed.). Upper Saddle River, NJ : New Delhi: Prentice Hall..
222
223	"""
224	# Compute the break points on the real axis for the root locus plot
225	k_break, real_break = _break_points(num, den)	9✔
226
227	# Decide on the maximum gain to use and create the gain vector
228	kmax = _k_max(num, den, real_break, k_break)	9✔
229	kvect = np.hstack((np.linspace(0, kmax, 50), np.real(k_break)))	9✔
230	kvect.sort()	9✔
231
232	# Find the roots for all of the gains and sort them
233	root_array = _RLFindRoots(num, den, kvect)	9✔
234	root_array = _RLSortRoots(root_array)	9✔
235
236	# Keep track of the open loop poles and zeros
237	open_loop_poles = den.roots	9✔
238	open_loop_zeros = num.roots	9✔
239
240	# ???
241	if open_loop_zeros.size != 0 and \	9✔
242	open_loop_zeros.size < open_loop_poles.size:
243	open_loop_zeros_xl = np.append(	9✔
244	open_loop_zeros,
245	np.ones(open_loop_poles.size - open_loop_zeros.size)
246	* open_loop_zeros[-1])
247	root_array_xl = np.append(root_array, open_loop_zeros_xl)	9✔
248	else:
249	root_array_xl = root_array	9✔
250	singular_points = np.concatenate((num.roots, den.roots), axis=0)	9✔
251	important_points = np.concatenate((singular_points, real_break), axis=0)	9✔
252	important_points = np.concatenate((important_points, np.zeros(2)), axis=0)	9✔
253	root_array_xl = np.append(root_array_xl, important_points)	9✔
254
255	false_gain = float(den.coeffs[0]) / float(num.coeffs[0])	9✔
256	if false_gain < 0 and not den.order > num.order:	9✔
257	# TODO: make error message more understandable
258	raise ValueError("Not implemented support for 0 degrees root locus "	9✔
259	"with equal order of numerator and denominator.")
260
261	if xlim is None and false_gain > 0:	9✔
262	x_tolerance = 0.05 * (np.max(np.real(root_array_xl))	9✔
263	- np.min(np.real(root_array_xl)))
264	xlim = _ax_lim(root_array_xl)	9✔
265	elif xlim is None and false_gain < 0:	9✔
266	axmin = np.min(np.real(important_points)) \	9✔
267	- (np.max(np.real(important_points))
268	- np.min(np.real(important_points)))
269	axmin = np.min(np.array([axmin, np.min(np.real(root_array_xl))]))	9✔
270	axmax = np.max(np.real(important_points)) \	9✔
271	+ np.max(np.real(important_points)) \
272	- np.min(np.real(important_points))
273	axmax = np.max(np.array([axmax, np.max(np.real(root_array_xl))]))	9✔
274	xlim = [axmin, axmax]	9✔
275	x_tolerance = 0.05 * (axmax - axmin)	9✔
276	else:
277	x_tolerance = 0.05 * (xlim[1] - xlim[0])	×
278
279	if ylim is None:	9✔
280	y_tolerance = 0.05 * (np.max(np.imag(root_array_xl))	9✔
281	- np.min(np.imag(root_array_xl)))
282	ylim = _ax_lim(root_array_xl * 1j)	9✔
283	else:
284	y_tolerance = 0.05 * (ylim[1] - ylim[0])	×
285
286	# Figure out which points are spaced too far apart
287	if x_tolerance == 0:	9✔
288	# Root locus is on imaginary axis (rare), use just y distance
289	tolerance = y_tolerance	×
290	elif y_tolerance == 0:	9✔
291	# Root locus is on imaginary axis (common), use just x distance
292	tolerance = x_tolerance	9✔
293	else:
294	tolerance = np.min([x_tolerance, y_tolerance])	9✔
295	indexes_too_far = _indexes_filt(root_array, tolerance)	9✔
296
297	# Add more points into the root locus for points that are too far apart
298	while len(indexes_too_far) > 0 and kvect.size < 5000:	9✔
299	for counter, index in enumerate(indexes_too_far):	9✔
300	index = index + counter*3	9✔
301	new_gains = np.linspace(kvect[index], kvect[index + 1], 5)	9✔
302	new_points = _RLFindRoots(num, den, new_gains[1:4])	9✔
303	kvect = np.insert(kvect, index + 1, new_gains[1:4])	9✔
304	root_array = np.insert(root_array, index + 1, new_points, axis=0)	9✔
305
306	root_array = _RLSortRoots(root_array)	9✔
307	indexes_too_far = _indexes_filt(root_array, tolerance)	9✔
308
309	new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4)))	9✔
310	new_points = _RLFindRoots(num, den, new_gains[1:4])	9✔
311	kvect = np.append(kvect, new_gains[1:4])	9✔
312	root_array = np.concatenate((root_array, new_points), axis=0)	9✔
313	root_array = _RLSortRoots(root_array)	9✔
314	return kvect, root_array, xlim, ylim	9✔
315
316
317	def _indexes_filt(root_array, tolerance):	9✔
318	"""Calculate the distance between points and return the indices.
319
320	Filter the indexes so only the resolution of points within the xlim and
321	ylim is improved when zoom is used.
322
323	"""
324	distance_points = np.abs(np.diff(root_array, axis=0))	9✔
325	indexes_too_far = list(np.unique(np.where(distance_points > tolerance)[0]))	9✔
326	indexes_too_far.sort()	9✔
327	return indexes_too_far	9✔
328
329
330	def _break_points(num, den):	9✔
331	"""Extract break points over real axis and gains given these locations"""
332	# type: (np.poly1d, np.poly1d) -> (np.array, np.array)
333	dnum = num.deriv(m=1)	9✔
334	dden = den.deriv(m=1)	9✔
335	polynom = den * dnum - num * dden	9✔
336	real_break_pts = polynom.r	9✔
337	# don't care about infinite break points
338	real_break_pts = real_break_pts[num(real_break_pts) != 0]	9✔
339	k_break = -den(real_break_pts) / num(real_break_pts)	9✔
340	idx = k_break >= 0 # only positives gains	9✔
341	k_break = k_break[idx]	9✔
342	real_break_pts = real_break_pts[idx]	9✔
343	if len(k_break) == 0:	9✔
344	k_break = [0]	9✔
345	real_break_pts = den.roots	9✔
346	return k_break, real_break_pts	9✔
347
348
349	def _ax_lim(root_array):	9✔
350	"""Utility to get the axis limits"""
351	axmin = np.min(np.real(root_array))	9✔
352	axmax = np.max(np.real(root_array))	9✔
353	if axmax != axmin:	9✔
354	deltax = (axmax - axmin) * 0.02	9✔
355	else:
356	deltax = np.max([1., axmax / 2])	9✔
357	axlim = [axmin - deltax, axmax + deltax]	9✔
358	return axlim	9✔
359
360
361	def _k_max(num, den, real_break_points, k_break_points):	9✔
362	""""Calculate the maximum gain for the root locus shown in the figure."""
363	asymp_number = den.order - num.order	9✔
364	singular_points = np.concatenate((num.roots, den.roots), axis=0)	9✔
365	important_points = np.concatenate(	9✔
366	(singular_points, real_break_points), axis=0)
367	false_gain = den.coeffs[0] / num.coeffs[0]	9✔
368
369	if asymp_number > 0:	9✔
370	asymp_center = (np.sum(den.roots) - np.sum(num.roots))/asymp_number	9✔
371	distance_max = 4 * np.max(np.abs(important_points - asymp_center))	9✔
372	asymp_angles = (2 * np.arange(0, asymp_number) - 1) \	9✔
373	* np.pi / asymp_number
374	if false_gain > 0:	9✔
375	# farthest points over asymptotes
376	farthest_points = asymp_center \	9✔
377	+ distance_max * np.exp(asymp_angles * 1j)
378	else:
379	asymp_angles = asymp_angles + np.pi	9✔
380	# farthest points over asymptotes
381	farthest_points = asymp_center \	9✔
382	+ distance_max * np.exp(asymp_angles * 1j)
383	kmax_asymp = np.real(np.abs(den(farthest_points)	9✔
384	/ num(farthest_points)))
385	else:
386	kmax_asymp = np.abs([np.abs(den.coeffs[0])	9✔
387	/ np.abs(num.coeffs[0]) * 3])
388
389	kmax = np.max(np.concatenate((np.real(kmax_asymp),	9✔
390	np.real(k_break_points)), axis=0))
391	if np.abs(false_gain) > kmax:	9✔
392	kmax = np.abs(false_gain)	9✔
393	return kmax	9✔
394
395
396	def _systopoly1d(sys):	9✔
397	"""Extract numerator and denominator polynomials for a system"""
398	# Allow inputs from the signal processing toolbox
399	if (isinstance(sys, scipy.signal.lti)):	9✔
400	nump = sys.num	×
401	denp = sys.den	×
402
403	else:
404	# Convert to a transfer function, if needed
405	sys = _convert_to_transfer_function(sys)	9✔
406
407	# Make sure we have a SISO system
408	if not sys.issiso():	9✔
409	raise ControlMIMONotImplemented()	×
410
411	# Start by extracting the numerator and denominator from system object
412	nump = sys.num[0][0]	9✔
413	denp = sys.den[0][0]	9✔
414
415	# Check to see if num, den are already polynomials; otherwise convert
416	if (not isinstance(nump, poly1d)):	9✔
417	nump = poly1d(nump)	9✔
418
419	if (not isinstance(denp, poly1d)):	9✔
420	denp = poly1d(denp)	9✔
421
422	return (nump, denp)	9✔
423
424
425	def _RLFindRoots(nump, denp, kvect):	9✔
426	"""Find the roots for the root locus."""
427	# Convert numerator and denominator to polynomials if they aren't
428	roots = []	9✔
429	for k in np.atleast_1d(kvect):	9✔
430	curpoly = denp + k * nump	9✔
431	curroots = curpoly.r	9✔
432	if len(curroots) < denp.order:	9✔
433	# if I have fewer poles than open loop, it is because i have
434	# one at infinity
435	curroots = np.append(curroots, np.inf)	×
436
437	curroots.sort()	9✔
438	roots.append(curroots)	9✔
439
440	return vstack(roots)	9✔
441
442
443	def _RLSortRoots(roots):	9✔
444	"""Sort the roots from _RLFindRoots, so that the root
445	locus doesn't show weird pseudo-branches as roots jump from
446	one branch to another."""
447
448	sorted = zeros_like(roots)	9✔
449	sorted[0] = roots[0]	9✔
450	for n, row in enumerate(roots[1:], start=1):	9✔
451	# sort the current row by finding the element with the
452	# smallest absolute distance to each root in the
453	# previous row
454	prevrow = sorted[n-1]	9✔
455	available = list(range(len(prevrow)))	9✔
456	for elem in row:	9✔
457	evect = elem - prevrow[available]	9✔
458	ind1 = abs(evect).argmin()	9✔
459	ind = available.pop(ind1)	9✔
460	sorted[n, ind] = elem	9✔
461	return sorted	9✔
462
463
464	# Alternative ways to call these functions
465	root_locus = root_locus_plot	9✔
466	rlocus = root_locus_plot	9✔

python-control / python-control / 13107996232

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous