From ffc304158d0016089fcc7698bcd4a01f2f45c1d1 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Mon, 20 Mar 2017 22:22:11 -0300
Subject: [PATCH 01/20] add unsupervised calculation on gains for root locus
 plot.

---
 control/rlocus.py | 116 +++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 104 insertions(+), 12 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index e272a833d..07281f49a 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -56,6 +56,7 @@
 
 __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):
@@ -93,11 +94,10 @@ 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):
@@ -130,11 +130,104 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
 
     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"""
+    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) & (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
+
+    if xlim is None:
+        x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
+        xlim = _ax_lim(mymat_xl)
+    else:
+        x_tolerance = 0.05 * (xlim[1] - xlim[0])
+    if ylim is None:
+        y_tolerance = 0.05 * (np.max(np.max(np.imag(mymat_xl))) - np.min(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) & (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))
+        indexes_too_far = np.where(distance_points > tolerance)
+    new_gains = np.hstack((np.logspace(np.log10(kvect[-1]), np.log10(kvect[-1]*200), 10)))
+    new_points = _RLFindRoots(num, den, new_gains[1:10])
+    kvect = np.append(kvect, new_gains[1:10])
+    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]
+    return k_break, real_break_pts
+
+
+def _ax_lim(mymat):
+    """Utility to get the axis limits"""
+    axmin = np.min(np.min(np.real(mymat)))
+    axmax = np.max(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):
+    """" Calculation the maximum gain for the root locus shown in tne 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)
+
+    if asymp_number > 0:
+        asymp_center = (np.sum(den.roots) - np.sum(num.roots))/asymp_number
+        distance_max = 2 * np.max(np.abs(important_points - asymp_center))
+        asymp_angles = (2 * np.arange(0, asymp_number)-1) * np.pi / asymp_number
+        farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
+        kmax_asymp = -den(farthest_points) / num(farthest_points)
+    else:
+        farthest_points = 2 * np.max(np.abs(important_points))
+        kmax_asymp = -den(farthest_points) / num(farthest_points)
+        if kmax_asymp == 0:
+            kmax_asymp = -den(1) / num(1)
+    kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
+    return kmax
+
+
 def _systopoly1d(sys):
     """Extract numerator and denominator polynomails for a system"""
     # Allow inputs from the signal processing toolbox
@@ -163,11 +256,10 @@ def _systopoly1d(sys):
     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:
