Skip to content

Commit c0e533e

Browse files
icam0icam0
committed
root locus smooth with zoom bug
1 parent 94a0ea1 commit c0e533e

1 file changed

Lines changed: 155 additions & 55 deletions

File tree

‎control/rlocus.py‎

Lines changed: 155 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848
# Packages used by this module
4949
import numpy as np
5050
import matplotlib
51+
import matplotlib.pyplot as plt
5152
from scipy import array, poly1d, row_stack, zeros_like, real, imag
5253
import scipy.signal # signal processing toolbox
5354
import pylab # plotting routines
@@ -58,9 +59,8 @@
5859

5960
__all__ = ['root_locus', 'rlocus']
6061

61-
6262
# Main function: compute a root locus diagram
63-
def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
63+
def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='b' if int(matplotlib.__version__[0]) == 1 else 'C0', Plot=True,
6464
PrintGain=True, grid=False, **kwargs):
6565

6666
"""Root locus plot
@@ -128,13 +128,13 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
128128

129129
if PrintGain and sisotool == False:
130130
f.canvas.mpl_connect(
131-
'button_release_event', partial(_RLFeedbackClicksPoint,sys=sys,fig=f))
131+
'button_release_event', partial(_RLClickDispatcher,sys=sys, fig=f,ax_rlocus=f.axes[0],plotstr=plotstr))
132132

133133
elif sisotool == True:
134134
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')
135135
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)
136136
f.canvas.mpl_connect(
137-
'button_release_event',partial(_RLFeedbackClicksSisotool,sys=sys, fig=f, bode_plot_params=kwargs['bode_plot_params'],tvect=kwargs['tvect']))
137+
'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']))
138138

139139
# plot open loop poles
140140
poles = array(denp.r)
@@ -146,8 +146,8 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
146146
ax.plot(real(zeros), imag(zeros), 'o')
147147

148148
# Now plot the loci
149-
for col in mymat.T:
150-
ax.plot(real(col), imag(col), plotstr)
149+
for index,col in enumerate(mymat.T):
150+
ax.plot(real(col), imag(col), plotstr,label='rootlocus')
151151

152152
# Set up plot axes and labels
153153
if xlim:
@@ -165,8 +165,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
165165
ax.axvline(0., linestyle=':', color='k')
166166
return mymat, kvect
167167

168-
169-
def _default_gains(num, den, xlim, ylim):
168+
def _default_gains(num, den, xlim, ylim, point_tolerance=5e-2,zoom_xlim=None,zoom_ylim=None):
170169
"""Unsupervised gains calculation for root locus plot.
171170
172171
References:
@@ -198,47 +197,127 @@ def _default_gains(num, den, xlim, ylim):
198197
"locus with equal order of numerator and denominator.")
199198

200199
if xlim is None and false_gain > 0:
201-
x_tolerance = 0.05 * (np.max(np.real(mymat_xl)) - np.min(np.real(mymat_xl)))
200+
x_tolerance = point_tolerance * (np.max(np.real(mymat_xl)) - np.min(np.real(mymat_xl)))
202201
xlim = _ax_lim(mymat_xl)
203202
elif xlim is None and false_gain < 0:
204203
axmin = np.min(np.real(important_points))-(np.max(np.real(important_points))-np.min(np.real(important_points)))
205204
axmin = np.min(np.array([axmin, np.min(np.real(mymat_xl))]))
206205
axmax = np.max(np.real(important_points))+np.max(np.real(important_points))-np.min(np.real(important_points))
207206
axmax = np.max(np.array([axmax, np.max(np.real(mymat_xl))]))
208207
xlim = [axmin, axmax]
209-
x_tolerance = 0.05 * (axmax - axmin)
208+
x_tolerance = 5e-2 * (axmax - axmin)
210209
else:
211-
x_tolerance = 0.05 * (xlim[1] - xlim[0])
210+
x_tolerance = 5e-2 * (xlim[1] - xlim[0])
212211

