From 775b03f672407f85cde922e9cf494767afd1894d Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Mon, 28 Dec 2020 08:00:51 -0800 Subject: [PATCH 1/3] implementation of initial_phase, wrap_phase keywords for bode() --- control/freqplot.py | 84 +++++++++++++++++++++++++++++----- control/tests/freqresp_test.py | 68 ++++++++++++++++++++++++++- 2 files changed, 139 insertions(+), 13 deletions(-) diff --git a/control/freqplot.py b/control/freqplot.py index 448814a55..72b96b037 100644 --- a/control/freqplot.py +++ b/control/freqplot.py @@ -78,6 +78,7 @@ 'bode.deg': True, # Plot phase in degrees 'bode.Hz': False, # Plot frequency in Hertz 'bode.grid': True, # Turn on grid for gain and phase + 'bode.wrap_phase': False, # Wrap the phase plot at a given value } @@ -131,7 +132,18 @@ def bode_plot(syslist, omega=None, grid : bool If True, plot grid lines on gain and phase plots. Default is set by `config.defaults['bode.grid']`. - + initial_phase : float + Set the reference phase to use for the lowest frequency. If set, the + initial phase of the Bode plot will be set to the value closest to the + value specified. Default is 180 if wrap_phase is False, 0 if + wrap_phase is True. + wrap_phase : bool or float + If wrap_phase is `False`, then the phase will be unwrapped so that it + is continuously increasing or decreasing. If wrap_phase is `True` the + phase will be restricted to the range [-180, 180) (or [:math:`-\\pi`, + :math:`\\pi`) radians). If `wrap_phase` is specified as a float, the + phase will be offset by 360 degrees if it falls below the specified + value. Default to `False`, set by config.defaults['bode.wrap_phase']. The default values for Bode plot configuration parameters can be reset using the `config.defaults` dictionary, with module name 'bode'. @@ -171,6 +183,10 @@ def bode_plot(syslist, omega=None, grid = config._get_param('bode', 'grid', kwargs, _bode_defaults, pop=True) plot = config._get_param('bode', 'grid', plot, True) margins = config._get_param('bode', 'margins', margins, False) + wrap_phase = config._get_param( + 'bode', 'wrap_phase', kwargs, _bode_defaults, pop=True) + initial_phase = config._get_param( + 'bode', 'initial_phase', kwargs, None, pop=True) # If argument was a singleton, turn it into a list if not getattr(syslist, '__iter__', False): @@ -209,11 +225,47 @@ def bode_plot(syslist, omega=None, # TODO: What distance to the Nyquist frequency is appropriate? else: nyquistfrq = None + # Get the magnitude and phase of the system mag_tmp, phase_tmp, omega_sys = sys.freqresp(omega_sys) mag = np.atleast_1d(np.squeeze(mag_tmp)) phase = np.atleast_1d(np.squeeze(phase_tmp)) - phase = unwrap(phase) + + # + # Post-process the phase to handle initial value and wrapping + # + + if initial_phase is None: + # Start phase in the range 0 to -360 w/ initial phase = -180 + # If wrap_phase is true, use 0 instead (phase \in (-pi, pi]) + initial_phase = -math.pi if wrap_phase is not True else 0 + elif isinstance(initial_phase, (int, float)): + # Allow the user to override the default calculation + if deg: + initial_phase = initial_phase/180. * math.pi + else: + raise ValueError("initial_phase must be a number.") + + # Shift the phase if needed + if abs(phase[0] - initial_phase) > math.pi: + phase -= 2*math.pi * \ + round((phase[0] - initial_phase) / (2*math.pi)) + + # Phase wrapping + if wrap_phase is False: + phase = unwrap(phase) # unwrap the phase + elif wrap_phase is True: + pass # default calculation OK + elif isinstance(wrap_phase, (int, float)): + phase = unwrap(phase) # unwrap the phase first + if deg: + wrap_phase *= math.pi/180. + + # Shift the phase if it is below the wrap_phase + phase += 2*math.pi * np.maximum( + 0, np.ceil((wrap_phase - phase)/(2*math.pi))) + else: + raise ValueError("wrap_phase must be bool or float.") mags.append(mag) phases.append(phase) @@ -270,7 +322,9 @@ def bode_plot(syslist, omega=None, label='control-bode-phase', sharex=ax_mag) + # # Magnitude plot + # if dB: pltline = ax_mag.semilogx(omega_plot, 20 * np.log10(mag), *args, **kwargs) @@ -285,19 +339,22 @@ def bode_plot(syslist, omega=None, ax_mag.grid(grid and not margins, which='both') ax_mag.set_ylabel("Magnitude (dB)" if dB else "Magnitude") + # # Phase plot - if deg: - phase_plot = phase * 180. / math.pi - else: - phase_plot = phase + # + phase_plot = phase * 180. / math.pi if deg else phase + + # Plot the data ax_phase.semilogx(omega_plot, phase_plot, *args, **kwargs) # Show the phase and gain margins in the plot if margins: + # Compute stability margins for the system margin = stability_margins(sys) - gm, pm, Wcg, Wcp = \ - margin[0], margin[1], margin[3], margin[4] - # TODO: add some documentation describing why this is here + gm, pm, Wcg, Wcp = (margin[i] for i in (0, 1, 3, 4)) + + # Figure out sign of the phase at the first gain crossing + # (needed if phase_wrap is True) phase_at_cp = phases[0][(np.abs(omegas[0] - Wcp)).argmin()] if phase_at_cp >= 0.: phase_limit = 180. @@ -307,6 +364,7 @@ def bode_plot(syslist, omega=None, if Hz: Wcg, Wcp = Wcg/(2*math.pi), Wcp/(2*math.pi) + # Draw lines at gain and phase limits ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':', zorder=-20) ax_phase.axhline(y=phase_limit if deg else @@ -315,6 +373,7 @@ def bode_plot(syslist, omega=None, mag_ylim = ax_mag.get_ylim() phase_ylim = ax_phase.get_ylim() + # Annotate the phase margin (if it exists) if pm != float('inf') and Wcp != float('nan'): if dB: ax_mag.semilogx( @@ -327,7 +386,7 @@ def bode_plot(syslist, omega=None, if deg: ax_phase.semilogx( - [Wcp, Wcp], [1e5, phase_limit+pm], + [Wcp, Wcp], [1e5, phase_limit + pm], color='k', linestyle=':', zorder=-20) ax_phase.semilogx( [Wcp, Wcp], [phase_limit + pm, phase_limit], @@ -343,6 +402,7 @@ def bode_plot(syslist, omega=None, math.radians(phase_limit)], color='k', zorder=-20) + # Annotate the gain margin (if it exists) if gm != float('inf') and Wcg != float('nan'): if dB: ax_mag.semilogx( @@ -360,11 +420,11 @@ def bode_plot(syslist, omega=None, if deg: ax_phase.semilogx( - [Wcg, Wcg], [1e-8, phase_limit], + [Wcg, Wcg], [0, phase_limit], color='k', linestyle=':', zorder=-20) else: ax_phase.semilogx( - [Wcg, Wcg], [1e-8, math.radians(phase_limit)], + [Wcg, Wcg], [0, math.radians(phase_limit)], color='k', linestyle=':', zorder=-20) ax_mag.set_ylim(mag_ylim) diff --git a/control/tests/freqresp_test.py b/control/tests/freqresp_test.py index 29c67d9af..b66988312 100644 --- a/control/tests/freqresp_test.py +++ b/control/tests/freqresp_test.py @@ -9,6 +9,7 @@ import matplotlib.pyplot as plt import numpy as np from numpy.testing import assert_allclose +import math import pytest import control as ctrl @@ -173,7 +174,7 @@ def test_bode_margin(dB, maginfty1, maginfty2, gminv, rtol=1e-5) phase_to_infinity = (np.array([Wcg, Wcg]), - np.array([1e-8, p0])) + np.array([0, p0])) assert_allclose(phase_to_infinity, allaxes[1].lines[4].get_data(), rtol=1e-5) @@ -271,3 +272,68 @@ def test_options(editsdefaults): # Make sure we got the right number of points assert numpoints1 != numpoints3 assert numpoints3 == 13 + +@pytest.mark.parametrize( + "TF, initial_phase, default_phase, expected_phase", + [pytest.param(ctrl.tf([1], [1, 0]), + None, -math.pi/2, -math.pi/2, id="order1, default"), + pytest.param(ctrl.tf([1], [1, 0]), + 180, -math.pi/2, 3*math.pi/2, id="order1, 180"), + pytest.param(ctrl.tf([1], [1, 0, 0]), + None, -math.pi, -math.pi, id="order2, default"), + pytest.param(ctrl.tf([1], [1, 0, 0]), + 180, -math.pi, math.pi, id="order2, 180"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0]), + None, -3*math.pi/2, -3*math.pi/2, id="order2, default"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0]), + 180, -3*math.pi/2, math.pi/2, id="order2, 180"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]), + None, 0, 0, id="order4, default"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]), + 180, 0, 0, id="order4, 180"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]), + -360, 0, -2*math.pi, id="order4, -360"), + ]) +def test_initial_phase(TF, initial_phase, default_phase, expected_phase): + # Check initial phase of standard transfer functions + mag, phase, omega = ctrl.bode(TF) + assert(abs(phase[0] - default_phase) < 0.1) + + # Now reset the initial phase to +180 and see if things work + mag, phase, omega = ctrl.bode(TF, initial_phase=initial_phase) + assert(abs(phase[0] - expected_phase) < 0.1) + + # Make sure everything works in rad/sec as well + # Turn off plotting since that seems to slow things down a lot (?) + if initial_phase: + mag, phase, omega = ctrl.bode( + TF, initial_phase=initial_phase/180. * math.pi, deg=False, + plot=False) + assert(abs(phase[0] - expected_phase) < 0.1) + + +@pytest.mark.parametrize( + "TF, wrap_phase, min_phase, max_phase", + [pytest.param(ctrl.tf([1], [1, 0]), + None, -math.pi/2, 0, id="order1, default"), + pytest.param(ctrl.tf([1], [1, 0]), + True, -math.pi, math.pi, id="order1, True"), + pytest.param(ctrl.tf([1], [1, 0]), + -270, -3*math.pi/2, math.pi/2, id="order1, -270"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0]), + None, -3*math.pi/2, 0, id="order3, default"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0]), + True, -math.pi, math.pi, id="order3, True"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0]), + -270, -3*math.pi/2, math.pi/2, id="order3, -270"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]), + True, -3*math.pi/2, 0, id="order5, default"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]), + True, -math.pi, math.pi, id="order5, True"), + pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]), + -270, -3*math.pi/2, math.pi/2, id="order5, -270"), + ]) +def test_phase_wrap(TF, wrap_phase, min_phase, max_phase): + mag, phase, omega = ctrl.bode(TF, wrap_phase=wrap_phase) + assert(min(phase) >= min_phase) + assert(max(phase) <= max_phase) From fe9928be9c2994a8f0d3ed06a9569458c541d4b4 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 2 Jan 2021 07:50:43 -0800 Subject: [PATCH 2/3] add additional documentation on initial_phase if deg=False --- control/freqplot.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/control/freqplot.py b/control/freqplot.py index 72b96b037..5a03694db 100644 --- a/control/freqplot.py +++ b/control/freqplot.py @@ -135,7 +135,8 @@ def bode_plot(syslist, omega=None, initial_phase : float Set the reference phase to use for the lowest frequency. If set, the initial phase of the Bode plot will be set to the value closest to the - value specified. Default is 180 if wrap_phase is False, 0 if + value specified. Units are in either degrees or radians, depending on + the `deg` parameter. Default is -180 if wrap_phase is False, 0 if wrap_phase is True. wrap_phase : bool or float If wrap_phase is `False`, then the phase will be unwrapped so that it From 2702ee7f4e5fd2b2ed7bfa1fa0d592dbcd82629b Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 2 Jan 2021 08:28:39 -0800 Subject: [PATCH 3/3] clear figure in unit tests to avoid slow plotting problem --- control/tests/freqresp_test.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/control/tests/freqresp_test.py b/control/tests/freqresp_test.py index b66988312..111d4296c 100644 --- a/control/tests/freqresp_test.py +++ b/control/tests/freqresp_test.py @@ -304,11 +304,10 @@ def test_initial_phase(TF, initial_phase, default_phase, expected_phase): assert(abs(phase[0] - expected_phase) < 0.1) # Make sure everything works in rad/sec as well - # Turn off plotting since that seems to slow things down a lot (?) if initial_phase: + plt.clf() # clear previous figure (speeds things up) mag, phase, omega = ctrl.bode( - TF, initial_phase=initial_phase/180. * math.pi, deg=False, - plot=False) + TF, initial_phase=initial_phase/180. * math.pi, deg=False) assert(abs(phase[0] - expected_phase) < 0.1)