@@ -179,7 +271,7 @@ def _RLFindRoots(sys, kvect):
     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."""

From 2aada8b1e0aaa73211fcccbfb79d02ccc5fbdbd0 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Mon, 20 Mar 2017 23:31:41 -0300
Subject: [PATCH 02/20] fix bug concatenating zero dimensional array

---
 control/rlocus.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 07281f49a..108980568 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -172,7 +172,7 @@ def _default_gains(num, den, xlim, ylim):
         mymat = _RLSortRoots(mymat)
         distance_points = np.abs(np.diff(mymat, axis=0))
         indexes_too_far = np.where(distance_points > tolerance)
-    new_gains = np.hstack((np.logspace(np.log10(kvect[-1]), np.log10(kvect[-1]*200), 10)))
+    new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*200, 10)))
     new_points = _RLFindRoots(num, den, new_gains[1:10])
     kvect = np.append(kvect, new_gains[1:10])
     mymat = np.concatenate((mymat, new_points), axis=0)
@@ -192,6 +192,9 @@ def _break_points(num, den):
     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
 
 
@@ -219,6 +222,8 @@ def _k_max(num, den, real_break_points, k_break_points):
         asymp_angles = (2 * np.arange(0, asymp_number)-1) * np.pi / asymp_number
         farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
         kmax_asymp = -den(farthest_points) / num(farthest_points)
+        if kmax_asymp == 0:
+            kmax_asymp = [den.coeffs[0]/num.coeffs[0]*3]
     else:
         farthest_points = 2 * np.max(np.abs(important_points))
         kmax_asymp = -den(farthest_points) / num(farthest_points)

From 71304344ffe724840e2af200d891fc9bc1d96b43 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Mon, 20 Mar 2017 23:55:21 -0300
Subject: [PATCH 03/20] fix bug concatenating zero dimensional when test in
 matlab_test.py

---
 control/rlocus.py | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 108980568..d244d947e 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -222,13 +222,11 @@ def _k_max(num, den, real_break_points, k_break_points):
         asymp_angles = (2 * np.arange(0, asymp_number)-1) * np.pi / asymp_number
         farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
         kmax_asymp = -den(farthest_points) / num(farthest_points)
-        if kmax_asymp == 0:
-            kmax_asymp = [den.coeffs[0]/num.coeffs[0]*3]
     else:
         farthest_points = 2 * np.max(np.abs(important_points))
-        kmax_asymp = -den(farthest_points) / num(farthest_points)
-        if kmax_asymp == 0:
-            kmax_asymp = -den(1) / num(1)
+        kmax_asymp = [-den(farthest_points) / num(farthest_points)]
+    if kmax_asymp == 0:
+        kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
     kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
     return kmax
 

From 8b829ccfcc051483c277c899af39e748a362a260 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Tue, 21 Mar 2017 00:13:08 -0300
Subject: [PATCH 04/20] fix bug with array==0! problem test in matlab_test.py

---
 control/rlocus.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index d244d947e..b4653740d 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -225,7 +225,7 @@ def _k_max(num, den, real_break_points, k_break_points):
     else:
         farthest_points = 2 * np.max(np.abs(important_points))
         kmax_asymp = [-den(farthest_points) / num(farthest_points)]
-    if kmax_asymp == 0:
+    if np.max(kmax_asymp) == 0:
         kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
     kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
     return kmax

From d4cbc958a123e864307a60d9a88bf1ea2d5ee4ee Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Wed, 22 Mar 2017 22:14:22 -0300
Subject: [PATCH 05/20] fix bug with axis limits with no asymptotes fix bug
 plotting zeros when only zeros in zero exist

---
 control/rlocus.py | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index b4653740d..ee28497d3 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -113,7 +113,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
@@ -148,6 +148,9 @@ def _default_gains(num, den, xlim, ylim):
         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)
+    mymat_xl = np.append(mymat_xl, important_points)
 
     if xlim is None:
         x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
@@ -223,11 +226,11 @@ def _k_max(num, den, real_break_points, k_break_points):
         farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
         kmax_asymp = -den(farthest_points) / num(farthest_points)
     else:
-        farthest_points = 2 * np.max(np.abs(important_points))
-        kmax_asymp = [-den(farthest_points) / num(farthest_points)]
-    if np.max(kmax_asymp) == 0:
         kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
-    kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
+    if np.max(kmax_asymp) == 0:
+        kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
+    else:
+        kmax = kmax_asymp
     return kmax
 
 

From 6d55d573d3618ed3ba25030beaf042bb1bc41b68 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Wed, 22 Mar 2017 22:55:30 -0300
Subject: [PATCH 06/20] fix bug calculating kmax

---
 control/rlocus.py | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index ee28497d3..93c1a9b78 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -227,10 +227,8 @@ def _k_max(num, den, real_break_points, k_break_points):
         kmax_asymp = -den(farthest_points) / num(farthest_points)
     else:
         kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
-    if np.max(kmax_asymp) == 0:
-        kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
-    else:
-        kmax = kmax_asymp
+
+    kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
     return kmax
 
 

From 01e33ea6ef9e606a36909e36129fe9fa85e6fdcc Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Mon, 20 Mar 2017 22:22:11 -0300
Subject: [PATCH 07/20] add unsupervised calculation on gains for root locus
 plot.

---
 control/rlocus.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 93c1a9b78..b4653740d 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -113,7 +113,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
 
         # plot open loop zeros
         zeros = array(nump.r)
-        if zeros.size > 0:
+        if zeros.any():
             ax.plot(real(zeros), imag(zeros), 'o')
 
         # Now plot the loci
@@ -148,9 +148,6 @@ def _default_gains(num, den, xlim, ylim):
         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)
-    mymat_xl = np.append(mymat_xl, important_points)
 
     if xlim is None:
         x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
@@ -226,8 +223,10 @@ def _k_max(num, den, real_break_points, k_break_points):
         farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
         kmax_asymp = -den(farthest_points) / num(farthest_points)
     else:
+        farthest_points = 2 * np.max(np.abs(important_points))
+        kmax_asymp = [-den(farthest_points) / num(farthest_points)]
+    if np.max(kmax_asymp) == 0:
         kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
-
     kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
     return kmax
 

From 95d69080af42a541ae98b2142d1a99ec377397d0 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Wed, 22 Mar 2017 22:14:22 -0300
Subject: [PATCH 08/20] fix bug with axis limits with no asymptotes fix bug
 plotting zeros when only zeros in zero exist

---
 control/rlocus.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index b4653740d..93c1a9b78 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -113,7 +113,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
@@ -148,6 +148,9 @@ def _default_gains(num, den, xlim, ylim):
         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)
+    mymat_xl = np.append(mymat_xl, important_points)
 
     if xlim is None:
         x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
@@ -223,10 +226,8 @@ def _k_max(num, den, real_break_points, k_break_points):
         farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
         kmax_asymp = -den(farthest_points) / num(farthest_points)
     else:
-        farthest_points = 2 * np.max(np.abs(important_points))
-        kmax_asymp = [-den(farthest_points) / num(farthest_points)]
-    if np.max(kmax_asymp) == 0:
         kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
+
     kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
     return kmax
 

From 1b20bd4e97adda170d9b2d4148b206287a81588d Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Fri, 31 Mar 2017 14:27:38 -0300
Subject: [PATCH 09/20] add sgrid to rlocus plot, to the last version of
 unsupervised gains.

---
 control/rlocus.py | 77 ++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 72 insertions(+), 5 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 93c1a9b78..1e14db1b8 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -59,7 +59,7 @@
 
 # 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)
@@ -99,9 +99,17 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
         mymat = _RLFindRoots(nump, denp, kvect)
         mymat = _RLSortRoots(mymat)
 
-    # Create the plot
-    if (Plot):
-        f = pylab.figure()
+    # 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)
+
         if PrintGain:
             f.canvas.mpl_connect(
                 'button_release_event', partial(_RLFeedbackClicks, sys=sys))
@@ -127,6 +135,8 @@ 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(f)
 
     return mymat, kvect
 
@@ -305,6 +315,63 @@ 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):
+    ax = fig.gca()
+    ylocator = ax.get_yaxis().get_major_locator()
+    xlocator = ax.get_yaxis().get_major_locator()
+
+    long_xaxis = xlocator()[-1] - xlocator()[0]
+    long_yaxis = ylocator()[-1] - ylocator()[0]
+
+    angules = np.arange(-90, 80, 15)*np.pi/180
+
+    # radial lines
+    y_over_x = np.tan(angules[1::])*ylocator()[-1]/xlocator()[-1]
+
+    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
+
+    index = 0
+    zeta = np.sin(np.pi/2-angules[1::])
+
+    for yp in y_over_x:
+        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)
+        elif 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
+                ytext_pos = - ytext_pos
+            else:
+                ytext_pos = ylim[0]
+                xtext_pos = -xtext_pos
+            ax.annotate(an, textcoords='data', xy=[xtext_pos, ytext_pos], fontsize=8)
+        index += 1
+
+    angules = np.linspace(-90, 90, 20)*np.pi/180
+    for xt in xlocator():
+        if xt < 0:
+            yp = np.sin(angules)*np.abs(xt)
+            xp = -np.cos(angules)*np.abs(xt)
+            ax.plot(xp, yp, color='gray',
+                    linestyle='dashed', linewidth=0.5)
+            an = "%.2f" % -xt
+            ax.annotate(an, textcoords='data', xy=[xt, 0], fontsize=8)
 
 rlocus = root_locus

From 5e47d1f128f1d2a573218e47c170d52ae02c00cd Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Fri, 31 Mar 2017 14:48:08 -0300
Subject: [PATCH 10/20] add origin to rlocus plot and x=0 line (zeta =0 grid
 line).

---
 control/rlocus.py | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 1e14db1b8..f76050aac 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -160,6 +160,7 @@ def _default_gains(num, den, xlim, ylim):
         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((singular_points, np.zeros(2)), axis=0)
     mymat_xl = np.append(mymat_xl, important_points)
 
     if xlim is None:
@@ -323,20 +324,14 @@ def sgrid_func(fig):
     ylocator = ax.get_yaxis().get_major_locator()
     xlocator = ax.get_yaxis().get_major_locator()
 
-    long_xaxis = xlocator()[-1] - xlocator()[0]
-    long_yaxis = ylocator()[-1] - ylocator()[0]
-
     angules = np.arange(-90, 80, 15)*np.pi/180
 
-    # radial lines
+    # zeta-constant lines
     y_over_x = np.tan(angules[1::])*ylocator()[-1]/xlocator()[-1]
-
     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
-
     index = 0
     zeta = np.sin(np.pi/2-angules[1::])
 
@@ -363,6 +358,7 @@ def sgrid_func(fig):
                 xtext_pos = -xtext_pos
             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
     for xt in xlocator():

From ebd39047108c35712cfc67fe819f3186e85c256c Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Fri, 31 Mar 2017 14:52:33 -0300
Subject: [PATCH 11/20] add grid arguments in docstring.

---
 control/rlocus.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index f76050aac..1edb63011 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -59,7 +59,7 @@
 
 # Main function: compute a root locus diagram
 def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
-               PrintGain=True, grid = False):
+               PrintGain=True, grid=False):
     """Root locus plot
 
     Calculate the root locus by finding the roots of 1+k*TF(s)
