diff --git a/control/__init__.py b/control/__init__.py index 99cf9a73d..4746d28a3 100644 --- a/control/__init__.py +++ b/control/__init__.py @@ -67,6 +67,7 @@ from .canonical import * from .robust import * from .config import * +from .sisotool import * # Exceptions from .exception import * diff --git a/control/freqplot.py b/control/freqplot.py index 26783d794..6600a5b4a 100644 --- a/control/freqplot.py +++ b/control/freqplot.py @@ -40,7 +40,7 @@ # SUCH DAMAGE. # # $Id$ - +import matplotlib import matplotlib.pyplot as plt import scipy as sp import numpy as np @@ -89,7 +89,7 @@ def bode_plot(syslist, omega=None, dB=None, Hz=None, deg=None, omega_num: int number of samples margins : boolean - if True, plot gain and phase margin + If True, plot gain and phase margin \*args, \**kwargs: Additional options to matplotlib (color, linestyle, etc) @@ -193,23 +193,35 @@ def bode_plot(syslist, omega=None, dB=None, Hz=None, deg=None, # The code below should work on all cases. # Get the current figure - fig = plt.gcf() - ax_mag = None - ax_phase = None - - # Get the current axes if they already exist - for ax in fig.axes: - if ax.get_label() == 'control-bode-magnitude': - ax_mag = ax - elif ax.get_label() == 'control-bode-phase': - ax_phase = ax - - # If no axes present, create them from scratch - if ax_mag is None or ax_phase is None: - plt.clf() - ax_mag = plt.subplot(211, label='control-bode-magnitude') - ax_phase = plt.subplot(212, label='control-bode-phase', - sharex=ax_mag) + + if 'sisotool' in kwargs: + fig = kwargs['fig'] + ax_mag = fig.axes[0] + ax_phase = fig.axes[2] + sisotool = kwargs['sisotool'] + del kwargs['fig'] + del kwargs['sisotool'] + else: + fig = plt.gcf() + ax_mag = None + ax_phase = None + sisotool = False + + # Get the current axes if they already exist + for ax in fig.axes: + if ax.get_label() == 'control-bode-magnitude': + ax_mag = ax + elif ax.get_label() == 'control-bode-phase': + ax_phase = ax + + # If no axes present, create them from scratch + if ax_mag is None or ax_phase is None: + plt.clf() + ax_mag = plt.subplot(211, + label='control-bode-magnitude') + ax_phase = plt.subplot(212, + label='control-bode-phase', + sharex=ax_mag) # Magnitude plot if dB: @@ -237,58 +249,107 @@ def bode_plot(syslist, omega=None, dB=None, Hz=None, deg=None, if margins: margin = stability_margins(sys) gm, pm, Wcg, Wcp = margin[0], margin[1], margin[3], margin[4] - if pm >= 0.: - phase_limit = -180. - else: + # TODO: add some documentation describing why this is here + phase_at_cp = phases[0][(np.abs(omegas[0] - Wcp)).argmin()] + if phase_at_cp >= 0.: phase_limit = 180. + else: + phase_limit = -180. + + if Hz: + Wcg, Wcp = Wcg/(2*math.pi),Wcp/(2*math.pi) - ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':') + ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':', + zorder=-20) ax_phase.axhline(y=phase_limit if deg else math.radians(phase_limit), - color='k', linestyle=':') + color='k', linestyle=':', zorder=-20) mag_ylim = ax_mag.get_ylim() phase_ylim = ax_phase.get_ylim() if pm != float('inf') and Wcp != float('nan'): if dB: - ax_mag.semilogx([Wcp, Wcp], [0., -1e5], color='k', linestyle=':') + ax_mag.semilogx([Wcp, Wcp], [0.,-1e5], + color='k', linestyle=':', + zorder=-20) else: - ax_mag.loglog([Wcp, Wcp], [1., 1e-8], color='k', linestyle=':') + ax_mag.loglog([Wcp,Wcp], [1.,1e-8],color='k', + linestyle=':', zorder=-20) if deg: - ax_phase.semilogx([Wcp, Wcp], [1e5, phase_limit + pm], - color='k', linestyle=':') - ax_phase.semilogx([Wcp, Wcp], [phase_limit + pm, phase_limit], - color='k') + ax_phase.semilogx([Wcp, Wcp], + [1e5, phase_limit+pm], + color='k', linestyle=':', + zorder=-20) + ax_phase.semilogx([Wcp, Wcp], + [phase_limit + pm, phase_limit], + color='k', zorder=-20) else: - ax_phase.semilogx([Wcp, Wcp], [1e5, math.radians(phase_limit) + - math.radians(pm)], - color='k', linestyle=':') - ax_phase.semilogx([Wcp, Wcp], [math.radians(phase_limit) + - math.radians(pm), - math.radians(phase_limit)], color='k') + ax_phase.semilogx([Wcp, Wcp], + [1e5, math.radians(phase_limit) + + math.radians(pm)], + color='k', linestyle=':', + zorder=-20) + ax_phase.semilogx([Wcp, Wcp], + [math.radians(phase_limit) + + math.radians(pm), + math.radians(phase_limit)], + color='k', zorder=-20) if gm != float('inf') and Wcg != float('nan'): if dB: - ax_mag.semilogx([Wcg, Wcg], [-20. * np.log10(gm), -1e5], color='k', - linestyle=':') - ax_mag.semilogx([Wcg, Wcg], [0, -20 * np.log10(gm)], color='k') + ax_mag.semilogx([Wcg, Wcg], + [-20.*np.log10(gm), -1e5], + color='k', linestyle=':', + zorder=-20) + ax_mag.semilogx([Wcg, Wcg], [0,-20*np.log10(gm)], + color='k', zorder=-20) else: - ax_mag.loglog([Wcg, Wcg], [1. / gm, 1e-8], color='k', linestyle=':') - ax_mag.loglog([Wcg, Wcg], [1., 1. / gm], color='k') + ax_mag.loglog([Wcg, Wcg], + [1./gm,1e-8],color='k', + linestyle=':', zorder=-20) + ax_mag.loglog([Wcg, Wcg], + [1.,1./gm],color='k', zorder=-20) if deg: - ax_phase.semilogx([Wcg, Wcg], [1e-8, phase_limit], - color='k', linestyle=':') + ax_phase.semilogx([Wcg, Wcg], [1e-8, phase_limit], + color='k', linestyle=':', + zorder=-20) else: - ax_phase.semilogx([Wcg, Wcg], [1e-8, math.radians(phase_limit)], - color='k', linestyle=':') + ax_phase.semilogx([Wcg, Wcg], + [1e-8, math.radians(phase_limit)], + color='k', linestyle=':', + zorder=-20) ax_mag.set_ylim(mag_ylim) ax_phase.set_ylim(phase_ylim) - plt.suptitle('Gm = %.2f %s(at %.2f rad/s), Pm = %.2f %s (at %.2f rad/s)' % - (20 * np.log10(gm) if dB else gm, - 'dB ' if dB else '\b', Wcg, pm if deg else math.radians(pm), - 'deg' if deg else 'rad', Wcp)) + + if sisotool: + ax_mag.text(0.04, 0.06, + 'G.M.: %.2f %s\nFreq: %.2f %s' % + (20*np.log10(gm) if dB else gm, + 'dB ' if dB else '', + Wcg, 'Hz' if Hz else 'rad/s'), + horizontalalignment='left', + verticalalignment='bottom', + transform=ax_mag.transAxes, + fontsize=8 if int(matplotlib.__version__[0]) == 1 else 6) + ax_phase.text(0.04, 0.06, + 'P.M.: %.2f %s\nFreq: %.2f %s' % + (pm if deg else math.radians(pm), + 'deg' if deg else 'rad', + Wcp, 'Hz' if Hz else 'rad/s'), + horizontalalignment='left', + verticalalignment='bottom', + transform=ax_phase.transAxes, + fontsize=8 if int(matplotlib.__version__[0]) == 1 else 6) + else: + plt.suptitle('Gm = %.2f %s(at %.2f %s), Pm = %.2f %s (at %.2f %s)' % + (20*np.log10(gm) if dB else gm, + 'dB ' if dB else '\b', + Wcg, 'Hz' if Hz else 'rad/s', + pm if deg else math.radians(pm), + 'deg' if deg else 'rad', + Wcp, 'Hz' if Hz else 'rad/s')) if nyquistfrq_plot: ax_phase.axvline(nyquistfrq_plot, color=pltline[0].get_color()) @@ -302,12 +363,19 @@ def gen_zero_centered_series(val_min, val_max, period): return np.arange(v1, v2 + 1) * period if deg: ylim = ax_phase.get_ylim() - ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], 45.)) - ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], 15.), minor=True) + ax_phase.set_yticks(gen_zero_centered_series(ylim[0], + ylim[1], 45.)) + ax_phase.set_yticks(gen_zero_centered_series(ylim[0], + ylim[1], 15.), + minor=True) else: ylim = ax_phase.get_ylim() - ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], math.pi / 4.)) - ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], math.pi / 12.), + ax_phase.set_yticks(gen_zero_centered_series(ylim[0], + ylim[1], + math.pi / 4.)) + ax_phase.set_yticks(gen_zero_centered_series(ylim[0], + ylim[1], + math.pi / 12.), minor=True) ax_phase.grid(False if margins else True, which='both') # ax_mag.grid(which='minor', alpha=0.3) @@ -316,7 +384,8 @@ def gen_zero_centered_series(val_min, val_max, period): # ax_phase.grid(which='major', alpha=0.9) # Label the frequency axis - ax_phase.set_xlabel("Frequency (Hz)" if Hz else "Frequency (rad/sec)") + ax_phase.set_xlabel("Frequency (Hz)" if Hz + else "Frequency (rad/sec)") if len(syslist) == 1: return mags[0], phases[0], omegas[0] @@ -324,7 +393,8 @@ def gen_zero_centered_series(val_min, val_max, period): return mags, phases, omegas -def nyquist_plot(syslist, omega=None, Plot=True, color=None, labelFreq=0, *args, **kwargs): +def nyquist_plot(syslist, omega=None, Plot=True, color=None, + labelFreq=0, *args, **kwargs): """ Nyquist plot for a system @@ -683,6 +753,10 @@ def gen_prefix(pow1000): 'z', # zepto (10^-21) 'y'][8 - pow1000] # yocto (10^-24) +def find_nearest_omega(omega_list, omega): + omega_list = np.asarray(omega_list) + idx = (np.abs(omega_list - omega)).argmin() + return omega_list[(np.abs(omega_list - omega)).argmin()] # Function aliases bode = bode_plot diff --git a/control/rlocus.py b/control/rlocus.py index 61322624c..a6870159b 100644 --- a/control/rlocus.py +++ b/control/rlocus.py @@ -47,22 +47,24 @@ # Packages used by this module import numpy as np -from scipy import array, poly1d, row_stack, zeros_like, real, imag, exp, sin, cos, linspace, sqrt -from math import pi +import matplotlib +import matplotlib.pyplot as plt +from scipy import array, poly1d, row_stack, zeros_like, real, imag import scipy.signal # signal processing toolbox import pylab # plotting routines from .xferfcn import _convert_to_transfer_function from .exception import ControlMIMONotImplemented +from .sisotool import _SisotoolUpdate from functools import partial from .lti import isdtime from .grid import sgrid, zgrid, nogrid __all__ = ['root_locus', 'rlocus'] - # Main function: compute a root locus diagram -def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, - PrintGain=True, grid=False): +def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='b' if int(matplotlib.__version__[0]) == 1 else 'C0', Plot=True, + PrintGain=True, grid=False, **kwargs): + """Root locus plot Calculate the root locus by finding the roots of 1+k*TF(s) @@ -99,35 +101,42 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, (nump, denp) = _systopoly1d(sys) if kvect is None: + start_mat = _RLFindRoots(nump, denp, [1]) kvect, mymat, xlim, ylim = _default_gains(nump, denp, xlim, ylim) else: + start_mat = _RLFindRoots(nump, denp, [kvect[0]]) mymat = _RLFindRoots(nump, denp, kvect) mymat = _RLSortRoots(mymat) + # Check for sisotool mode + sisotool = False if 'sisotool' not in kwargs else True + # Create the Plot if Plot: - figure_number = pylab.get_fignums() - figure_title = [pylab.figure(numb).canvas.get_window_title() for numb in figure_number] - new_figure_name = "Root Locus" - rloc_num = 1 - while new_figure_name in figure_title: - new_figure_name = "Root Locus " + str(rloc_num) - rloc_num += 1 - if grid: - if isdtime(sys, strict=True): - ax, f = zgrid() - else: - ax, f = sgrid() - else: - ax, f = nogrid() - pylab.title(new_figure_name) + if sisotool: + f = kwargs['fig'] + ax = f.axes[1] - ax = pylab.axes() + else: + figure_number = pylab.get_fignums() + figure_title = [pylab.figure(numb).canvas.get_window_title() for numb in figure_number] + new_figure_name = "Root Locus" + rloc_num = 1 + while new_figure_name in figure_title: + new_figure_name = "Root Locus " + str(rloc_num) + rloc_num += 1 + f = pylab.figure(new_figure_name) + ax = pylab.axes() + + if PrintGain and sisotool == False: + f.canvas.mpl_connect( + 'button_release_event', partial(_RLClickDispatcher,sys=sys, fig=f,ax_rlocus=f.axes[0],plotstr=plotstr)) - if PrintGain: - click_point, = ax.plot([0], [0],color='k',markersize = 0,marker='s',zorder=20) + elif sisotool == True: + f.axes[1].plot([root.real for root in start_mat], [root.imag for root in start_mat], 'm.', marker='s', markersize=8,zorder=20,label='gain_point') + f.suptitle("Clicked at: %10.4g%+10.4gj gain: %10.4g damp: %10.4g" % (start_mat[0][0].real, start_mat[0][0].imag, 1, -1 * start_mat[0][0].real / abs(start_mat[0][0])),fontsize = 12 if int(matplotlib.__version__[0]) == 1 else 10) f.canvas.mpl_connect( - 'button_release_event', partial(_RLFeedbackClicks, sys=sys,fig=f,point=click_point)) + 'button_release_event',partial(_RLClickDispatcher,sys=sys, fig=f,ax_rlocus=f.axes[1],plotstr=plotstr, sisotool=sisotool, bode_plot_params=kwargs['bode_plot_params'],tvect=kwargs['tvect'])) # plot open loop poles poles = array(denp.r) @@ -139,20 +148,30 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, ax.plot(real(zeros), imag(zeros), 'o') # Now plot the loci - for col in mymat.T: - ax.plot(real(col), imag(col), plotstr, lw=3) + for index,col in enumerate(mymat.T): + ax.plot(real(col), imag(col), plotstr,label='rootlocus') # Set up plot axes and labels if xlim: ax.set_xlim(xlim) if ylim: ax.set_ylim(ylim) + ax.set_xlabel('Real') + ax.set_ylabel('Imaginary') + if grid and sisotool: + _sgrid_func(f) + elif grid: + _sgrid_func() + else: + ax.axhline(0., linestyle=':', color='k',zorder=-20) + ax.axvline(0., linestyle=':', color='k') + return mymat, kvect -def _default_gains(num, den, xlim, ylim): - """Unsupervised gains calculation for root locus plot. - +def _default_gains(num, den, xlim, ylim,zoom_xlim=None,zoom_ylim=None): + """Unsupervised gains calculation for root locus plot. + References: Ogata, K. (2002). Modern control engineering (4th ed.). Upper Saddle River, NJ : New Delhi: Prentice Hall..""" @@ -176,7 +195,8 @@ def _default_gains(num, den, xlim, ylim): important_points = np.concatenate((singular_points, real_break), axis=0) important_points = np.concatenate((important_points, np.zeros(2)), axis=0) mymat_xl = np.append(mymat_xl, important_points) - false_gain = den.coeffs[0] / num.coeffs[0] + + false_gain = float(den.coeffs[0]) / float(num.coeffs[0]) if false_gain < 0 and not den.order > num.order: raise ValueError("Not implemented support for 0 degrees root " "locus with equal order of numerator and denominator.") @@ -185,9 +205,11 @@ def _default_gains(num, den, xlim, ylim): x_tolerance = 0.05 * (np.max(np.real(mymat_xl)) - np.min(np.real(mymat_xl))) xlim = _ax_lim(mymat_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.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(mymat_xl))])) - axmax = np.max(np.real(important_points))+np.max(np.real(important_points))-np.min(np.real(important_points)) + 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(mymat_xl))])) xlim = [axmin, axmax] x_tolerance = 0.05 * (axmax - axmin) @@ -200,20 +222,19 @@ def _default_gains(num, den, xlim, ylim): else: y_tolerance = 0.05 * (ylim[1] - ylim[0]) - tolerance = np.max([x_tolerance, y_tolerance]) - distance_points = np.abs(np.diff(mymat, axis=0)) - indexes_too_far = np.where(distance_points > tolerance) + tolerance = np.min([x_tolerance, y_tolerance]) + indexes_too_far = _indexes_filt(mymat,tolerance,zoom_xlim,zoom_ylim) - while (indexes_too_far[0].size > 0) and (kvect.size < 5000): - for index in indexes_too_far[0]: - new_gains = np.linspace(kvect[index], kvect[index+1], 5) + 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]) - mymat = np.insert(mymat, index+1, new_points, axis=0) + kvect = np.insert(kvect, index + 1, new_gains[1:4]) + mymat = np.insert(mymat, index + 1, new_points, axis=0) mymat = _RLSortRoots(mymat) - distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance # distance between points - indexes_too_far = np.where(distance_points) + indexes_too_far = _indexes_filt(mymat,tolerance,zoom_xlim,zoom_ylim) new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4))) new_points = _RLFindRoots(num, den, new_gains[1:4]) @@ -222,6 +243,49 @@ def _default_gains(num, den, xlim, ylim): mymat = _RLSortRoots(mymat) return kvect, mymat, xlim, ylim +def _indexes_filt(mymat,tolerance,zoom_xlim=None,zoom_ylim=None): + """Calculate the distance between points and return the indexes. + 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(mymat, axis=0)) + indexes_too_far = list(np.unique(np.where(distance_points > tolerance)[0])) + + if zoom_xlim != None and zoom_ylim != None: + x_tolerance_zoom = 0.05 * (zoom_xlim[1] - zoom_xlim[0]) + y_tolerance_zoom = 0.05 * (zoom_ylim[1] - zoom_ylim[0]) + tolerance_zoom = np.min([x_tolerance_zoom, y_tolerance_zoom]) + indexes_too_far_zoom = list(np.unique(np.where(distance_points > tolerance_zoom)[0])) + indexes_too_far_filtered = [] + + for index in indexes_too_far_zoom: + for point in mymat[index]: + if (zoom_xlim[0] <= point.real <= zoom_xlim[1]) and (zoom_ylim[0] <= point.imag <= zoom_ylim[1]): + indexes_too_far_filtered.append(index) + break + + # Check if the zoom box is not overshot and insert points where neccessary + if len(indexes_too_far_filtered) == 0 and len(mymat) <300: + limits = [zoom_xlim[0],zoom_xlim[1],zoom_ylim[0],zoom_ylim[1]] + for index,limit in enumerate(limits): + if index <= 1: + asign = np.sign(real(mymat)-limit) + else: + asign = np.sign(imag(mymat) - limit) + signchange = ((np.roll(asign, 1, axis=0) - asign) != 0).astype(int) + signchange[0] = np.zeros((len(mymat[0]))) + if len(np.where(signchange ==1)[0]) > 0: + indexes_too_far_filtered.append(np.where(signchange == 1)[0][0]-1) + + if len(indexes_too_far_filtered) > 0 : + if indexes_too_far_filtered[0] != 0: + indexes_too_far_filtered.insert(0,indexes_too_far_filtered[0]-1) + if not indexes_too_far_filtered[-1] +1 >= len(mymat)-2: + indexes_too_far_filtered.append(indexes_too_far_filtered[-1]+1) + + indexes_too_far.extend(indexes_too_far_filtered) + + indexes_too_far = list(np.unique(indexes_too_far)) + indexes_too_far.sort() + return indexes_too_far def _break_points(num, den): """Extract break points over real axis and the gains give these location""" @@ -310,7 +374,6 @@ def _systopoly1d(sys): 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 kvect: @@ -326,7 +389,6 @@ def _RLFindRoots(nump, denp, kvect): mymat = row_stack(roots) return mymat - def _RLSortRoots(mymat): """Sort the roots from sys._RLFindRoots, so that the root locus doesn't show weird pseudo-branches as roots jump from @@ -349,20 +411,161 @@ def _RLSortRoots(mymat): prevrow = sorted[n, :] return sorted +def _RLClickDispatcher(event,sys,fig,ax_rlocus,plotstr,sisotool=False,bode_plot_params=None,tvect=None): + """Rootlocus plot click dispatcher""" -def _RLFeedbackClicks(event, sys,fig,point): - """Print root-locus gain feedback for clicks on the root-locus plot - """ - s = complex(event.xdata, event.ydata) - K = -1./sys.horner(s) - if abs(K.real) > 1e-8 and abs(K.imag/K.real) < 0.04: + # If zoom is used on the rootlocus plot smooth and update it + if plt.get_current_fig_manager().toolbar.mode in ['zoom rect','pan/zoom'] and event.inaxes == ax_rlocus.axes: + (nump, denp) = _systopoly1d(sys) + xlim,ylim = ax_rlocus.get_xlim(),ax_rlocus.get_ylim() + + kvect,mymat, xlim,ylim = _default_gains(nump, denp,xlim=None,ylim=None, zoom_xlim=xlim,zoom_ylim=ylim) + _removeLine('rootlocus', ax_rlocus) + + for i,col in enumerate(mymat.T): + ax_rlocus.plot(real(col), imag(col), plotstr,label='rootlocus') + + # if a point is clicked on the rootlocus plot visually emphasize it + else: + K = _RLFeedbackClicksPoint(event, sys, fig,ax_rlocus,sisotool) + if sisotool and K is not None: + _SisotoolUpdate(sys, fig, K, bode_plot_params, tvect) + + # Update the canvas + fig.canvas.draw() + + +def _RLFeedbackClicksPoint(event,sys,fig,ax_rlocus,sisotool=False): + """Display root-locus gain feedback point for clicks on the root-locus plot""" + + (nump, denp) = _systopoly1d(sys) + + # 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.horner(s) + + except TypeError: + K = float('inf') + + xlim = ax_rlocus.get_xlim() + ylim = ax_rlocus.get_ylim() + x_tolerance = 0.05 * (xlim[1] - xlim[0]) + y_tolerance = 0.05 * (ylim[1] - ylim[0]) + gain_tolerance = np.min([x_tolerance, y_tolerance])*1e-1 + + if abs(K.real) > 1e-8 and abs(K.imag / K.real) < gain_tolerance and event.inaxes == ax_rlocus.axes: + + # Display the parameters in the output window and figure print("Clicked at %10.4g%+10.4gj gain %10.4g damp %10.4g" % (s.real, s.imag, K.real, -1 * s.real / abs(s))) - point.set_ydata(s.imag) - point.set_xdata(s.real) - point.set_markersize(8) fig.suptitle("Clicked at: %10.4g%+10.4gj gain: %10.4g damp: %10.4g" % - (s.real, s.imag, K.real, -1 * s.real / abs(s))) - fig.canvas.draw() + (s.real, s.imag, K.real, -1 * s.real / abs(s)),fontsize = 12 if int(matplotlib.__version__[0]) == 1 else 10) + + # Remove the previous line + _removeLine(label='gain_point',ax=ax_rlocus) + + # Visualise clicked point, display all roots for sisotool mode + if sisotool: + mymat = _RLFindRoots(nump, denp, K.real) + ax_rlocus.plot([root.real for root in mymat], [root.imag for root in mymat], 'm.', marker='s', markersize=8, + zorder=20,label='gain_point') + else: + ax_rlocus.plot(s.real, s.imag, 'k.', marker='s', markersize=8, zorder=20,label='gain_point') + + return K.real[0][0] + + +def _removeLine(label,ax): + """Remove a line from the ax when a label is specified""" + for line in reversed(ax.lines): + if line.get_label() == label: + line.remove() + del line + +def _sgrid_func(fig=None, zeta=None, wn=None): + if fig is None: + fig = pylab.gcf() + ax = fig.gca() + else: + ax = fig.axes[1] + xlocator = ax.get_xaxis().get_major_locator() + + ylim = ax.get_ylim() + ytext_pos_lim = ylim[1] - (ylim[1] - ylim[0]) * 0.03 + xlim = ax.get_xlim() + xtext_pos_lim = xlim[0] + (xlim[1] - xlim[0]) * 0.0 + + if zeta is None: + zeta = _default_zetas(xlim, ylim) + + angules = [] + for z in zeta: + if (z >= 1e-4) and (z <= 1): + angules.append(np.pi/2 + np.arcsin(z)) + else: + zeta.remove(z) + y_over_x = np.tan(angules) + + # zeta-constant lines + + index = 0 + + for yp in y_over_x: + ax.plot([0, xlocator()[0]], [0, yp*xlocator()[0]], color='gray', + linestyle='dashed', linewidth=0.5) + ax.plot([0, xlocator()[0]], [0, -yp * xlocator()[0]], color='gray', + linestyle='dashed', linewidth=0.5) + an = "%.2f" % zeta[index] + if yp < 0: + xtext_pos = 1/yp * ylim[1] + ytext_pos = yp * xtext_pos_lim + if np.abs(xtext_pos) > np.abs(xtext_pos_lim): + xtext_pos = xtext_pos_lim + else: + ytext_pos = ytext_pos_lim + ax.annotate(an, textcoords='data', xy=[xtext_pos, ytext_pos], fontsize=8) + index += 1 + ax.plot([0, 0], [ylim[0], ylim[1]], color='gray', linestyle='dashed', linewidth=0.5) + + angules = np.linspace(-90, 90, 20)*np.pi/180 + if wn is None: + wn = _default_wn(xlocator(), ylim) + + for om in wn: + if om < 0: + yp = np.sin(angules)*np.abs(om) + xp = -np.cos(angules)*np.abs(om) + ax.plot(xp, yp, color='gray', + linestyle='dashed', linewidth=0.5) + an = "%.2f" % -om + ax.annotate(an, textcoords='data', xy=[om, 0], fontsize=8) + + +def _default_zetas(xlim, ylim): + """Return default list of dumps coefficients""" + sep1 = -xlim[0]/4 + ang1 = [np.arctan((sep1*i)/ylim[1]) for i in np.arange(1, 4, 1)] + sep2 = ylim[1] / 3 + ang2 = [np.arctan(-xlim[0]/(ylim[1]-sep2*i)) for i in np.arange(1, 3, 1)] + + angules = np.concatenate((ang1, ang2)) + angules = np.insert(angules, len(angules), np.pi/2) + zeta = np.sin(angules) + return zeta.tolist() + + +def _default_wn(xloc, ylim): + """Return default wn for root locus plot""" + + wn = xloc + sep = xloc[1]-xloc[0] + while np.abs(wn[0]) < ylim[1]: + wn = np.insert(wn, 0, wn[0]-sep) + + while len(wn) > 7: + wn = wn[0:-1:2] + + return wn rlocus = root_locus diff --git a/control/sisotool.py b/control/sisotool.py new file mode 100644 index 000000000..7a312cf5c --- /dev/null +++ b/control/sisotool.py @@ -0,0 +1,146 @@ +__all__ = ['sisotool'] + +from .freqplot import bode_plot +from .timeresp import step_response +from .lti import issiso +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): + """ + Sisotool style collection of plots inspired by MATLAB's sisotool. + The left two plots contain the bode magnitude and phase diagrams. + The top right plot is a clickable root locus plot, clicking on the + root locus will change the gain of the system. The bottom left plot + shows a closed loop time response. + + Parameters + ---------- + sys : LTI object + Linear input/output systems (SISO only) + kvect : list or ndarray, optional + List of gains to use for plotting root locus + xlim_rlocus : tuple or list, optional + control of x-axis range, normally with tuple (see matplotlib.axes) + ylim_rlocus : tuple or list, optional + control of y-axis range + plotstr_rlocus : Additional options to matplotlib + 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 + 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 + 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 + margins_bode : boolean + If True, plot gain and phase margin in the bode plot + tvect : list or ndarray, optional + List of timesteps to use for closed loop step response + + Examples + -------- + >>> sys = tf([1000], [1,25,100,0]) + >>> sisotool(sys) + + """ + from .rlocus import root_locus + + # Check if it is a single SISO system + issiso(sys,strict=True) + + # Setup sisotool figure or superimpose if one is already present + fig = plt.gcf() + if fig.canvas.get_window_title() != 'Sisotool': + plt.close(fig) + fig,axes = plt.subplots(2, 2) + fig.canvas.set_window_title('Sisotool') + + # Extract bode plot parameters + bode_plot_params = { + 'omega': omega, + 'dB': dB, + 'Hz': Hz, + 'deg': deg, + 'omega_limits': omega_limits, + 'omega_num' : omega_num, + 'sisotool': True, + 'fig': fig, + 'margins': margins_bode + } + + # 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) + + # 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) + +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 + + # 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] + + # 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() + + # Update the bodeplot + bode_plot_params['syslist'] = sys*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_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_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.get_xaxis().set_label_coords(0.5, -0.15) + ax_step.get_yaxis().set_label_coords(-0.15, 0.5) + + 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) + + # Generate the step response and plot it + sys_closed = (K*sys).feedback(1) + if tvect is None: + tvect, yout = step_response(sys_closed) + else: + tvect, yout = step_response(sys_closed,tvect) + ax_step.plot(tvect, yout) + ax_step.axhline(1.,linestyle=':',color='k',zorder=-20) + + # Manually adjust the spacing and draw the canvas + fig.subplots_adjust(top=0.9,wspace = 0.3,hspace=0.35) + fig.canvas.draw() + diff --git a/control/tests/freqresp_test.py b/control/tests/freqresp_test.py index 36493e4d9..a06e4e8e2 100644 --- a/control/tests/freqresp_test.py +++ b/control/tests/freqresp_test.py @@ -113,7 +113,7 @@ def test_bode_margin(self): num = [1000] den = [1, 25, 100, 0] sys = ctrl.tf(num, den) - ctrl.bode_plot(sys, margins=True,dB=False,deg = True) + ctrl.bode_plot(sys, margins=True,dB=False,deg = True, Hz=False) fig = plt.gcf() allaxes = fig.get_axes() @@ -136,7 +136,7 @@ def test_bode_margin(self): phase_to_infinity = (np.array([10., 10.]), np.array([1.00000000e-08, -1.80000000e+02])) assert_array_almost_equal(phase_to_infinity, allaxes[1].lines[4].get_data()) - + def test_discrete(self): # Test discrete time frequency response diff --git a/control/tests/rlocus_test.py b/control/tests/rlocus_test.py index d2522c881..464f04066 100644 --- a/control/tests/rlocus_test.py +++ b/control/tests/rlocus_test.py @@ -5,10 +5,13 @@ import unittest import numpy as np -from control.rlocus import root_locus +from control.rlocus import root_locus, _RLClickDispatcher from control.xferfcn import TransferFunction from control.statesp import StateSpace from control.bdalg import feedback +import matplotlib.pyplot as plt +from control.tests.margin_test import assert_array_almost_equal + class TestRootLocus(unittest.TestCase): """These are tests for the feedback function in rlocus.py.""" @@ -42,6 +45,29 @@ def test_without_gains(self): roots, kvect = root_locus(sys, Plot=False) self.check_cl_poles(sys, roots, kvect) + def test_root_locus_zoom(self): + """Check the zooming functionality of the Root locus plot""" + system = TransferFunction([1000], [1, 25, 100, 0]) + root_locus(system) + fig = plt.gcf() + ax_rlocus = fig.axes[0] + + event = type('test', (object,), {'xdata': 14.7607954359, 'ydata': -35.6171379864, 'inaxes': ax_rlocus.axes})() + ax_rlocus.set_xlim((-10.813628105112421, 14.760795435937652)) + ax_rlocus.set_ylim((-35.61713798641108, 33.879716621220311)) + plt.get_current_fig_manager().toolbar.mode = 'zoom rect' + _RLClickDispatcher(event, system, fig, ax_rlocus, '-') + + zoom_x = ax_rlocus.lines[-2].get_data()[0][0:5] + zoom_y = ax_rlocus.lines[-2].get_data()[1][0:5] + zoom_y = [abs(y) for y in zoom_y] + + zoom_x_valid = [-5. ,- 4.61281263, - 4.16689986, - 4.04122642, - 3.90736502] + zoom_y_valid = [0. ,0., 0., 0., 0.] + + assert_array_almost_equal(zoom_x,zoom_x_valid) + assert_array_almost_equal(zoom_y,zoom_y_valid) + def test_suite(): return unittest.TestLoader().loadTestsFromTestCase(TestRootLocus) diff --git a/control/tests/sisotool_test.py b/control/tests/sisotool_test.py new file mode 100644 index 000000000..40ef0f966 --- /dev/null +++ b/control/tests/sisotool_test.py @@ -0,0 +1,69 @@ +import unittest +import numpy as np +from control.sisotool import sisotool +from control.tests.margin_test import assert_array_almost_equal +from control.rlocus import _RLClickDispatcher +from control.xferfcn import TransferFunction +import matplotlib.pyplot as plt + +class TestSisotool(unittest.TestCase): + """These are tests for the sisotool in sisotool.py.""" + + def setUp(self): + # One random SISO system. + self.system = TransferFunction([1000],[1,25,100,0]) + + def test_sisotool(self): + sisotool(self.system,Hz=False) + fig = plt.gcf() + ax_mag,ax_rlocus,ax_phase,ax_step = fig.axes[0],fig.axes[1],fig.axes[2],fig.axes[3] + + # 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])) + assert_array_almost_equal(ax_rlocus.lines[0].get_data(),initial_point_0) + assert_array_almost_equal(ax_rlocus.lines[1].get_data(),initial_point_1) + assert_array_almost_equal(ax_rlocus.lines[2].get_data(),initial_point_2) + + # Check the step response before moving the point + step_response_original = np.array([ 0., 0.02233651, 0.13118374, 0.33078542, 0.5907113, 0.87041549, 1.13038536, 1.33851053, 1.47374666, 1.52757114]) + assert_array_almost_equal(ax_step.lines[0].get_data()[1][:10],step_response_original) + + bode_plot_params = { + 'omega': None, + 'dB': False, + 'Hz': False, + 'deg': True, + 'omega_limits': None, + 'omega_num': None, + 'sisotool': True, + 'fig': fig, + 'margins': True + } + + # Move the rootlocus to another point + event = type('test', (object,), {'xdata': 2.31206868287,'ydata':15.5983051046, 'inaxes':ax_rlocus.axes})() + _RLClickDispatcher(event=event, sys=self.system, fig=fig,ax_rlocus=ax_rlocus,sisotool=True, plotstr='-' ,bode_plot_params=bode_plot_params, tvect=None) + + # Check the moved root locus plot points + moved_point_0 = (np.array([-29.91742755]), np.array([0.])) + moved_point_1 = (np.array([2.45871378]), np.array([-15.52647768])) + moved_point_2 = (np.array([2.45871378]), np.array([15.52647768])) + assert_array_almost_equal(ax_rlocus.lines[-3].get_data(),moved_point_0) + assert_array_almost_equal(ax_rlocus.lines[-2].get_data(),moved_point_1) + assert_array_almost_equal(ax_rlocus.lines[-1].get_data(),moved_point_2) + + # Check if the bode_mag line has moved + bode_mag_moved = np.array([ 111.83321224, 92.29238035, 76.02822315, 62.46884113, 51.14108703, 41.6554004, 33.69409534, 27.00237344, 21.38086717, 16.67791585]) + assert_array_almost_equal(ax_mag.lines[0].get_data()[1][10:20],bode_mag_moved) + + # Check if the step response has changed + step_response_moved = np.array([[ 0., 0.02458187, 0.16529784 , 0.46602716 , 0.91012035 , 1.43364313, 1.93996334 , 2.3190105 , 2.47041552 , 2.32724853] ]) + assert_array_almost_equal(ax_step.lines[0].get_data()[1][:10],step_response_moved) + +def test_suite(): + return unittest.TestLoader().loadTestsFromTestCase(TestSisotool) + +if __name__ == "__main__": + unittest.main()