56
56
57
57
__all__ = ['root_locus' , 'rlocus' ]
58
58
59
+
59
60
# Main function: compute a root locus diagram
60
61
def root_locus (sys , kvect = None , xlim = None , ylim = None , plotstr = '-' , Plot = True ,
61
- PrintGain = True ):
62
+ PrintGain = True , grid = False ):
62
63
"""Root locus plot
63
64
64
65
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,
76
77
ylim : tuple or list, optional
77
78
control of y-axis range
78
79
Plot : boolean, optional (default = True)
79
- If True, plot magnitude and phase
80
+ If True, plot root locus diagram.
80
81
PrintGain: boolean (default = True)
81
82
If True, report mouse clicks when close to the root-locus branches,
82
83
calculate gain, damping and print
84
+ grid: boolean (default = False)
85
+ If True plot s-plane grid.
83
86
84
87
Returns
85
88
-------
@@ -93,15 +96,22 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
93
96
(nump , denp ) = _systopoly1d (sys )
94
97
95
98
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 )
101
114
102
- # Create the plot
103
- if (Plot ):
104
- f = pylab .figure ()
105
115
if PrintGain :
106
116
f .canvas .mpl_connect (
107
117
'button_release_event' , partial (_RLFeedbackClicks , sys = sys ))
@@ -113,7 +123,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
113
123
114
124
# plot open loop zeros
115
125
zeros = array (nump .r )
116
- if zeros .any () :
126
+ if zeros .size > 0 :
117
127
ax .plot (real (zeros ), imag (zeros ), 'o' )
118
128
119
129
# Now plot the loci
@@ -127,14 +137,139 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
127
137
ax .set_ylim (ylim )
128
138
ax .set_xlabel ('Real' )
129
139
ax .set_ylabel ('Imaginary' )
130
-
140
+ if grid :
141
+ _sgrid_func ()
131
142
return mymat , kvect
132
143
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 )
136
144
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
+
138
273
def _systopoly1d (sys ):
139
274
"""Extract numerator and denominator polynomails for a system"""
140
275
# Allow inputs from the signal processing toolbox
@@ -157,29 +292,33 @@ def _systopoly1d(sys):
157
292
# Check to see if num, den are already polynomials; otherwise convert
158
293
if (not isinstance (nump , poly1d )):
159
294
nump = poly1d (nump )
295
+
160
296
if (not isinstance (denp , poly1d )):
161
297
denp = poly1d (denp )
162
298
163
299
return (nump , denp )
164
300
165
301
166
- def _RLFindRoots (sys , kvect ):
302
+ def _RLFindRoots (nump , denp , kvect ):
167
303
"""Find the roots for the root locus."""
168
304
169
305
# Convert numerator and denominator to polynomials if they aren't
170
- (nump , denp ) = _systopoly1d (sys )
171
-
172
306
roots = []
173
307
for k in kvect :
174
308
curpoly = denp + k * nump
175
309
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
+
176
314
curroots .sort ()
177
315
roots .append (curroots )
316
+
178
317
mymat = row_stack (roots )
179
318
return mymat
180
319
181
320
182
- def _RLSortRoots (sys , mymat ):
321
+ def _RLSortRoots (mymat ):
183
322
"""Sort the roots from sys._RLFindRoots, so that the root
184
323
locus doesn't show weird pseudo-branches as roots jump from
185
324
one branch to another."""
@@ -209,6 +348,90 @@ def _RLFeedbackClicks(event, sys):
209
348
K = - 1. / sys .horner (s )
210
349
if abs (K .real ) > 1e-8 and abs (K .imag / K .real ) < 0.04 :
211
350
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
213
436
214
437
rlocus = root_locus
0 commit comments