@@ -77,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
     -------

From de87bb0ff1b8bace2efc78efd45d19c81bd5affc Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Sun, 2 Apr 2017 12:22:01 -0300
Subject: [PATCH 12/20] reorder code that calculate defaults damp coefficents
 constant lines and wn constant lines. grids ok in most of the examples.

---
 control/rlocus.py | 69 +++++++++++++++++++++++++++++------------------
 1 file changed, 43 insertions(+), 26 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 1edb63011..9e446ad50 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -139,7 +139,6 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
         ax.set_ylabel('Imaginary')
         if grid:
             sgrid_func(f)
-
     return mymat, kvect
 
 
@@ -162,7 +161,7 @@ def _default_gains(num, den, xlim, ylim):
         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((singular_points, np.zeros(2)), axis=0)
+    important_points = np.concatenate((important_points, np.zeros(2)), axis=0)
     mymat_xl = np.append(mymat_xl, important_points)
 
     if xlim is None:
@@ -267,6 +266,7 @@ 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)
 
@@ -321,55 +321,72 @@ def _RLFeedbackClicks(event, sys):
               (s.real, s.imag, K.real, -1 * s.real / abs(s)))
 
 
-def sgrid_func(fig):
+def sgrid_func(fig, zeta=None, wn=None):
     ax = fig.gca()
     ylocator = ax.get_yaxis().get_major_locator()
