Expand Up @@ -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) Expand All @@ -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, plotmagnitude and phase If True, plotroot 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 ------- Expand All @@ -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)) Expand All @@ -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 Expand All @@ -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 Expand All @@ -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.""" Expand Down Expand Up @@ -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