Skip to content

Commit 92ccf2d

Browse files
authored
Merge pull request #138 from gonmolina/unsup_rlocus_gain_selection
add unsupervised calculation on gains for root locus plot.
2 parents 7c5f28d + 4eedde4 commit 92ccf2d

File tree

1 file changed

+244
-21
lines changed

1 file changed

+244
-21
lines changed

control/rlocus.py

Lines changed: 244 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -56,9 +56,10 @@
5656

5757
__all__ = ['root_locus', 'rlocus']
5858

59+
5960
# Main function: compute a root locus diagram
6061
def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
61-
PrintGain=True):
62+
PrintGain=True, grid=False):
6263
"""Root locus plot
6364
6465
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,
7677
ylim : tuple or list, optional
7778
control of y-axis range
7879
Plot : boolean, optional (default = True)
79-
If True, plot magnitude and phase
80+
If True, plot root locus diagram.
8081
PrintGain: boolean (default = True)
8182
If True, report mouse clicks when close to the root-locus branches,
8283
calculate gain, damping and print
84+
grid: boolean (default = False)
85+
If True plot s-plane grid.
8386
8487
Returns
8588
-------
@@ -93,15 +96,22 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
9396
(nump, denp) = _systopoly1d(sys)
9497

9598
if kvect is None:
96-
kvect = _default_gains(sys)
97-
98-
# Compute out the loci
99-
mymat = _RLFindRoots(sys, kvect)
100-
mymat = _RLSortRoots(sys, mymat)
99+
kvect, mymat, xlim, ylim = _default_gains(nump, denp, xlim, ylim)
100+
else:
101+
mymat = _RLFindRoots(nump, denp, kvect)
102+
mymat = _RLSortRoots(mymat)
103+
104+
# Create the Plot
105+
if Plot:
106+
figure_number = pylab.get_fignums()
107+
figure_title = [pylab.figure(numb).canvas.get_window_title() for numb in figure_number]
108+
new_figure_name = "Root Locus"
109+
rloc_num = 1
110+
while new_figure_name in figure_title:
111+
new_figure_name = "Root Locus " + str(rloc_num)
112+
rloc_num += 1
113+
f = pylab.figure(new_figure_name)
101114

102-
# Create the plot
103-
if (Plot):
104-
f = pylab.figure()
105115
if PrintGain:
106116
f.canvas.mpl_connect(
107117
'button_release_event', partial(_RLFeedbackClicks, sys=sys))
@@ -113,7 +123,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
113123

114124
# plot open loop zeros
115125
zeros = array(nump.r)
116-
if zeros.any():
126+
if zeros.size > 0:
117127
ax.plot(real(zeros), imag(zeros), 'o')
118128

119129
# Now plot the loci
@@ -127,14 +137,139 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
127137
ax.set_ylim(ylim)
128138
ax.set_xlabel('Real')
129139
ax.set_ylabel('Imaginary')
130-
140+
if grid:
141+
_sgrid_func()
131142
return mymat, kvect
132143

133-
def _default_gains(sys):
134-
# TODO: update with a smart calculation of the gains using sys poles/zeros
135-
return np.logspace(-3, 3)
136144

137-
# Utility function to extract numerator and denominator polynomials
145+
def _default_gains(num, den, xlim, ylim):
146+
"""Unsupervised gains calculation for root locus plot.
147+
148+
References:
149+
Ogata, K. (2002). Modern control engineering (4th ed.). Upper Saddle River, NJ : New Delhi: Prentice Hall.."""
150+
151+
k_break, real_break = _break_points(num, den)
152+
kmax = _k_max(num, den, real_break, k_break)
153+
kvect = np.hstack((np.linspace(0, kmax, 50), np.real(k_break)))
154+
kvect.sort()
155+
mymat = _RLFindRoots(num, den, kvect)
156+
mymat = _RLSortRoots(mymat)
157+
open_loop_poles = den.roots
158+
open_loop_zeros = num.roots
159+
160+
if (open_loop_zeros.size != 0) and (open_loop_zeros.size < open_loop_poles.size):
161+
open_loop_zeros_xl = np.append(open_loop_zeros,
162+
np.ones(open_loop_poles.size - open_loop_zeros.size) * open_loop_zeros[-1])
163+
mymat_xl = np.append(mymat, open_loop_zeros_xl)
164+
else:
165+
mymat_xl = mymat
166+
singular_points = np.concatenate((num.roots, den.roots), axis=0)
167+
important_points = np.concatenate((singular_points, real_break), axis=0)
168+
important_points = np.concatenate((important_points, np.zeros(2)), axis=0)
169+
mymat_xl = np.append(mymat_xl, important_points)
170+
false_gain = den.coeffs[0] / num.coeffs[0]
171+
if false_gain < 0 and not den.order > num.order:
172+
raise ValueError("Not implemented support for 0 degrees root "
173+
"locus with equal order of numerator and denominator.")
174+
175+
if xlim is None and false_gain > 0:
176+
x_tolerance = 0.05 * (np.max(np.real(mymat_xl)) - np.min(np.real(mymat_xl)))
177+
xlim = _ax_lim(mymat_xl)
178+
elif xlim is None and false_gain < 0:
179+
axmin = np.min(np.real(important_points))-(np.max(np.real(important_points))-np.min(np.real(important_points)))
180+
axmin = np.min(np.array([axmin, np.min(np.real(mymat_xl))]))
181+
axmax = np.max(np.real(important_points))+np.max(np.real(important_points))-np.min(np.real(important_points))
182+
axmax = np.max(np.array([axmax, np.max(np.real(mymat_xl))]))
183+
xlim = [axmin, axmax]
184+
x_tolerance = 0.05 * (axmax - axmin)
185+
else:
186+
x_tolerance = 0.05 * (xlim[1] - xlim[0])
187+
188+
if ylim is None:
189+
y_tolerance = 0.05 * (np.max(np.imag(mymat_xl)) - np.min(np.imag(mymat_xl)))
190+
ylim = _ax_lim(mymat_xl * 1j)
191+
else:
192+
y_tolerance = 0.05 * (ylim[1] - ylim[0])
193+
194+
tolerance = np.max([x_tolerance, y_tolerance])
195+
distance_points = np.abs(np.diff(mymat, axis=0))
196+
indexes_too_far = np.where(distance_points > tolerance)
197+
198+
while (indexes_too_far[0].size > 0) and (kvect.size < 5000):
199+
for index in indexes_too_far[0]:
200+
new_gains = np.linspace(kvect[index], kvect[index+1], 5)
201+
new_points = _RLFindRoots(num, den, new_gains[1:4])
202+
kvect = np.insert(kvect, index+1, new_gains[1:4])
203+
mymat = np.insert(mymat, index+1, new_points, axis=0)
204+
205+
mymat = _RLSortRoots(mymat)
206+
distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance # distance between points
207+
indexes_too_far = np.where(distance_points)
208+
209+
new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4)))
210+
new_points = _RLFindRoots(num, den, new_gains[1:4])
211+
kvect = np.append(kvect, new_gains[1:4])
212+
mymat = np.concatenate((mymat, new_points), axis=0)
213+
mymat = _RLSortRoots(mymat)
214+
return kvect, mymat, xlim, ylim
215+
216+
217+
def _break_points(num, den):
218+
"""Extract break points over real axis and the gains give these location"""
219+
# type: (np.poly1d, np.poly1d) -> (np.array, np.array)
220+
dnum = num.deriv(m=1)
221+
dden = den.deriv(m=1)
222+
polynom = den * dnum - num * dden
223+
real_break_pts = polynom.r
224+
real_break_pts = real_break_pts[num(real_break_pts) != 0] # don't care about infinite break points
225+
k_break = -den(real_break_pts) / num(real_break_pts)
226+
idx = k_break >= 0 # only positives gains
227+
k_break = k_break[idx]
228+
real_break_pts = real_break_pts[idx]
229+
if len(k_break) == 0:
230+
k_break = [0]
231+
real_break_pts = den.roots
232+
return k_break, real_break_pts
233+
234+
235+
def _ax_lim(mymat):
236+
"""Utility to get the axis limits"""
237+
axmin = np.min(np.real(mymat))
238+
axmax = np.max(np.real(mymat))
239+
if axmax != axmin:
240+
deltax = (axmax - axmin) * 0.02
241+
else:
242+
deltax = np.max([1., axmax / 2])
243+
axlim = [axmin - deltax, axmax + deltax]
244+
return axlim
245+
246+
247+
def _k_max(num, den, real_break_points, k_break_points):
248+
"""" Calculate the maximum gain for the root locus shown in the figure"""
249+
asymp_number = den.order - num.order
250+
singular_points = np.concatenate((num.roots, den.roots), axis=0)
251+
important_points = np.concatenate((singular_points, real_break_points), axis=0)
252+
false_gain = den.coeffs[0] / num.coeffs[0]
253+
254+
if asymp_number > 0:
255+
asymp_center = (np.sum(den.roots) - np.sum(num.roots))/asymp_number
256+
distance_max = 4 * np.max(np.abs(important_points - asymp_center))
257+
asymp_angles = (2 * np.arange(0, asymp_number)-1) * np.pi / asymp_number
258+
if false_gain > 0:
259+
farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j) # farthest points over asymptotes
260+
else:
261+
asymp_angles = asymp_angles + np.pi
262+
farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j) # farthest points over asymptotes
263+
kmax_asymp = np.real(np.abs(den(farthest_points) / num(farthest_points)))
264+
else:
265+
kmax_asymp = np.abs([np.abs(den.coeffs[0]) / np.abs(num.coeffs[0]) * 3])
266+
267+
kmax = np.max(np.concatenate((np.real(kmax_asymp), np.real(k_break_points)), axis=0))
268+
if np.abs(false_gain) > kmax:
269+
kmax = np.abs(false_gain)
270+
return kmax
271+
272+
138273
def _systopoly1d(sys):
139274
"""Extract numerator and denominator polynomails for a system"""
140275
# Allow inputs from the signal processing toolbox
@@ -157,29 +292,33 @@ def _systopoly1d(sys):
157292
# Check to see if num, den are already polynomials; otherwise convert
158293
if (not isinstance(nump, poly1d)):
159294
nump = poly1d(nump)
295+
160296
if (not isinstance(denp, poly1d)):
161297
denp = poly1d(denp)
162298