-    xlocator = ax.get_yaxis().get_major_locator()
+    xlocator = ax.get_xaxis().get_major_locator()
 
-    angules = np.arange(-90, 80, 15)*np.pi/180
+    if zeta is None:
+        zeta = _default_zetas(xlocator(), ylocator())
+
+    angules = []
+    for z in zeta:
+        if (z >= 1e-4) & (z < 1):
+            angules.append(np.pi/2 + np.arcsin(z))
+        else:
+            zeta.remove(z)
+    y_over_x = np.tan(angules)
 
     # zeta-constant lines
-    y_over_x = np.tan(angules[1::])*ylocator()[-1]/xlocator()[-1]
     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
     index = 0
-    zeta = np.sin(np.pi/2-angules[1::])
 
     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)
-        elif yp < 0:
-            xtext_pos = -1/yp * ylim[1]
+        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
-                ytext_pos = - ytext_pos
             else:
-                ytext_pos = ylim[0]
-                xtext_pos = -xtext_pos
+                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
-    for xt in xlocator():
-        if xt < 0:
-            yp = np.sin(angules)*np.abs(xt)
-            xp = -np.cos(angules)*np.abs(xt)
+    if wn is None:
+        wn = _default_wn(xlocator(), ylocator())
+
+    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" % -xt
-            ax.annotate(an, textcoords='data', xy=[xt, 0], fontsize=8)
+            an = "%.2f" % -om
+            ax.annotate(an, textcoords='data', xy=[om, 0], fontsize=8)
+
+
+def _default_zetas(xloc, yloc):
+    """Return default list of dumps coefficients"""
+    # TODO: smart selection on zetas to draw in root locus plot
+    angules = np.arange(0, 80, 15) * np.pi / 180
+    zeta = np.sin(np.pi/2 - angules[1::])
+    return zeta.tolist()
+
+
+def _default_wn(xloc, yloc):
+    """Return default wn for root locus plot"""
+    # TODO: better selection of wn (up to maximum ylim with same separation in xloc)
+    wn = xloc
+    return wn
 
 rlocus = root_locus