213212
if ylim is None:
214-
y_tolerance = 0.05 * (np.max(np.imag(mymat_xl)) - np.min(np.imag(mymat_xl)))
213+
y_tolerance = 5e-2 * (np.max(np.imag(mymat_xl)) - np.min(np.imag(mymat_xl)))
215214
ylim = _ax_lim(mymat_xl * 1j)
216215
else:
217-
y_tolerance = 0.05 * (ylim[1] - ylim[0])
218-
216+
y_tolerance = 5e-2 * (ylim[1] - ylim[0])
217+
print(len(mymat))
219218
tolerance = np.max([x_tolerance, y_tolerance])
220-
distance_points = np.abs(np.diff(mymat, axis=0))
221-
indexes_too_far = np.where(distance_points > tolerance)
222-
223-
while (indexes_too_far[0].size > 0) and (kvect.size < 5000):
224-
for index in indexes_too_far[0]:
225-
new_gains = np.linspace(kvect[index], kvect[index+1], 5)
226-
new_points = _RLFindRoots(num, den, new_gains[1:4])
227-
kvect = np.insert(kvect, index+1, new_gains[1:4])
228-
mymat = np.insert(mymat, index+1, new_points, axis=0)
229-
219+
# print('normal smoothing start')
220+
# mymat,kvect =_smooth_rootlocus(num, den, tolerance,mymat,kvect, xlim=None, ylim=None)
221+
# print('normal smoothing end')
222+
mymat = _RLSortRoots(mymat)
223+
print(len(mymat))
224+
225+
# If a zoom on the plot is used insert points on this interval and use a smaller tolerance
226+
if zoom_xlim != None and zoom_ylim != None:
227+
print('zoom smoothing start')
228+
y_tolerance = 5e-2 * (zoom_ylim[1] - zoom_ylim[0])
229+
x_tolerance = 5e-2 * (zoom_xlim[1] - zoom_xlim[0])
230+
tolerance = np.max([x_tolerance, y_tolerance])
231+
tolerance = np.max([x_tolerance, y_tolerance])
232+
#print(mymat)
233+
mymat,kvect = _smooth_rootlocus(num,den,tolerance,mymat,kvect,zoom_xlim,zoom_ylim)
230234
mymat = _RLSortRoots(mymat)
231-
distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance # distance between points
232-
indexes_too_far = np.where(distance_points)
235+
#print(mymat)
236+
print('zoom smoothing end')
237+
print(len(mymat))
238+
kvect = np.sort(kvect)
233239

234-
new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4)))
235-
new_points = _RLFindRoots(num, den, new_gains[1:4])
236-
kvect = np.append(kvect, new_gains[1:4])
237-
mymat = np.concatenate((mymat, new_points), axis=0)
238-
mymat = _RLSortRoots(mymat)
240+
mymat = _RLFindRoots(num, den, kvect)
239241
return kvect, mymat, xlim, ylim
240242

241243

244+
def _smooth_rootlocus(num,den,tolerance,mymat,kvect, xlim,ylim):
245+
"""Smooth the rootlocus plot by inserting points at locations where the distance between
246+
two points exceeds a certain tolerance."""
247+
248+
distance_points = np.abs(np.diff(mymat, axis=0))
249+
indexes_too_far = np.where(distance_points > tolerance)
250+
indexes_too_far_filtered = _indexes_filter(indexes_too_far,mymat,xlim,ylim)
251+
252+
print('length of indexes too_far_filtered')
253+
print(len(indexes_too_far_filtered))
254+
print(indexes_too_far_filtered)
255+
256+
if len(indexes_too_far_filtered) > 0:
257+
print('points are added')
258+
# if indexes_too_far_filtered[0] != 0:
259+
# point_before_start = indexes_too_far_filtered[0] -1
260+
# indexes_too_far_filtered.insert(0,point_before_start)
261+
#
262+
# if indexes_too_far_filtered[-1] != len(mymat):
263+
# point_at_end = indexes_too_far_filtered[-1]+1
264+
# indexes_too_far_filtered.append(point_at_end)
265+
266+
print('before while loop')
267+
print(len(indexes_too_far_filtered))
268+
print(indexes_too_far_filtered)
269+
while (len(indexes_too_far_filtered) > 0) and (kvect.size < 5000):
270+
counter = 0
271+
for list_index, index in enumerate(indexes_too_far_filtered):
272+
index+= counter*3
273+
new_gains = np.linspace(kvect[index], kvect[index+1], 5)
274+
new_points = _RLFindRoots(num, den, new_gains[1:4])
275+
276+
print('inside while loop')
277+
print(index)
278+
print(kvect[index],kvect[index+1])
279+
print(new_gains[1:4])
280+
281+
kvect = np.insert(kvect, index+1, new_gains[1:4])
282+
283+
mymat = np.insert(mymat, index+1, new_points, axis=0)
284+
counter +=1
285+
286+
287+
288+
mymat = _RLSortRoots(mymat)
289+
distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance
290+
indexes_too_far = np.where(distance_points)
291+
indexes_too_far_filtered = _indexes_filter(indexes_too_far, mymat, xlim, ylim)
292+
print('new indexes too far')
293+
print(indexes_too_far_filtered)
294+
print('K SORTED?')
295+
print(all(kvect[i] <= kvect[i+1] for i in range(len(kvect)-1)))
296+
297+
#print(kvect)
298+
299+
new_gains = kvect[-1] * np.hstack((np.logspace(0, 3, 4)))
300+
new_points = _RLFindRoots(num, den, new_gains[1:4])
301+
kvect = np.append(kvect, new_gains[1:4])
302+
mymat = np.concatenate((mymat, new_points), axis=0)
303+
304+
305+
return mymat,kvect
306+
307+
def _indexes_filter(indexes_too_far,mymat,xlim,ylim):
308+
"""Filter the indexes so only the resolution of points within the xlim and ylim is improved"""
309+
if xlim== None and ylim == None:
310+
indexes_too_far_filtered = list(np.unique(indexes_too_far[0]))
311+
else:
312+
indexes_too_far_filtered = []
313+
for index in np.unique(indexes_too_far[0]):
314+
for point in mymat[index]:
315+
if (xlim[0] <= point.real <= xlim[1]) and (ylim[0] <= point.imag <=ylim[1]):
316+
indexes_too_far_filtered.append(index)
317+
break
318+
319+
return indexes_too_far_filtered
320+
242321
def _break_points(num, den):
243322
"""Extract break points over real axis and the gains give these location"""
244323
# type: (np.poly1d, np.poly1d) -> (np.array, np.array)
@@ -365,27 +444,46 @@ def _RLSortRoots(mymat):
365444
prevrow = sorted[n, :]
366445
return sorted
367446

