From 5d0b3d0ab1ff003ba8772bf8debfc5301791119f Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 24 Dec 2022 13:59:14 -0800 Subject: [PATCH 1/2] add code + tests for discrete time find_eqpt --- control/iosys.py | 38 ++++++++-------- control/tests/iosys_test.py | 87 +++++++++++++++++++++++++++++++++---- 2 files changed, 100 insertions(+), 25 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 2152092fc..58c99ff9d 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -2010,11 +2010,6 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, if np.isscalar(y0): y0 = np.ones((ninputs,)) * y0 - # Discrete-time not yet supported - if isdtime(sys, strict=True): - raise NotImplementedError( - "Discrete time systems are not yet supported.") - # Make sure the input arguments match the sizes of the system if len(x0) != nstates or \ (u0 is not None and len(u0) != ninputs) or \ @@ -2030,18 +2025,28 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, # Special cases: either inputs or outputs are constrained if y0 is None: # Take u0 as fixed and minimize over x - # TODO: update to allow discrete time systems - def ode_rhs(z): return sys._rhs(t, z, u0) - result = root(ode_rhs, x0) + if sys.isdtime(strict=True): + def state_rhs(z): return sys._rhs(t, z, u0) - z + else: + def state_rhs(z): return sys._rhs(t, z, u0) + + result = root(state_rhs, x0) z = (result.x, u0, sys._out(t, result.x, u0)) + else: # Take y0 as fixed and minimize over x and u - def rootfun(z): - # Split z into x and u - x, u = np.split(z, [nstates]) - # TODO: update to allow discrete time systems - return np.concatenate( - (sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0) + if sys.isdtime(strict=True): + def rootfun(z): + x, u = np.split(z, [nstates]) + return np.concatenate( + (sys._rhs(t, x, u) - x, sys._out(t, x, u) - y0), + axis=0) + else: + def rootfun(z): + x, u = np.split(z, [nstates]) + return np.concatenate( + (sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0) + z0 = np.concatenate((x0, u0), axis=0) # Put variables together result = root(rootfun, z0) # Find the eq point x, u = np.split(result.x, [nstates]) # Split result back in two @@ -2135,7 +2140,6 @@ def rootfun(z): # Keep track of the number of states in the set of free variables nstate_vars = len(state_vars) - dtime = isdtime(sys, strict=True) def rootfun(z): # Map the vector of values into the states and inputs @@ -2144,8 +2148,8 @@ def rootfun(z): # Compute the update and output maps dx = sys._rhs(t, x, u) - dx0 - if dtime: - dx -= x # TODO: check + if sys.isdtime(strict=True): + dx -= x # If no y0 is given, don't evaluate the output function if y0 is None: diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py index c6b8d9d15..527f597ba 100644 --- a/control/tests/iosys_test.py +++ b/control/tests/iosys_test.py @@ -10,9 +10,10 @@ import re import warnings +import pytest import numpy as np -import pytest +from math import sqrt import control as ct from control import iosys as ios @@ -238,7 +239,7 @@ def test_linearize_named_signals(self, kincar): assert lin_nocopy.find_state('x') is None # if signal names are provided, they should override those of kincar - linearized_newnames = kincar.linearize([0, 0, 0], [0, 0], + linearized_newnames = kincar.linearize([0, 0, 0], [0, 0], name='linearized', copy_names=True, inputs=['v2', 'phi2'], outputs=['x2','y2']) assert linearized_newnames.name == 'linearized' @@ -766,8 +767,8 @@ def nlsys_output(t, x, u, params): np.testing.assert_allclose(ios_t, lin_t,atol=0.002,rtol=0.) np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.) - def test_find_eqpts(self, tsys): - """Test find_eqpt function""" + def test_find_eqpts_dfan(self, tsys): + """Test find_eqpt function on dfan example""" # Simple equilibrium point with no inputs nlsys = ios.NonlinearIOSystem(predprey) xeq, ueq, result = ios.find_eqpt( @@ -836,7 +837,7 @@ def test_find_eqpts(self, tsys): np.testing.assert_array_almost_equal( nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5) - # The same test as previous, but now all constraints are in the state vector + # Same test as before, but now all constraints are in the state vector nlsys_full = ios.NonlinearIOSystem(pvtol_full, None) xeq, ueq, result = ios.find_eqpt( nlsys_full, [0, 0, 0.1, 0.1, 0, 0], [0.01, 4*9.8], @@ -1482,7 +1483,7 @@ def test_linear_interconnection(): tf_siso = ct.tf(1, [0.1, 1]) ss_siso = ct.ss(1, 2, 1, 1) nl_siso = ios.NonlinearIOSystem( - lambda t, x, u, params: x*x, + lambda t, x, u, params: x*x, lambda t, x, u, params: u*x, states=1, inputs=1, outputs=1) # Create a "regular" InterconnectedSystem @@ -1530,7 +1531,7 @@ def test_linear_interconnection(): np.testing.assert_array_almost_equal(io_connect.C, ss_connect.C) np.testing.assert_array_almost_equal(io_connect.D, ss_connect.D) - # make sure interconnections of linear systems are linear and + # make sure interconnections of linear systems are linear and # if a nonlinear system is included then system is nonlinear assert isinstance(ss_siso*ss_siso, ios.LinearIOSystem) assert isinstance(tf_siso*ss_siso, ios.LinearIOSystem) @@ -1541,7 +1542,7 @@ def test_linear_interconnection(): assert ~isinstance(tf_siso*nl_siso, ios.LinearIOSystem) assert ~isinstance(nl_siso*tf_siso, ios.LinearIOSystem) assert ~isinstance(nl_siso*nl_siso, ios.LinearIOSystem) - + def predprey(t, x, u, params={}): """Predator prey dynamics""" @@ -1898,3 +1899,73 @@ def test_rss(): with pytest.warns(UserWarning, match="may be interpreted as continuous"): sys = ct.drss(2, 1, 1, dt=None) assert np.all(np.abs(sys.poles()) < 1) + + +def eqpt_rhs(t, x, u, params): + return np.array([x[0]/2 + u[0], x[0] - x[1]**2 + u[1], x[1] - x[2]]) + +def eqpt_out(t, x, u, params): + return np.array([x[0], x[1] + u[1]]) + +@pytest.mark.parametrize( + "x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect", [ + # Equilibrium points with input given + (0, None, 0, None, None, None, None, None, 0, [0, 0, 0], [0, 0]), + (0, None, 0, None, None, None, None, None, None, [0, 0, 0], [0, 0]), + ([0.9, 0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0, + [2, sqrt(2), sqrt(2)], [-1, 0]), + ([0.9, -0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0, + [2, -sqrt(2), -sqrt(2)], [-1, 0]), # same input, different eqpt + (0, None, 0, None, None, None, None, None, 1, [0, 0, 0], [0, 0]), #DT + (0, None, [-1, 0], None, None, None, None, None, 1, None, None), #DT + ([0, -0.1, 0], None, [0, -0.25], None, None, None, None, None, 1, #DT + [0, -0.5, -0.25], [0, -0.25]), + + # Equilibrium points with output given + ([0.9, 0.9, 0.9], None, [-0.9, 0], None, [2, sqrt(2)], None, None, + None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]), + (0, None, [0, -0.25], None, [0, -0.75], None, None, None, 1, #DT + [0, -0.5, -0.25], [0, -0.25]), + + # Equilibrium points with mixture of inputs and outputs given + ([0.9, 0.9, 0.9], None, [-1, 0], [0], [2, sqrt(2)], [1], None, + None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]), + (0, None, [0, -0.22], [0], [0, -0.75], [1], None, None, 1, #DT + [0, -0.5, -0.25], [0, -0.25]), + ]) + +def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect): + sys = ct.NonlinearIOSystem( + eqpt_rhs, eqpt_out, dt=dt, states=3, inputs=2, outputs=2) + + xeq, ueq = ct.find_eqpt( + sys, x0, u0, y0, ix=ix, iu=iu, iy=iy, dx0=dx0, idx=idx) + + # If no equilibrium points, skip remaining tests + if x_expect is None: + assert xeq is None + assert ueq is None + return + + # Make sure we are at an appropriate equilibrium point + if dt is None or dt == 0: + # Continuous time system + np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), 0, atol=1e-6) + if y0 is not None: + y0 = np.array(y0) + iy = np.s_[:] if iy is None else np.array(iy) + np.testing.assert_allclose( + eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6) + + else: + # Discrete time system + np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), xeq, atol=1e-6) + if y0 is not None: + y0 = np.array(y0) + iy = np.s_[:] if iy is None else np.array(iy) + np.testing.assert_allclose( + eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6) + + # Check that we got the expected result as well + np.testing.assert_allclose(np.array(xeq), x_expect, atol=1e-6) + np.testing.assert_allclose(np.array(ueq), u_expect, atol=1e-6) From 0bde5710c6a770054f50cb02f524b3a0e59b7ef2 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Wed, 28 Dec 2022 12:25:06 -0800 Subject: [PATCH 2/2] add documentation for continuous vs discrete find_eqpt computation --- control/iosys.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/control/iosys.py b/control/iosys.py index 58c99ff9d..23429b308 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -1994,6 +1994,14 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None, If `return_result` is True, returns the `result` from the :func:`scipy.optimize.root` function. + Notes + ----- + For continuous time systems, equilibrium points are defined as points for + which the right hand side of the differential equation is zero: + :math:`f(t, x_e, u_e) = 0`. For discrete time systems, equilibrium points + are defined as points for which the right hand side of the difference + equation returns the current state: :math:`f(t, x_e, u_e) = x_e`. + """ from scipy.optimize import root