From 5438746ba1b7ce69bf9b9f26d727c5fe79c7b893 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Sun, 2 Apr 2017 15:31:21 -0300
Subject: [PATCH 13/20] formatting code to make easier sgrid() function in
 future.

---
 control/rlocus.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 9e446ad50..248991f87 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -138,7 +138,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
         ax.set_xlabel('Real')
         ax.set_ylabel('Imaginary')
         if grid:
-            sgrid_func(f)
+            _sgrid_func()
     return mymat, kvect
 
 
@@ -321,7 +321,9 @@ def _RLFeedbackClicks(event, sys):
               (s.real, s.imag, K.real, -1 * s.real / abs(s)))
 
 
-def sgrid_func(fig, zeta=None, wn=None):
+def _sgrid_func(fig=None, zeta=None, wn=None):
+    if fig is None:
+        fig = pylab.gcf()
     ax = fig.gca()
     ylocator = ax.get_yaxis().get_major_locator()
     xlocator = ax.get_xaxis().get_major_locator()

From 1e356b8515436a61dd443b1468bcd702a07d4437 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Thu, 8 Jun 2017 18:11:54 -0300
Subject: [PATCH 14/20] trying that algorithm work with 0 degrees root locus
 with equal number of poles and zeros

---
 control/rlocus.py | 30 ++++++++++++++++++++++++------
 1 file changed, 24 insertions(+), 6 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 248991f87..36dde3ad1 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -127,6 +127,8 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
             ax.plot(real(zeros), imag(zeros), 'o')
 
         # Now plot the loci
+        infinity_roots=np.where(mymat.T == np.inf)
+        print(infinity_roots)
         for col in mymat.T:
             ax.plot(real(col), imag(col), plotstr)
 
@@ -163,12 +165,17 @@ 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]
 
-    if xlim is None:
+    if xlim is None and false_gain > 0:
         x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
         xlim = _ax_lim(mymat_xl)
+    elif xlim is None and false_gain < 0:
+        xlim = _ax_lim(important_points)
+        x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
     else:
         x_tolerance = 0.05 * (xlim[1] - xlim[0])
+
     if ylim is None:
         y_tolerance = 0.05 * (np.max(np.max(np.imag(mymat_xl))) - np.min(np.min(np.imag(mymat_xl))))
         ylim = _ax_lim(mymat_xl * 1j)
@@ -178,6 +185,7 @@ def _default_gains(num, den, xlim, ylim):
     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) & (kvect.size < 5000):
         for index in indexes_too_far[0]:
             new_gains = np.linspace(kvect[index], kvect[index+1], 5)
@@ -185,8 +193,10 @@ def _default_gains(num, den, xlim, ylim):
             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))