163299
return (nump, denp)
164300

165301

166-
def _RLFindRoots(sys, kvect):
302+
def _RLFindRoots(nump, denp, kvect):
167303
"""Find the roots for the root locus."""
168304

169305
# Convert numerator and denominator to polynomials if they aren't
170-
(nump, denp) = _systopoly1d(sys)
171-
172306
roots = []
173307
for k in kvect:
174308
curpoly = denp + k * nump
175309
curroots = curpoly.r
310+
if len(curroots) < denp.order:
311+
# if I have fewer poles than open loop, it is because i have one at infinity
312+
curroots = np.insert(curroots, len(curroots), np.inf)
313+
176314
curroots.sort()
177315
roots.append(curroots)
316+
178317
mymat = row_stack(roots)
179318
return mymat
180319

181320

182-
def _RLSortRoots(sys, mymat):
321+
def _RLSortRoots(mymat):
183322
"""Sort the roots from sys._RLFindRoots, so that the root
184323
locus doesn't show weird pseudo-branches as roots jump from
185324
one branch to another."""
@@ -209,6 +348,90 @@ def _RLFeedbackClicks(event, sys):
209348
K = -1./sys.horner(s)
210349
if abs(K.real) > 1e-8 and abs(K.imag/K.real) < 0.04:
211350
print("Clicked at %10.4g%+10.4gj gain %10.4g damp %10.4g" %
212-
(s.real, s.imag, K.real, -1*s.real/abs(s)))
351+
(s.real, s.imag, K.real, -1 * s.real / abs(s)))
352+
353+
354+
def _sgrid_func(fig=None, zeta=None, wn=None):
355+
if fig is None:
356+
fig = pylab.gcf()
357+
ax = fig.gca()
358+
xlocator = ax.get_xaxis().get_major_locator()
359+
360+
ylim = ax.get_ylim()
361+
ytext_pos_lim = ylim[1] - (ylim[1] - ylim[0]) * 0.03
362+
xlim = ax.get_xlim()
363+
xtext_pos_lim = xlim[0] + (xlim[1] - xlim[0]) * 0.0
364+
365+
if zeta is None:
366+
zeta = _default_zetas(xlim, ylim)
367+
368+
angules = []
369+
for z in zeta:
370+
if (z >= 1e-4) and (z <= 1):
371+
angules.append(np.pi/2 + np.arcsin(z))
372+
else:
373+
zeta.remove(z)
374+
y_over_x = np.tan(angules)
375+
376+
# zeta-constant lines
377+
378+
index = 0
379+
380+
for yp in y_over_x:
381+
ax.plot([0, xlocator()[0]], [0, yp*xlocator()[0]], color='gray',
382+
linestyle='dashed', linewidth=0.5)
383+
ax.plot([0, xlocator()[0]], [0, -yp * xlocator()[0]], color='gray',
384+
linestyle='dashed', linewidth=0.5)
385+
an = "%.2f" % zeta[index]
386+
if yp < 0:
387+
xtext_pos = 1/yp * ylim[1]
388+
ytext_pos = yp * xtext_pos_lim
389+
if np.abs(xtext_pos) > np.abs(xtext_pos_lim):
390+
xtext_pos = xtext_pos_lim
391+
else:
392+
ytext_pos = ytext_pos_lim
393+
ax.annotate(an, textcoords='data', xy=[xtext_pos, ytext_pos], fontsize=8)
394+
index += 1
395+
ax.plot([0, 0], [ylim[0], ylim[1]], color='gray', linestyle='dashed', linewidth=0.5)
396+
397+
angules = np.linspace(-90, 90, 20)*np.pi/180
398+
if wn is None:
399+
wn = _default_wn(xlocator(), ylim)
400+
401+
for om in wn:
402+
if om < 0:
403+
yp = np.sin(angules)*np.abs(om)
404+
xp = -np.cos(angules)*np.abs(om)
405+
ax.plot(xp, yp, color='gray',
406+
linestyle='dashed', linewidth=0.5)
407+
an = "%.2f" % -om
408+
ax.annotate(an, textcoords='data', xy=[om, 0], fontsize=8)
409+
410+
411+
def _default_zetas(xlim, ylim):
412+
"""Return default list of dumps coefficients"""
413+
sep1 = -xlim[0]/4
414+
ang1 = [np.arctan((sep1*i)/ylim[1]) for i in np.arange(1, 4, 1)]
415+
sep2 = ylim[1] / 3
416+
ang2 = [np.arctan(-xlim[0]/(ylim[1]-sep2*i)) for i in np.arange(1, 3, 1)]
417+
418+
angules = np.concatenate((ang1, ang2))
419+
angules = np.insert(angules, len(angules), np.pi/2)
420+
zeta = np.sin(angules)
421+
return zeta.tolist()
422+
423+
424+
def _default_wn(xloc, ylim):
425+
"""Return default wn for root locus plot"""
426+
427+
wn = xloc
428+
sep = xloc[1]-xloc[0]
429+
while np.abs(wn[0]) < ylim[1]:
430+
wn = np.insert(wn, 0, wn[0]-sep)
431+
432+
while len(wn) > 7:
433+
wn = wn[0:-1:2]
434+
435+
return wn
213436

214437
rlocus = root_locus

0 commit comments

Comments
 (0)