Skip to content

Commit 908fabe

Browse files
murrayrmmurrayrm
authored
Merge pull request #209 from icam0/sisotool_final
Sisotool and dynamic root locus zoom
2 parents 626c459 + 17c4a95 commit 908fabe

File tree

7 files changed

+632
-113
lines changed

7 files changed

+632
-113
lines changed

‎control/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@
6767
from .canonical import *
6868
from .robust import *
6969
from .config import *
70+
from .sisotool import *
7071

7172
# Exceptions
7273
from .exception import *

‎control/freqplot.py‎

Lines changed: 129 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@
4040
# SUCH DAMAGE.
4141
#
4242
# $Id$
43-
43+
import matplotlib
4444
import matplotlib.pyplot as plt
4545
import scipy as sp
4646
import numpy as np
@@ -89,7 +89,7 @@ def bode_plot(syslist, omega=None, dB=None, Hz=None, deg=None,
8989
omega_num: int
9090
number of samples
9191
margins : boolean
92-
if True, plot gain and phase margin
92+
If True, plot gain and phase margin
9393
\*args, \**kwargs:
9494
Additional options to matplotlib (color, linestyle, etc)
9595
@@ -193,23 +193,35 @@ def bode_plot(syslist, omega=None, dB=None, Hz=None, deg=None,
193193
# The code below should work on all cases.
194194

195195
# Get the current figure
196-
fig = plt.gcf()
197-
ax_mag = None
198-
ax_phase = None
199-
200-
# Get the current axes if they already exist
201-
for ax in fig.axes:
202-
if ax.get_label() == 'control-bode-magnitude':
203-
ax_mag = ax
204-
elif ax.get_label() == 'control-bode-phase':
205-
ax_phase = ax
206-
207-
# If no axes present, create them from scratch
208-
if ax_mag is None or ax_phase is None:
209-
plt.clf()
210-
ax_mag = plt.subplot(211, label='control-bode-magnitude')
211-
ax_phase = plt.subplot(212, label='control-bode-phase',
212-
sharex=ax_mag)
196+
197+
if 'sisotool' in kwargs:
198+
fig = kwargs['fig']
199+
ax_mag = fig.axes[0]
200+
ax_phase = fig.axes[2]
201+
sisotool = kwargs['sisotool']
202+
del kwargs['fig']
203+
del kwargs['sisotool']
204+
else:
205+
fig = plt.gcf()
206+
ax_mag = None
207+
ax_phase = None
208+
sisotool = False
209+
210+
# Get the current axes if they already exist
211+
for ax in fig.axes:
212+
if ax.get_label() == 'control-bode-magnitude':
213+
ax_mag = ax
214+
elif ax.get_label() == 'control-bode-phase':
215+
ax_phase = ax
216+
217+
# If no axes present, create them from scratch
218+
if ax_mag is None or ax_phase is None:
219+
plt.clf()
220+
ax_mag = plt.subplot(211,
221+
label='control-bode-magnitude')
222+
ax_phase = plt.subplot(212,
223+
label='control-bode-phase',
224+
sharex=ax_mag)
213225

214226
# Magnitude plot
215227
if dB:
@@ -237,58 +249,107 @@ def bode_plot(syslist, omega=None, dB=None, Hz=None, deg=None,
237249
if margins:
238250
margin = stability_margins(sys)
239251
gm, pm, Wcg, Wcp = margin[0], margin[1], margin[3], margin[4]
240-
if pm >= 0.:
241-
phase_limit = -180.
242-
else:
252+
# TODO: add some documentation describing why this is here
253+
phase_at_cp = phases[0][(np.abs(omegas[0] - Wcp)).argmin()]
254+
if phase_at_cp >= 0.:
243255
phase_limit = 180.
256+
else:
257+
phase_limit = -180.
258+
259+
if Hz:
260+
Wcg, Wcp = Wcg/(2*math.pi),Wcp/(2*math.pi)
244261

245-
ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':')
262+
ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':',
263+
zorder=-20)
246264
ax_phase.axhline(y=phase_limit if deg else math.radians(phase_limit),
247-
color='k', linestyle=':')
265+
color='k', linestyle=':', zorder=-20)
248266
mag_ylim = ax_mag.get_ylim()
249267
phase_ylim = ax_phase.get_ylim()
250268