-        indexes_too_far = np.where(distance_points > tolerance)
+        distance_points = np.abs(np.diff(mymat, axis=0))>tolerance
+        points_in_figure = np.logical_and(mymat[1:]>xlim[0], mymat[1:]<xlim[1])
+        indexes_too_far = np.where(np.logical_and(distance_points, points_in_figure))
+
     new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*200, 10)))
     new_points = _RLFindRoots(num, den, new_gains[1:10])
     kvect = np.append(kvect, new_gains[1:10])
@@ -230,15 +240,20 @@ def _k_max(num, den, real_break_points, k_break_points):
     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 = 2 * np.max(np.abs(important_points - asymp_center))
         asymp_angles = (2 * np.arange(0, asymp_number)-1) * np.pi / asymp_number
-        farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j)  # farthest points over asymptotes
+        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 = -den(farthest_points) / num(farthest_points)
     else:
-        kmax_asymp = [den.coeffs[0] / num.coeffs[0] * 3]
+        kmax_asymp = np.abs([den.coeffs[0] / num.coeffs[0] * 3])
 
     kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
     return kmax
@@ -277,13 +292,16 @@ 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:
         curpoly = denp + k * nump
         curroots = curpoly.r
+        if len(curroots) < denp.order:
+            curroots = np.insert(curroots, len(curroots), np.inf) # if i have less poles than open loop is becuase i have one in infinity
+
         curroots.sort()
         roots.append(curroots)
+
     mymat = row_stack(roots)
     return mymat
 

From 6c813ce107cc121ee96e9cafa9c280c8e5b9cc38 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Tue, 13 Jun 2017 18:41:09 -0300
Subject: [PATCH 15/20] adding some support for 0 degrees root locus. better
 selection of sgrid zetas and omega

---
 control/rlocus.py | 78 +++++++++++++++++++++++++++++------------------
 1 file changed, 49 insertions(+), 29 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 36dde3ad1..376c40a47 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -127,8 +127,6 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
             ax.plot(real(zeros), imag(zeros), 'o')
 
         # Now plot the loci
-        infinity_roots=np.where(mymat.T == np.inf)
-        print(infinity_roots)
         for col in mymat.T:
             ax.plot(real(col), imag(col), plotstr)
 
@@ -166,18 +164,25 @@ def _default_gains(num, den, xlim, ylim):
     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.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
+        x_tolerance = 0.03 * (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:
-        xlim = _ax_lim(important_points)
-        x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
+        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.max(np.imag(mymat_xl))) - np.min(np.min(np.imag(mymat_xl))))
+        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])
@@ -192,10 +197,10 @@ def _default_gains(num, den, xlim, ylim):
             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
-        points_in_figure = np.logical_and(mymat[1:]>xlim[0], mymat[1:]<xlim[1])
-        indexes_too_far = np.where(np.logical_and(distance_points, points_in_figure))
+        distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance  # distance between points
+        indexes_too_far = np.where(distance_points)
 
     new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*200, 10)))
     new_points = _RLFindRoots(num, den, new_gains[1:10])
@@ -225,8 +230,8 @@ def _break_points(num, den):
 
 def _ax_lim(mymat):
     """Utility to get the axis limits"""
-    axmin = np.min(np.min(np.real(mymat)))
-    axmax = np.max(np.max(np.real(mymat)))
+    axmin = np.min(np.real(mymat))
+    axmax = np.max(np.real(mymat))
     if axmax != axmin:
         deltax = (axmax - axmin) * 0.02
     else:
@@ -244,18 +249,18 @@ def _k_max(num, den, real_break_points, k_break_points):
 
     if asymp_number > 0:
         asymp_center = (np.sum(den.roots) - np.sum(num.roots))/asymp_number
-        distance_max = 2 * np.max(np.abs(important_points - asymp_center))
+        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 = -den(farthest_points) / num(farthest_points)
+        kmax_asymp = np.real(np.abs(den(farthest_points) / num(farthest_points)))
     else:
