8000 Merge pull request #824 from murrayrm/findeqpt_dtime-24Dec2022 · ruturajsambhusvt/python-control@313ca74 · GitHub
[go: up one dir, main page]
Skip to content

Commit 313ca74

Browse files
murrayrmmurrayrm
authored
Merge pull request python-control#824 from murrayrm/findeqpt_dtime-24Dec2022
Update find_eqpts to handle discrete time systems
2 parents f15fc0f + 0bde571 commit 313ca74

File tree

2 files changed

+108
-25
lines changed

2 files changed

+108
-25
lines changed

‎control/iosys.py‎

Lines changed: 29 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1994,6 +1994,14 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
19941994
If `return_result` is True, returns the `result` from the
19951995
:func:`scipy.optimize.root` function.
19961996
1997+
Notes
1998+
-----
1999+
For continuous time systems, equilibrium points are defined as points for
2000+
which the right hand side of the differential equation is zero:
2001+
:math:`f(t, x_e, u_e) = 0`. For discrete time systems, equilibrium points
2002+
are defined as points for which the right hand side of the difference
2003+
equation returns the current state: :math:`f(t, x_e, u_e) = x_e`.
2004+
19972005
"""
19982006
from scipy.optimize import root
19992007

@@ -2010,11 +2018,6 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
20102018
if np.isscalar(y0):
20112019
y0 = np.ones((ninputs,)) * y0
20122020

2013-
# Discrete-time not yet supported
2014-
if isdtime(sys, strict=True):
2015-
raise NotImplementedError(
2016-
"Discrete time systems are not yet supported.")
2017-
20182021
# Make sure the input arguments match the sizes of the system
20192022
if len(x0) != nstates or \
20202023
(u0 is not None and len(u0) != ninputs) or \
@@ -2030,18 +2033,28 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
20302033
# Special cases: either inputs or outputs are constrained
20312034
if y0 is None:
20322035
# Take u0 as fixed and minimize over x
2033-
# TODO: update to allow discrete time systems
2034-
def ode_rhs(z): return sys._rhs(t, z, u0)
2035-
result = root(ode_rhs, x0)
2036+
if sys.isdtime(strict=True):
2037+
def state_rhs(z): return sys._rhs(t, z, u0) - z
2038+
else:
2039+
def state_rhs(z): return sys._rhs(t, z, u0)
2040+
2041+
result = root(state_rhs, x0)
20362042
z = (result.x, u0, sys._out(t, result.x, u0))
2043+
20372044
else:
20382045
# Take y0 as fixed and minimize over x and u
2039-
def rootfun(z):
2040-
# Split z into x and u
2041-
x, u = np.split(z, [nstates])
2042-
# TODO: update to allow discrete time systems
2043-
return np.concatenate(
2044-
(sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0)
2046+
if sys.isdtime(strict=True):
2047+
def rootfun(z):
2048+
x, u = np.split(z, [nstates])
2049+
return np.concatenate(
2050+
(sys._rhs(t, x, u) - x, sys._out(t, x, u) - y0),
2051+
axis=0)
2052+
else:
2053+
def rootfun(z):
2054+
x, u = np.split(z, [nstates])
2055+
return np.concatenate(
2056+
(sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0)
2057+
20452058
z0 = np.concatenate((x0, u0), axis=0) # Put variables together
20462059
result = root(rootfun, z0) # Find the eq point
20472060
x, u = np.split(result.x, [nstates]) # Split result back in two
@@ -2135,7 +2148,6 @@ def rootfun(z):
21352148

21362149
# Keep track of the number of states in the set of free variables
21372150
nstate_vars = len(state_vars)
2138-
dtime = isdtime(sys, strict=True)
21392151

21402152
def rootfun(z):
21412153
# Map the vector of values into the states and inputs
@@ -2144,8 +2156,8 @@ def rootfun(z):
21442156

21452157
# Compute the update and output maps
21462158
dx = sys._rhs(t, x, u) - dx0
2147-
if dtime:
2148-
dx -= x # TODO: check
2159+
if sys.isdtime(strict=True):
2160+
dx -= x
21492161

21502162
# If no y0 is given, don't evaluate the output function
21512163
if y0 is None:

‎control/tests/iosys_test.py‎

Lines changed: 79 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,10 @@
1010

1111
import re
1212
import warnings
13+
import pytest
1314

1415
import numpy as np
15-
import pytest
16+
from math import sqrt
1617

1718
import control as ct
1819
from control import iosys as ios
@@ -238,7 +239,7 @@ def test_linearize_named_signals(self, kincar):
238239
assert lin_nocopy.find_state('x') is None
239240

240241
# if signal names are provided, they should override those of kincar
241-
linearized_newnames = kincar.linearize([0, 0, 0], [0, 0],
242+
linearized_newnames = kincar.linearize([0, 0, 0], [0, 0],
242243
name='linearized',
243244
copy_names=True, inputs=['v2', 'phi2'], outputs=['x2','y2'])
244245
assert linearized_newnames.name == 'linearized'
@@ -766,8 +767,8 @@ def nlsys_output(t, x, u, params):
766767
np.testing.assert_allclose(ios_t, lin_t,atol=0.002,rtol=0.)
767768
np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.)
768769

769-
def test_find_eqpts(self, tsys):
770-
"""Test find_eqpt function"""
770+
def test_find_eqpts_dfan(self, tsys):
771+
"""Test find_eqpt function on dfan example"""
771772
# Simple equilibrium point with no inputs
772773
nlsys = ios.NonlinearIOSystem(predprey)
773774
xeq, ueq, result = ios.find_eqpt(
@@ -836,7 +837,7 @@ def test_find_eqpts(self, tsys):
836837
np.testing.assert_array_almost_equal(
837838
nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5)
838839

839-
# The same test as previous, but now all constraints are in the state vector
840+
# Same test as before, but now all constraints are in the state vector
840841
nlsys_full = ios.NonlinearIOSystem(pvtol_full, None)
841842
xeq, ueq, result = ios.find_eqpt(
842843
nlsys_full, [0, 0, 0.1, 0.1, 0, 0], [0.01, 4*9.8],
@@ -1482,7 +1483,7 @@ def test_linear_interconnection():
14821483
tf_siso = ct.tf(1, [0.1, 1])
14831484
ss_siso = ct.ss(1, 2, 1, 1)
14841485
nl_siso = ios.NonlinearIOSystem(
1485-
lambda t, x, u, params: x*x,
1486+
lambda t, x, u, params: x*x,
14861487
lambda t, x, u, params: u*x, states=1, inputs=1, outputs=1)
14871488

14881489
# Create a "regular" InterconnectedSystem
@@ -1530,7 +1531,7 @@ def test_linear_interconnection():
15301531
np.testing.assert_array_almost_equal(io_connect.C, ss_connect.C)
15311532
np.testing.assert_array_almost_equal(io_connect.D, ss_connect.D)
15321533

1533-
# make sure interconnections of linear systems are linear and
1534+
# make sure interconnections of linear systems are linear and
15341535
# if a nonlinear system is included then system is nonlinear
15351536
assert isinstance(ss_siso*ss_siso, ios.LinearIOSystem)
15361537
assert isinstance(tf_siso*ss_siso, ios.LinearIOSystem)
@@ -1541,7 +1542,7 @@ def test_linear_interconnection():
15411542
assert ~isinstance(tf_siso*nl_siso, ios.LinearIOSystem)
15421543
assert ~isinstance(nl_siso*tf_siso, ios.LinearIOSystem)
15431544
assert ~isinstance(nl_siso*nl_siso, ios.LinearIOSystem)
1544-
1545+
15451546

15461547
def predprey(t, x, u, params={}):
15471548
"""Predator prey dynamics"""
@@ -1898,3 +1899,73 @@ def test_rss():
18981899
with pytest.warns(UserWarning, match="may be interpreted as continuous"):
18991900
sys = ct.drss(2, 1, 1, dt=None)
19001901
assert np.all(np.abs(sys.poles()) < 1)
1902+
1903+
1904+
def eqpt_rhs(t, x, u, params):
1905+
return np.array([x[0]/2 + u[0], x[0] - x[1]**2 + u[1], x[1] - x[2]])
1906+
1907+
def eqpt_out(t, x, u, params):
1908+
return np.array([x[0], x[1] + u[1]])
1909+
1910+
@pytest.mark.parametrize(
1911+
"x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect", [
1912+
# Equilibrium points with input given
1913+
(0, None, 0, None, None, None, None, None, 0, [0, 0, 0], [0, 0]),
1914+
(0, None, 0, None, None, None, None, None, None, [0, 0, 0], [0, 0]),
1915+
([0.9, 0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0,
1916+
[2, sqrt(2), sqrt(2)], [-1, 0]),
1917+
([0.9, -0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0,
1918+
[2, -sqrt(2), -sqrt(2)], [-1, 0]), # same input, different eqpt
1919+
(0, None, 0, None, None, None, None, None, 1, [0, 0, 0], [0, 0]), #DT
1920+
(0, None, [-1, 0], None, None, None, None, None, 1, None, None), #DT
1921+
([0, -0.1, 0], None, [0, -0.25], None, None, None, None, None, 1, #DT
1922+
[0, -0.5, -0.25], [0, -0.25]),
1923+
1924+
# Equilibrium points with output given
1925+
([0.9, 0.9, 0.9], None, [-0.9, 0], None, [2, sqrt(2)], None, None,
1926+
None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]),
1927+
(0, None, [0, -0.25], None, [0, -0.75], None, None, None, 1, #DT
1928+
[0, -0.5, -0.25], [0, -0.25]),
1929+
1930+
# Equilibrium points with mixture of inputs and outputs given
1931+
([0.9, 0.9, 0.9], None, [-1, 0], [0], [2, sqrt(2)], [1], None,
1932+
None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]),
1933+
(0, None, [0, -0.22], [0], [0, -0.75], [1], None, None, 1, #DT
1934+
[0, -0.5, -0.25], [0, -0.25]),
1935+
])
1936+
1937+
def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect):
1938+
sys = ct.NonlinearIOSystem(
1939+
eqpt_rhs, eqpt_out, dt=dt, states=3, inputs=2, outputs=2)
1940+
1941+
xeq, ueq = ct.find_eqpt(
1942+
sys, x0, u0, y0, ix=ix, iu=iu, iy=iy, dx0=dx0, idx=idx)
1943+
1944+
# If no equilibrium points, skip remaining tests
1945+
if x_expect is None:
1946+
assert xeq is None
1947+
assert ueq is None
1948+
return
1949+
1950+
# Make sure we are at an appropriate equilibrium point
1951+
if dt is None or dt == 0:
1952+
# Continuous time system
1953+
np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), 0, atol=1e-6)
1954+
if y0 is not None:
1955+
y0 = np.array(y0)
1956+
iy = np.s_[:] if iy is None else np.array(iy)
1957+
np.testing.assert_allclose(
1958+
eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6)
1959+
1960+
else:
1961+
# Discrete time system
1962+
np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), xeq, atol=1e-6)
1963+
if y0 is not None:
1964+
y0 = np.array(y0)
1965+
iy = np.s_[:] if iy is None else np.array(iy)
1966+
np.testing.assert_allclose(
1967+
eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6)
1968+
1969+
# Check that we got the expected result as well
1970+
np.testing.assert_allclose(np.array(xeq), x_expect, atol=1e-6)
1971+
np.testing.assert_allclose(np.array(ueq), u_expect, atol=1e-6)

0 commit comments

Comments
 (0)
0