Skip to content

Commit 6c813ce

Browse files
gonmolinagonmolina
committed
adding some support for 0 degrees root locus.
better selection of sgrid zetas and omega
1 parent 1e356b8 commit 6c813ce

File tree

1 file changed

+49
-29
lines changed

1 file changed

+49
-29
lines changed

‎control/rlocus.py‎

Lines changed: 49 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -127,8 +127,6 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
127127
ax.plot(real(zeros), imag(zeros), 'o')
128128

129129
# Now plot the loci
130-
infinity_roots=np.where(mymat.T == np.inf)
131-
print(infinity_roots)
132130
for col in mymat.T:
133131
ax.plot(real(col), imag(col), plotstr)
134132

@@ -166,18 +164,25 @@ def _default_gains(num, den, xlim, ylim):
166164
important_points = np.concatenate((important_points, np.zeros(2)), axis=0)
167165
mymat_xl = np.append(mymat_xl, important_points)
168166
false_gain = den.coeffs[0] / num.coeffs[0]
167+
if false_gain < 0 and not den.order > num.order:
168+
raise ValueError("Not implemented support for 0 degrees root "
169+
"locus with equal order of numerator and denominator.")
169170

170171
if xlim is None and false_gain > 0:
171-
x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
172+
x_tolerance = 0.03 * (np.max(np.real(mymat_xl)) - np.min(np.real(mymat_xl)))
172173
xlim = _ax_lim(mymat_xl)
173174
elif xlim is None and false_gain < 0:
174-
xlim = _ax_lim(important_points)
175-
x_tolerance = 0.05 * (np.max(np.max(np.real(mymat_xl))) - np.min(np.min(np.real(mymat_xl))))
175+
axmin = np.min(np.real(important_points))-(np.max(np.real(important_points))-np.min(np.real(important_points)))
176+
axmin = np.min(np.array([axmin, np.min(np.real(mymat_xl))]))
177+
axmax = np.max(np.real(important_points))+np.max(np.real(important_points))-np.min(np.real(important_points))
178+
axmax = np.max(np.array([axmax, np.max(np.real(mymat_xl))]))
179+
xlim = [axmin, axmax]
180+
x_tolerance = 0.05 * (axmax - axmin)
176181
else:
177182
x_tolerance = 0.05 * (xlim[1] - xlim[0])
178183

179184
if ylim is None:
180-
y_tolerance = 0.05 * (np.max(np.max(np.imag(mymat_xl))) - np.min(np.min(np.imag(mymat_xl))))
185+
y_tolerance = 0.05 * (np.max(np.imag(mymat_xl)) - np.min(np.imag(mymat_xl)))
181186
ylim = _ax_lim(mymat_xl * 1j)
182187
else:
183188
y_tolerance = 0.05 * (ylim[1] - ylim[0])
@@ -192,10 +197,10 @@ def _default_gains(num, den, xlim, ylim):
192197
new_points = _RLFindRoots(num, den, new_gains[1:4])
193198
kvect = np.insert(kvect, index+1, new_gains[1:4])
194199
mymat = np.insert(mymat, index+1, new_points, axis=0)
200+
195201
mymat = _RLSortRoots(mymat)
196-
distance_points = np.abs(np.diff(mymat, axis=0))>tolerance
197-
points_in_figure = np.logical_and(mymat[1:]>xlim[0], mymat[1:]<xlim[1])
198-
indexes_too_far = np.where(np.logical_and(distance_points, points_in_figure))
202+
distance_points = np.abs(np.diff(mymat, axis=0)) > tolerance # distance between points
203+
indexes_too_far = np.where(distance_points)
199204

200205
new_gains = np.hstack((np.linspace(kvect[-1], kvect[-1]*200, 10)))
201206
new_points = _RLFindRoots(num, den, new_gains[1:10])
@@ -225,8 +230,8 @@ def _break_points(num, den):
225230

226231
def _ax_lim(mymat):
227232
"""Utility to get the axis limits"""
228-
axmin = np.min(np.min(np.real(mymat)))
229-
axmax = np.max(np.max(np.real(mymat)))
233+
axmin = np.min(np.real(mymat))
234+
axmax = np.max(np.real(mymat))
230235
if axmax != axmin:
231236
deltax = (axmax - axmin) * 0.02
232237
else:
@@ -244,18 +249,18 @@ def _k_max(num, den, real_break_points, k_break_points):
244249