-        kmax_asymp = np.abs([den.coeffs[0] / num.coeffs[0] * 3])
+        kmax_asymp = np.abs([np.abs(den.coeffs[0]) / np.abs(num.coeffs[0]) * 3])
 
-    kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
+    kmax = np.max(np.concatenate((np.real(kmax_asymp), np.real(k_break_points)), axis=0))
     return kmax
 
 
@@ -297,7 +302,8 @@ def _RLFindRoots(nump, denp, kvect):
         curpoly = denp + k * nump
         curroots = curpoly.r
         if len(curroots) < denp.order:
-            curroots = np.insert(curroots, len(curroots), np.inf) # if i have less poles than open loop is becuase i have one in infinity
+            curroots = np.insert(curroots, len(curroots), np.inf)  # if i have less poles than open loop is because  i
+            #  have one in infinity
 
         curroots.sort()
         roots.append(curroots)
@@ -346,22 +352,24 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
     ylocator = ax.get_yaxis().get_major_locator()
     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(xlocator(), ylocator())
+        zeta = _default_zetas(xlim, ylim)
 
     angules = []
     for z in zeta:
-        if (z >= 1e-4) & (z < 1):
+        if (z >= 1e-4) & (z <= 1):
             angules.append(np.pi/2 + np.arcsin(z))
         else:
             zeta.remove(z)
     y_over_x = np.tan(angules)
 
     # zeta-constant lines
-    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
+
     index = 0
 
     for yp in y_over_x:
@@ -383,7 +391,7 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
 
     angules = np.linspace(-90, 90, 20)*np.pi/180
     if wn is None:
-        wn = _default_wn(xlocator(), ylocator())
+        wn = _default_wn(xlocator(), ylim)
 
     for om in wn:
         if om < 0:
@@ -395,18 +403,30 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
             ax.annotate(an, textcoords='data', xy=[om, 0], fontsize=8)
 
 
-def _default_zetas(xloc, yloc):
+def _default_zetas(xlim, ylim):
     """Return default list of dumps coefficients"""
-    # TODO: smart selection on zetas to draw in root locus plot
-    angules = np.arange(0, 80, 15) * np.pi / 180
-    zeta = np.sin(np.pi/2 - angules[1::])
+    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, yloc):
+def _default_wn(xloc, ylim):
     """Return default wn for root locus plot"""
-    # TODO: better selection of wn (up to maximum ylim with same separation in xloc)
+
     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

From 6e4d14f105c75dbbb0a4c1d121c4f82bb5492d5e Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Tue, 13 Jun 2017 19:02:16 -0300
Subject: [PATCH 16/20] add book reference of root locus calculation. fix a
 minor bug.

---
 control/rlocus.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 376c40a47..19941262c 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -143,7 +143,11 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
 
 
 def _default_gains(num, den, xlim, ylim):