251269
if pm != float('inf') and Wcp != float('nan'):
252270
if dB:
253-
ax_mag.semilogx([Wcp, Wcp], [0., -1e5], color='k', linestyle=':')
271+
ax_mag.semilogx([Wcp, Wcp], [0.,-1e5],
272+
color='k', linestyle=':',
273+
zorder=-20)
254274
else:
255-
ax_mag.loglog([Wcp, Wcp], [1., 1e-8], color='k', linestyle=':')
275+
ax_mag.loglog([Wcp,Wcp], [1.,1e-8],color='k',
276+
linestyle=':', zorder=-20)
256277

257278
if deg:
258-
ax_phase.semilogx([Wcp, Wcp], [1e5, phase_limit + pm],
259-
color='k', linestyle=':')
260-
ax_phase.semilogx([Wcp, Wcp], [phase_limit + pm, phase_limit],
261-
color='k')
279+
ax_phase.semilogx([Wcp, Wcp],
280+
[1e5, phase_limit+pm],
281+
color='k', linestyle=':',
282+
zorder=-20)
283+
ax_phase.semilogx([Wcp, Wcp],
284+
[phase_limit + pm, phase_limit],
285+
color='k', zorder=-20)
262286
else:
263-
ax_phase.semilogx([Wcp, Wcp], [1e5, math.radians(phase_limit) +
264-
math.radians(pm)],
265-
color='k', linestyle=':')
266-
ax_phase.semilogx([Wcp, Wcp], [math.radians(phase_limit) +
267-
math.radians(pm),
268-
math.radians(phase_limit)], color='k')
287+
ax_phase.semilogx([Wcp, Wcp],
288+
[1e5, math.radians(phase_limit) +
289+
math.radians(pm)],
290+
color='k', linestyle=':',
291+
zorder=-20)
292+
ax_phase.semilogx([Wcp, Wcp],
293+
[math.radians(phase_limit) +
294+
math.radians(pm),
295+
math.radians(phase_limit)],
296+
color='k', zorder=-20)
269297

270298
if gm != float('inf') and Wcg != float('nan'):
271299
if dB:
272-
ax_mag.semilogx([Wcg, Wcg], [-20. * np.log10(gm), -1e5], color='k',
273-
linestyle=':')
274-
ax_mag.semilogx([Wcg, Wcg], [0, -20 * np.log10(gm)], color='k')
300+
ax_mag.semilogx([Wcg, Wcg],
301+
[-20.*np.log10(gm), -1e5],
302+
color='k', linestyle=':',
303+
zorder=-20)
304+
ax_mag.semilogx([Wcg, Wcg], [0,-20*np.log10(gm)],
305+
color='k', zorder=-20)
275306
else:
276-
ax_mag.loglog([Wcg, Wcg], [1. / gm, 1e-8], color='k', linestyle=':')
277-
ax_mag.loglog([Wcg, Wcg], [1., 1. / gm], color='k')
307+
ax_mag.loglog([Wcg, Wcg],
308+
[1./gm,1e-8],color='k',
309+
linestyle=':', zorder=-20)
310+
ax_mag.loglog([Wcg, Wcg],
311+
[1.,1./gm],color='k', zorder=-20)
278312

279313
if deg:
280-
ax_phase.semilogx([Wcg, Wcg], [1e-8, phase_limit],
281-
color='k', linestyle=':')
314+
ax_phase.semilogx([Wcg, Wcg], [1e-8, phase_limit],
315+
color='k', linestyle=':',
316+
zorder=-20)
282317
else:
283-
ax_phase.semilogx([Wcg, Wcg], [1e-8, math.radians(phase_limit)],
284-
color='k', linestyle=':')
318+
ax_phase.semilogx([Wcg, Wcg],
319+
[1e-8, math.radians(phase_limit)],
320+
color='k', linestyle=':',
321+
zorder=-20)
285322

