diff --git a/control/rlocus.py b/control/rlocus.py index e272a833d..e22a31c25 100644 --- a/control/rlocus.py +++ b/control/rlocus.py @@ -56,9 +56,10 @@ __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): + PrintGain=True, grid=False): """Root locus plot Calculate the root locus by finding the roots of 1+k*TF(s) @@ -76,10 +77,12 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, ylim : tuple or list, optional control of y-axis range Plot : boolean, optional (default = True) - If True, plot magnitude and phase + If True, plot root locus diagram. PrintGain: boolean (default = True) If True, report mouse clicks when close to the root-locus branches, calculate gain, damping and print + grid: boolean (default = False) + If True plot s-plane grid. Returns ------- @@ -93,15 +96,22 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, (nump, denp) = _systopoly1d(sys) if kvect is None: - kvect = _default_gains(sys) - - # Compute out the loci - mymat = _RLFindRoots(sys, kvect) - mymat = _RLSortRoots(sys, mymat) + kvect, mymat, xlim, ylim = _default_gains(nump, denp, xlim, ylim) + else: + mymat = _RLFindRoots(nump, denp, kvect) + mymat = _RLSortRoots(mymat) + + # 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 + f = pylab.figure(new_figure_name) - # Create the plot - if (Plot): - f = pylab.figure() if PrintGain: f.canvas.mpl_connect( 'button_release_event', partial(_RLFeedbackClicks, sys=sys)) @@ -113,7 +123,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, # plot open loop zeros zeros = array(nump.r) - if zeros.any(): + if zeros.size > 0: ax.plot(real(zeros), imag(zeros), 'o') # Now plot the loci @@ -127,14 +137,139 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, ax.set_ylim(ylim) ax.set_xlabel('Real') ax.set_ylabel('Imaginary') - + if grid: + _sgrid_func() return mymat, kvect -def _default_gains(sys): - # TODO: update with a smart calculation of the gains using sys poles/zeros - return np.logspace(-3, 3) -# Utility function to extract numerator and denominator polynomials +def _default_gains(num, den, xlim, ylim): + """Unsupervised gains calculation for root locus plot. + + References: + Ogata, K. (2002). Modern control engineering (4th ed.). Upper Saddle River, NJ : New Delhi: Prentice Hall..""" + + k_break, real_break = _break_points(num, den) + kmax = _k_max(num, den, real_break, k_break) + kvect = np.hstack((np.linspace(0, kmax, 50), np.real(k_break))) + kvect.sort() + mymat = _RLFindRoots(num, den, kvect) + mymat = _RLSortRoots(mymat) + 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]) + mymat_xl = np.append(mymat, open_loop_zeros_xl) + else: + mymat_xl = mymat + 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) + mymat_xl = np.append(mymat_xl, important_points) + false_gain = den.coeffs[0] / 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.") + + if xlim is None and false_gain > 0: + 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.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.array([axmax, np.max(np.real(mymat_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(mymat_xl)) - np.min(np.imag(mymat_xl))) + ylim = _ax_lim(mymat_xl * 1j) + 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) + + 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) + 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) + + mymat = _RLSortRoots(mymat) + distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance # distance between points + indexes_too_far = np.where(distance_points) + + 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]) + mymat = np.concatenate((mymat, new_points), axis=0) + mymat = _RLSortRoots(mymat) + return kvect, mymat, xlim, ylim + + +def _break_points(num, den): + """Extract break points over real axis and the gains give these location""" + # 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 + real_break_pts = real_break_pts[num(real_break_pts) != 0] # don't care about infinite break points + 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(mymat): + """Utility to get the axis limits""" + axmin = np.min(np.real(mymat)) + axmax = np.max(np.real(mymat)) + 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 = asymp_center + distance_max * np.exp(asymp_angles * 1j) # farthest points over asymptotes + else: + asymp_angles = asymp_angles + np.pi + farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j) # farthest points over asymptotes + 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 polynomails for a system""" # Allow inputs from the signal processing toolbox @@ -157,29 +292,33 @@ def _systopoly1d(sys): # 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(sys, kvect): +def _RLFindRoots(nump, denp, kvect): """Find the roots for the root locus.""" # Convert numerator and denominator to polynomials if they aren't - (nump, denp) = _systopoly1d(sys) - roots = [] for k in 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.insert(curroots, len(curroots), np.inf) + curroots.sort() roots.append(curroots) + mymat = row_stack(roots) return mymat -def _RLSortRoots(sys, 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 one branch to another.""" @@ -209,6 +348,90 @@ def _RLFeedbackClicks(event, sys): K = -1./sys.horner(s) if abs(K.real) > 1e-8 and abs(K.imag/K.real) < 0.04: print("Clicked at %10.4g%+10.4gj gain %10.4g damp %10.4g" % - (s.real, s.imag, K.real, -1*s.real/abs(s))) + (s.real, s.imag, K.real, -1 * s.real / abs(s))) + + +def _sgrid_func(fig=None, zeta=None, wn=None): + if fig is None: + fig = pylab.gcf() + ax = fig.gca() + 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