368-
def _RLFeedbackClicksSisotool(event,sys,fig,bode_plot_params,tvect):
369-
"""Update Sisotool plots if a new point on the root locus plot is clicked
370-
"""
447+
def _RLClickDispatcher(event,sys,fig,ax_rlocus,plotstr,sisotool=False,bode_plot_params=None,tvect=None):
448+
"""Rootlocus plot click dispatcher"""
449+
450+
# If zoom is used on the rootlocus plot smooth and update it
451+
if plt.get_current_fig_manager().toolbar.mode == 'zoom rect' and event.inaxes == ax_rlocus.axes:
452+
453+
(nump, denp) = _systopoly1d(sys)
454+
xlim = ax_rlocus.get_xlim()
455+
ylim = ax_rlocus.get_ylim()
456+
kvect,mymat, xlim,ylim = _default_gains(nump, denp,xlim=None,ylim=None, zoom_xlim=xlim,zoom_ylim=ylim)
457+
_removeLine('rootlocus', ax_rlocus)
458+
459+
for index,col in enumerate(mymat.T):
460+
ax_rlocus.plot(real(col), imag(col), plotstr,label=index)
461+
462+
fig.canvas.draw()
463+
464+
# if a point is clicked on the rootlocus plot visually emphasize it
465+
else:
466+
K = _RLFeedbackClicksPoint(event, sys, fig,ax_rlocus,sisotool)
467+
if sisotool and K is not None:
468+
_SisotoolUpdate(sys, fig, K, bode_plot_params, tvect)
469+
470+
# Update the canvas
471+
fig.canvas.draw()
472+
473+
474+
def _RLFeedbackClicksPoint(event,sys,fig,ax_rlocus,sisotool=False):
475+
"""Display root-locus gain feedback point for clicks on the root-locus plot"""
476+
477+
(nump, denp) = _systopoly1d(sys)
478+
479+
# Catch type error when event click is in the figure but not in an axis
371480
try:
372481
s = complex(event.xdata, event.ydata)
373482
K = -1. / sys.horner(s)
374-
ax_rlocus = fig.axes[1]
375-
if _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=True):
376-
_SisotoolUpdate(sys, fig, K.real[0][0], bode_plot_params, tvect)
377-
except TypeError:
378-
pass
379483

380-
def _RLFeedbackClicksPoint(event,sys,fig,ax_rlocus=None,sisotool=False):
381-
"""Display root-locus gain feedback point for clicks on the root-locus plot
382-
"""
383-
if sisotool == False:
384-
ax_rlocus = fig.axes[0]
484+
except TypeError:
485+
K = float('inf')
385486

386-
(nump, denp) = _systopoly1d(sys)
387-
s = complex(event.xdata, event.ydata)
388-
K = -1. / sys.horner(s)
389487
if abs(K.real) > 1e-8 and abs(K.imag / K.real) < 0.04 and event.inaxes == ax_rlocus.axes:
390488

391489
# Display the parameters in the output window and figure
@@ -394,11 +492,8 @@ def _RLFeedbackClicksPoint(event,sys,fig,ax_rlocus=None,sisotool=False):
394492
fig.suptitle("Clicked at: %10.4g%+10.4gj gain: %10.4g damp: %10.4g" %
395493
(s.real, s.imag, K.real, -1 * s.real / abs(s)),fontsize = 12 if int(matplotlib.__version__[0]) == 1 else 10)
396494

397-
# Remove the previous points
398-
for line in reversed(ax_rlocus.lines):
399-
if len(line.get_xdata()) == 1 and line.get_label()=='gain_point':
400-
line.remove()
401-
del line
495+
# Remove the previous line
496+
_removeLine(label='gain_point',ax=ax_rlocus)
402497

403498
# Visualise clicked point, display all roots for sisotool mode
404499
if sisotool:
@@ -408,10 +503,15 @@ def _RLFeedbackClicksPoint(event,sys,fig,ax_rlocus=None,sisotool=False):
408503
else:
409504
ax_rlocus.plot(s.real, s.imag, 'k.', marker='s', markersize=8, zorder=20,label='gain_point')
410505

411-
# Update the canvas
412-
fig.canvas.draw()
506+
return K.real[0][0]
507+
413508

414-
return True
509+
def _removeLine(label,ax):
510+
"""Remove a line from the ax when a label is specified"""
511+
for line in reversed(ax.lines):
512+
if line.get_label() == label:
513+
line.remove()
514+
del line
415515

416516
def _sgrid_func(fig=None, zeta=None, wn=None):
417517
if fig is None:

0 commit comments

Comments
 (0)