245250
if asymp_number > 0:
246251
asymp_center = (np.sum(den.roots) - np.sum(num.roots))/asymp_number
247-
distance_max = 2 * np.max(np.abs(important_points - asymp_center))
252+
distance_max = 4 * np.max(np.abs(important_points - asymp_center))
248253
asymp_angles = (2 * np.arange(0, asymp_number)-1) * np.pi / asymp_number
249254
if false_gain > 0:
250255
farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j) # farthest points over asymptotes
251256
else:
252257
asymp_angles = asymp_angles + np.pi
253258
farthest_points = asymp_center + distance_max * np.exp(asymp_angles * 1j) # farthest points over asymptotes
254-
kmax_asymp = -den(farthest_points) / num(farthest_points)
259+
kmax_asymp = np.real(np.abs(den(farthest_points) / num(farthest_points)))
255260
else:
256-
kmax_asymp = np.abs([den.coeffs[0] / num.coeffs[0] * 3])
261+
kmax_asymp = np.abs([np.abs(den.coeffs[0]) / np.abs(num.coeffs[0]) * 3])
257262

258-
kmax = np.max(np.concatenate((np.real(kmax_asymp), k_break_points), axis=0))
263+
kmax = np.max(np.concatenate((np.real(kmax_asymp), np.real(k_break_points)), axis=0))
259264
return kmax
260265

261266

@@ -297,7 +302,8 @@ def _RLFindRoots(nump, denp, kvect):
297302
curpoly = denp + k * nump
298303
curroots = curpoly.r
299304
if len(curroots) < denp.order:
300-
curroots = np.insert(curroots, len(curroots), np.inf) # if i have less poles than open loop is becuase i have one in infinity
305+
curroots = np.insert(curroots, len(curroots), np.inf) # if i have less poles than open loop is because i
306+
# have one in infinity
301307

302308
curroots.sort()
303309
roots.append(curroots)
@@ -346,22 +352,24 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
346352
ylocator = ax.get_yaxis().get_major_locator()
347353
xlocator = ax.get_xaxis().get_major_locator()
348354

355+
ylim = ax.get_ylim()
356+
ytext_pos_lim = ylim[1] - (ylim[1] - ylim[0]) * 0.03
357+
xlim = ax.get_xlim()
358+
xtext_pos_lim = xlim[0] + (xlim[1] - xlim[0]) * 0.0
359+
349360
if zeta is None:
350-
zeta = _default_zetas(xlocator(), ylocator())
361+
zeta = _default_zetas(xlim, ylim)
351362

352363
angules = []
353364
for z in zeta:
354-
if (z >= 1e-4) & (z < 1):
365+
if (z >= 1e-4) & (z <= 1):
355366
angules.append(np.pi/2 + np.arcsin(z))
356367
else:
357368
zeta.remove(z)
358369
y_over_x = np.tan(angules)
359370

360371
# zeta-constant lines
361-
ylim = ax.get_ylim()
362-
ytext_pos_lim = ylim[1]-(ylim[1]-ylim[0])*0.03
363-
xlim = ax.get_xlim()
364-
xtext_pos_lim = xlim[0]+(xlim[1]-xlim[0])*0.0
372+
365373
index = 0
366374

367375
for yp in y_over_x:
@@ -383,7 +391,7 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
383391

384392
angules = np.linspace(-90, 90, 20)*np.pi/180
385393
if wn is None:
386-
wn = _default_wn(xlocator(), ylocator())
394+
wn = _default_wn(xlocator(), ylim)
387395

388396
for om in wn:
389397
if om < 0:
@@ -395,18 +403,30 @@ def _sgrid_func(fig=None, zeta=None, wn=None):
395403
ax.annotate(an, textcoords='data', xy=[om, 0], fontsize=8)
396404

397405

398-
def _default_zetas(xloc, yloc):
406+
def _default_zetas(xlim, ylim):
399407
"""Return default list of dumps coefficients"""
400-
# TODO: smart selection on zetas to draw in root locus plot
401-
angules = np.arange(0, 80, 15) * np.pi / 180
402-
zeta = np.sin(np.pi/2 - angules[1::])
408+
sep1 = -xlim[0]/4
409+
ang1 = [np.arctan((sep1*i)/ylim[1]) for i in np.arange(1,4,1)]
410+
sep2 = ylim[1] / 3
411+
ang2 = [np.arctan(-xlim[0]/(ylim[1]-sep2*i)) for i in np.arange(1,3,1)]
412+
413+
angules = np.concatenate((ang1, ang2))
414+
angules = np.insert(angules, len(angules), np.pi/2)
415+
zeta = np.sin(angules)
403416
return zeta.tolist()
404417

405418

406-
def _default_wn(xloc, yloc):
419+
def _default_wn(xloc, ylim):
407420
"""Return default wn for root locus plot"""
408-
# TODO: better selection of wn (up to maximum ylim with same separation in xloc)
421+
409422
wn = xloc
423+
sep = xloc[1]-xloc[0]
424+
while np.abs(wn[0]) < ylim[1]:
425+
wn = np.insert(wn, 0, wn[0]-sep)
426+
427+
while len(wn)>7:
428+
wn = wn[0:-1:2]
429+
410430
return wn
411431

412432
rlocus = root_locus

0 commit comments

Comments
 (0)