-    """Unsupervised gains calculation for root locus plot"""
+    """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)))
@@ -169,7 +173,7 @@ def _default_gains(num, den, xlim, ylim):
                          "locus with equal order of numerator and denominator.")
 
     if xlim is None and false_gain > 0:
-        x_tolerance = 0.03 * (np.max(np.real(mymat_xl)) - np.min(np.real(mymat_xl)))
+        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)))

From a680c6a234e9a022b64b15ab1053b3c7a419bb6b Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Wed, 14 Jun 2017 10:07:30 -0300
Subject: [PATCH 17/20] fix minor bug when fartest point is zero.

---
 control/rlocus.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/control/rlocus.py b/control/rlocus.py
index 19941262c..99783e39f 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -265,6 +265,7 @@ def _k_max(num, den, real_break_points, k_break_points):
         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))
+    kmax = np.max([kmax, np.abs(false_gain)])
     return kmax
 
 

From 81b73a6b6e949e7666692821c45d834c8169958d Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Sat, 17 Jun 2017 15:15:09 -0300
Subject: [PATCH 18/20] fix minor bugs when that solves problems in paricular
 cases

---
 control/rlocus.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 99783e39f..57d1ee920 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -191,6 +191,7 @@ 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)
@@ -206,7 +207,7 @@ def _default_gains(num, den, xlim, ylim):
         distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance  # distance between points
         indexes_too_far = np.where(distance_points)
 
-    new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*200, 10)))
+    new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*1e10, 10)))
     new_points = _RLFindRoots(num, den, new_gains[1:10])
     kvect = np.append(kvect, new_gains[1:10])
     mymat = np.concatenate((mymat, new_points), axis=0)

From 62ed2aac9aaa0423e0aa5d781ec6cfb050a6c775 Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Fri, 30 Jun 2017 18:46:44 -0300
Subject: [PATCH 19/20] Fixed requested issues.

---
 control/rlocus.py | 16 +++++++---------
 1 file changed, 7 insertions(+), 9 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 57d1ee920..15f39e5ba 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -157,7 +157,7 @@ def _default_gains(num, den, xlim, ylim):
     open_loop_poles = den.roots
     open_loop_zeros = num.roots
 
-    if (open_loop_zeros.size != 0) & (open_loop_zeros.size < open_loop_poles.size):
+    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)
@@ -191,12 +191,11 @@ 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)
 
-    while (indexes_too_far[0].size > 0) & (kvect.size < 5000):
+    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])
@@ -246,7 +245,7 @@ def _ax_lim(mymat):
 
 
 def _k_max(num, den, real_break_points, k_break_points):
-    """" Calculation the maximum gain for the root locus shown in tne figure"""
+    """" 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)
@@ -308,8 +307,8 @@ def _RLFindRoots(nump, denp, kvect):
         curpoly = denp + k * nump
         curroots = curpoly.r
         if len(curroots) < denp.order:
-            curroots = np.insert(curroots, len(curroots), np.inf)  # if i have less poles than open loop is because  i
-            #  have one in infinity
+            # 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)
@@ -355,7 +354,6 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
     if fig is None:
         fig = pylab.gcf()
     ax = fig.gca()
-    ylocator = ax.get_yaxis().get_major_locator()
     xlocator = ax.get_xaxis().get_major_locator()
 
     ylim = ax.get_ylim()
@@ -368,7 +366,7 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
 
     angules = []
     for z in zeta:
-        if (z >= 1e-4) & (z <= 1):
+        if (z >= 1e-4) and (z <= 1):
             angules.append(np.pi/2 + np.arcsin(z))
         else:
             zeta.remove(z)
@@ -430,7 +428,7 @@ def _default_wn(xloc, ylim):
     while np.abs(wn[0]) < ylim[1]:
         wn = np.insert(wn, 0, wn[0]-sep)
 
-    while len(wn)>7:
+    while len(wn) > 7:
         wn = wn[0:-1:2]
 
     return wn

From 4eedde4bf8844a28f9a583d83fee4b3a450d5f1c Mon Sep 17 00:00:00 2001
From: gonmolina <gonmolina@gmail.com>
Date: Sat, 1 Jul 2017 00:09:48 -0300
Subject: [PATCH 20/20] fix build problem (numerical problem)

---
 control/rlocus.py | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/control/rlocus.py b/control/rlocus.py
index 15f39e5ba..e22a31c25 100644
--- a/control/rlocus.py
+++ b/control/rlocus.py
@@ -206,9 +206,9 @@ def _default_gains(num, den, xlim, ylim):
         distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance  # distance between points
         indexes_too_far = np.where(distance_points)
 
-    new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*1e10, 10)))
-    new_points = _RLFindRoots(num, den, new_gains[1:10])
-    kvect = np.append(kvect, new_gains[1:10])
+    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
@@ -265,7 +265,8 @@ def _k_max(num, den, real_break_points, k_break_points):
         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))
-    kmax = np.max([kmax, np.abs(false_gain)])
+    if np.abs(false_gain) > kmax:
+        kmax = np.abs(false_gain)
     return kmax
 
 
@@ -410,9 +411,9 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
 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)]
+    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)]
+    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)