286323
ax_mag.set_ylim(mag_ylim)
287324
ax_phase.set_ylim(phase_ylim)
288-
plt.suptitle('Gm = %.2f %s(at %.2f rad/s), Pm = %.2f %s (at %.2f rad/s)' %
289-
(20 * np.log10(gm) if dB else gm,
290-
'dB ' if dB else '\b', Wcg, pm if deg else math.radians(pm),
291-
'deg' if deg else 'rad', Wcp))
325+
326+
if sisotool:
327+
ax_mag.text(0.04, 0.06,
328+
'G.M.: %.2f %s\nFreq: %.2f %s' %
329+
(20*np.log10(gm) if dB else gm,
330+
'dB ' if dB else '',
331+
Wcg, 'Hz' if Hz else 'rad/s'),
332+
horizontalalignment='left',
333+
verticalalignment='bottom',
334+
transform=ax_mag.transAxes,
335+
fontsize=8 if int(matplotlib.__version__[0]) == 1 else 6)
336+
ax_phase.text(0.04, 0.06,
337+
'P.M.: %.2f %s\nFreq: %.2f %s' %
338+
(pm if deg else math.radians(pm),
339+
'deg' if deg else 'rad',
340+
Wcp, 'Hz' if Hz else 'rad/s'),
341+
horizontalalignment='left',
342+
verticalalignment='bottom',
343+
transform=ax_phase.transAxes,
344+
fontsize=8 if int(matplotlib.__version__[0]) == 1 else 6)
345+
else:
346+
plt.suptitle('Gm = %.2f %s(at %.2f %s), Pm = %.2f %s (at %.2f %s)' %
347+
(20*np.log10(gm) if dB else gm,
348+
'dB ' if dB else '\b',
349+
Wcg, 'Hz' if Hz else 'rad/s',
350+
pm if deg else math.radians(pm),
351+
'deg' if deg else 'rad',
352+
Wcp, 'Hz' if Hz else 'rad/s'))
292353

293354
if nyquistfrq_plot:
294355
ax_phase.axvline(nyquistfrq_plot, color=pltline[0].get_color())
@@ -302,12 +363,19 @@ def gen_zero_centered_series(val_min, val_max, period):
302363
return np.arange(v1, v2 + 1) * period
303364
if deg:
304365
ylim = ax_phase.get_ylim()
305-
ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], 45.))
306-
ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], 15.), minor=True)
366+
ax_phase.set_yticks(gen_zero_centered_series(ylim[0],
367+
ylim[1], 45.))
368+
ax_phase.set_yticks(gen_zero_centered_series(ylim[0],
369+
ylim[1], 15.),
370+
minor=True)
307371
else:
308372
ylim = ax_phase.get_ylim()
309-
ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], math.pi / 4.))
310-
ax_phase.set_yticks(gen_zero_centered_series(ylim[0], ylim[1], math.pi / 12.),
373+
ax_phase.set_yticks(gen_zero_centered_series(ylim[0],
374+
ylim[1],
375+
math.pi / 4.))
376+
ax_phase.set_yticks(gen_zero_centered_series(ylim[0],
377+
ylim[1],
378+
math.pi / 12.),
311379
minor=True)
312380
ax_phase.grid(False if margins else True, which='both')
313381
# ax_mag.grid(which='minor', alpha=0.3)
@@ -316,15 +384,17 @@ def gen_zero_centered_series(val_min, val_max, period):
316384
# ax_phase.grid(which='major', alpha=0.9)
317385

318386
# Label the frequency axis
319-
ax_phase.set_xlabel("Frequency (Hz)" if Hz else "Frequency (rad/sec)")
387+
ax_phase.set_xlabel("Frequency (Hz)" if Hz
388+
else "Frequency (rad/sec)")
320389

321390
if len(syslist) == 1:
322391
return mags[0], phases[0], omegas[0]
323392
else:
324393
return mags, phases, omegas
325394

326395

327-
def nyquist_plot(syslist, omega=None, Plot=True, color=None, labelFreq=0, *args, **kwargs):
396+
def nyquist_plot(syslist, omega=None, Plot=True, color=None,
397+
labelFreq=0, *args, **kwargs):
328398
"""
329399
Nyquist plot for a system
330400
@@ -683,6 +753,10 @@ def gen_prefix(pow1000):
683753
'z', # zepto (10^-21)
684754
'y'][8 - pow1000] # yocto (10^-24)
685755

756+
def find_nearest_omega(omega_list, omega):
757+
omega_list = np.asarray(omega_list)
758+
idx = (np.abs(omega_list - omega)).argmin()
759+
return omega_list[(np.abs(omega_list - omega)).argmin()]
686760

687761
# Function aliases
688762
bode = bode_plot

0 commit comments

Comments
 (0)