From e99273a8e84a947a1618f065decfcf2112f0bfab Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Wed, 10 Mar 2021 20:22:50 -0800 Subject: [PATCH 1/5] added dynamics and output to statespace and iosystems --- control/iosys.py | 74 +++++++++++++++++++++++----- control/statesp.py | 90 +++++++++++++++++++++++++++++++++++ control/tests/statesp_test.py | 64 +++++++++++++++++++++++++ 3 files changed, 215 insertions(+), 13 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 7ed4c8b05..5308fdf74 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -358,7 +358,7 @@ def _update_params(self, params, warning=False): if (warning): warn("Parameters passed to InputOutputSystem ignored.") - def _rhs(self, t, x, u): + def _rhs(self, t, x, u, params={}): """Evaluate right hand side of a differential or difference equation. Private function used to compute the right hand side of an @@ -368,6 +368,39 @@ def _rhs(self, t, x, u): NotImplemented("Evaluation not implemented for system of type ", type(self)) + def dynamics(self, t, x, u, params={}): + """Compute the dynamics of a differential or difference equation. + + Given time `t`, input `u` and state `x`, returns the value of the + right hand side of the dynamical system. If the system is continuous, + returns the time derivative + + dx/dt = f(t, x, u) + + where `f` is the system's (possibly nonlinear) dynamics function. + If the system is discrete-time, returns the next value of `x`: + + x[t+dt] = f(t, x[t], u[t]) + + Where `t` is a scalar. + + The inputs `x` and `u` must be of the correct length. + + Parameters + ---------- + t : float + the time at which to evaluate + x : array_like + current state + u : array_like + input + + Returns + ------- + dx/dt or x[t+dt] : ndarray + """ + return self._rhs(t, x, u, params) + def _out(self, t, x, u, params={}): """Evaluate the output of a system at a given state, input, and time @@ -378,6 +411,31 @@ def _out(self, t, x, u, params={}): # If no output function was defined in subclass, return state return x + def output(self, t, x, u, params={}): + """Compute the output of the system + + Given time `t`, input `u` and state `x`, returns the output of the + system: + + y = g(t, x, u) + + The inputs `x` and `u` must be of the correct length. + + Parameters + ---------- + t : float + the time at which to evaluate + x : array_like + current state + u : array_like + input + + Returns + ------- + y : ndarray + """ + return self._out(t, x, u, params) + def set_inputs(self, inputs, prefix='u'): """Set the number/names of the system inputs. @@ -694,18 +752,8 @@ def _update_params(self, params={}, warning=True): if params and warning: warn("Parameters passed to LinearIOSystems are ignored.") - def _rhs(self, t, x, u): - # Convert input to column vector and then change output to 1D array - xdot = np.dot(self.A, np.reshape(x, (-1, 1))) \ - + np.dot(self.B, np.reshape(u, (-1, 1))) - return np.array(xdot).reshape((-1,)) - - def _out(self, t, x, u): - # Convert input to column vector and then change output to 1D array - y = np.dot(self.C, np.reshape(x, (-1, 1))) \ - + np.dot(self.D, np.reshape(u, (-1, 1))) - return np.array(y).reshape((-1,)) - + _rhs = StateSpace.dynamics + _out = StateSpace.output class NonlinearIOSystem(InputOutputSystem): """Nonlinear I/O system. diff --git a/control/statesp.py b/control/statesp.py index d2b613024..a25f10358 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -1227,12 +1227,102 @@ def dcgain(self, warn_infinite=False): return self(0, warn_infinite=warn_infinite) if self.isctime() \ else self(1, warn_infinite=warn_infinite) + def dynamics(self, *args): + """Compute the dynamics of the system + + Given input `u` and state `x`, returns the dynamics of the state-space + system. If the system is continuous, returns the time derivative dx/dt + + dx/dt = A x + B u + + where A and B are the state-space matrices of the system. If the + system is discrete-time, returns the next value of `x`: + + x[t+dt] = A x[t] + B u[t] + + The inputs `x` and `u` must be of the correct length for the system. + + The calling signature is ``out = sys.dynamics(t, x[, u])`` + The first argument `t` is ignored because :class:`StateSpace` systems + are time-invariant. It is included so that the dynamics can be passed + to most numerical integrators, such as scipy's `integrate.solve_ivp` and + for consistency with :class:`IOSystem` systems. + + Parameters + ---------- + t : float (ignored) + time + x : array_like + current state + u : array_like (optional) + input, zero if omitted + + Returns + ------- + dx/dt or x[t+dt] : ndarray + """ + if len(args) not in (2, 3): + raise ValueError("received"+len(args)+"args, expected 2 or 3") + if np.size(args[1]) != self.nstates: + raise ValueError("len(x) must be equal to number of states") + if len(args) == 2: # received t and x, ignore t + return self.A.dot(args[1]).reshape((-1,)) # return as row vector + else: # received t, x, and u, ignore t + if np.size(args[2]) != self.ninputs: + raise ValueError("len(u) must be equal to number of inputs") + return self.A.dot(args[1]).reshape((-1,)) \ + + self.B.dot(args[2]).reshape((-1,)) # return as row vector + + def output(self, *args): + """Compute the output of the system + + Given input `u` and state `x`, returns the output `y` of the + state-space system: + + y = C x + D u + + where A and B are the state-space matrices of the system. + + The calling signature is ``y = sys.output(t, x[, u])`` + The first argument `t` is ignored because :class:`StateSpace` systems + are time-invariant. It is included so that the dynamics can be passed + to most numerical integrators, such as scipy's `integrate.solve_ivp` and + for consistency with :class:`IOSystem` systems. + + The inputs `x` and `u` must be of the correct length for the system. + + Parameters + ---------- + t : float (ignored) + time + x : array_like + current state + u : array_like (optional) + input (zero if omitted) + + Returns + ------- + y : ndarray + """ + if len(args) not in (2, 3): + raise ValueError("received"+len(args)+"args, expected 2 or 3") + if np.size(args[1]) != self.nstates: + raise ValueError("len(x) must be equal to number of states") + if len(args) == 2: # received t and x, ignore t + return self.C.dot(args[1]).reshape((-1,)) # return as row vector + else: # received t, x, and u, ignore t + if np.size(args[2]) != self.ninputs: + raise ValueError("len(u) must be equal to number of inputs") + return self.C.dot(args[1]).reshape((-1,)) \ + + self.D.dot(args[2]).reshape((-1,)) # return as row vector + def _isstatic(self): """True if and only if the system has no dynamics, that is, if A and B are zero. """ return not np.any(self.A) and not np.any(self.B) + # TODO: add discrete time check def _convert_to_statespace(sys, **kw): """Convert a system to state space form (if needed). diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 983b9d7a6..2f86578a4 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -8,6 +8,7 @@ """ import numpy as np +from numpy.testing import assert_array_almost_equal import pytest import operator from numpy.linalg import solve @@ -47,6 +48,17 @@ def sys322(self, sys322ABCD): """3-states square system (2 inputs x 2 outputs)""" return StateSpace(*sys322ABCD) + @pytest.fixture + def sys121(self): + """2 state, 1 input, 1 output (siso) system""" + A121 = [[4., 1.], + [2., -3]] + B121 = [[5.], + [-3.]] + C121 = [[2., -4]] + D121 = [[3.]] + return StateSpace(A121, B121, C121, D121) + @pytest.fixture def sys222(self): """2-states square system (2 inputs x 2 outputs)""" @@ -751,6 +763,58 @@ def test_horner(self, sys322): np.squeeze(sys322.horner(1.j)), mag[:, :, 0] * np.exp(1.j * phase[:, :, 0])) + @pytest.mark.parametrize('x', + [[1, 1], [[1], [1]], np.atleast_2d([1,1]).T]) + @pytest.mark.parametrize('u', [0, 1, np.atleast_1d(2)]) + def test_dynamics_and_output_siso(self, x, u, sys121): + assert_array_almost_equal( + sys121.dynamics(0, x, u), + sys121.A.dot(x).reshape((-1,)) + sys121.B.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys121.output(0, x, u), + sys121.C.dot(x).reshape((-1,)) + sys121.D.dot(u).reshape((-1,))) + + # too few and too many states and inputs + @pytest.mark.parametrize('x', [0, 1, [], [1, 2, 3], np.atleast_1d(2)]) + def test_error_x_dynamics_and_output_siso(self, x, sys121): + with pytest.raises(ValueError): + sys121.dynamics(0, x) + with pytest.raises(ValueError): + sys121.output(0, x) + @pytest.mark.parametrize('u', [[1, 1], np.atleast_1d((2, 2))]) + def test_error_u_dynamics_output_siso(self, u, sys121): + with pytest.raises(ValueError): + sys121.dynamics(0, 1, u) + with pytest.raises(ValueError): + sys121.output(0, 1, u) + + @pytest.mark.parametrize('x', + [[1, 1], [[1], [1]], np.atleast_2d([1,1]).T]) + @pytest.mark.parametrize('u', + [[1, 1], [[1], [1]], np.atleast_2d([1,1]).T]) + def test_dynamics_and_output_mimo(self, x, u, sys222): + assert_array_almost_equal( + sys222.dynamics(0, x, u), + sys222.A.dot(x).reshape((-1,)) + sys222.B.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys222.output(0, x, u), + sys222.C.dot(x).reshape((-1,)) + sys222.D.dot(u).reshape((-1,))) + + # too few and too many states and inputs + @pytest.mark.parametrize('x', [0, 1, [1, 1, 1]]) + def test_error_x_dynamics_mimo(self, x, sys222): + with pytest.raises(ValueError): + sys222.dynamics(0, x) + with pytest.raises(ValueError): + sys222.output(0, x) + @pytest.mark.parametrize('u', [0, 1, [1, 1, 1]]) + def test_error_u_dynamics_mimo(self, u, sys222): + with pytest.raises(ValueError): + sys222.dynamics(0, (1, 1), u) + with pytest.raises(ValueError): + sys222.output(0, (1, 1), u) + + class TestRss: """These are tests for the proper functionality of statesp.rss.""" From 9678bd133eee165c2c9e513d7f94361ae0a801e8 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Thu, 11 Mar 2021 09:17:36 -0800 Subject: [PATCH 2/5] add remark in docstring for iosys._rhs and _out that they are intended for fast evaluation. --- control/iosys.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 5308fdf74..4fd3dd5af 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -362,7 +362,9 @@ def _rhs(self, t, x, u, params={}): """Evaluate right hand side of a differential or difference equation. Private function used to compute the right hand side of an - input/output system model. + input/output system model. Intended for fast + evaluation; for a more user-friendly interface + you may want to use :meth:`dynamics`. """ NotImplemented("Evaluation not implemented for system of type ", @@ -405,7 +407,9 @@ def _out(self, t, x, u, params={}): """Evaluate the output of a system at a given state, input, and time Private function used to compute the output of of an input/output - system model given the state, input, parameters, and time. + system model given the state, input, parameters. Intended for fast + evaluation; for a more user-friendly interface you may want to use + :meth:`output`. """ # If no output function was defined in subclass, return state From c0f7d06ff86894d37af3fcef65a9484a93ee0a13 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Thu, 11 Mar 2021 09:45:00 -0800 Subject: [PATCH 3/5] fix to possibly fix pytest errors when using matrix --- control/statesp.py | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index a25f10358..9ef476e8e 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -1262,16 +1262,20 @@ def dynamics(self, *args): dx/dt or x[t+dt] : ndarray """ if len(args) not in (2, 3): - raise ValueError("received"+len(args)+"args, expected 2 or 3") - if np.size(args[1]) != self.nstates: + raise ValueError("received" + len(args) + "args, expected 2 or 3") + + x = np.reshape(args[1], (-1, 1)) # force to a column in case matrix + if np.size(x) != self.nstates: raise ValueError("len(x) must be equal to number of states") + if len(args) == 2: # received t and x, ignore t - return self.A.dot(args[1]).reshape((-1,)) # return as row vector + return self.A.dot(x).reshape((-1,)) # return as row vector else: # received t, x, and u, ignore t - if np.size(args[2]) != self.ninputs: + u = np.reshape(args[2], (-1, 1)) # force to a column in case matrix + if np.size(u) != self.ninputs: raise ValueError("len(u) must be equal to number of inputs") - return self.A.dot(args[1]).reshape((-1,)) \ - + self.B.dot(args[2]).reshape((-1,)) # return as row vector + return self.A.dot(x).reshape((-1,)) \ + + self.B.dot(u).reshape((-1,)) # return as row vector def output(self, *args): """Compute the output of the system @@ -1306,15 +1310,19 @@ def output(self, *args): """ if len(args) not in (2, 3): raise ValueError("received"+len(args)+"args, expected 2 or 3") - if np.size(args[1]) != self.nstates: + + x = np.reshape(args[1], (-1, 1)) # force to a column in case matrix + if np.size(x) != self.nstates: raise ValueError("len(x) must be equal to number of states") + if len(args) == 2: # received t and x, ignore t - return self.C.dot(args[1]).reshape((-1,)) # return as row vector + return self.C.dot(x).reshape((-1,)) # return as row vector else: # received t, x, and u, ignore t - if np.size(args[2]) != self.ninputs: + u = np.reshape(args[2], (-1, 1)) # force to a column in case matrix + if np.size(u) != self.ninputs: raise ValueError("len(u) must be equal to number of inputs") - return self.C.dot(args[1]).reshape((-1,)) \ - + self.D.dot(args[2]).reshape((-1,)) # return as row vector + return self.C.dot(x).reshape((-1,)) \ + + self.D.dot(u).reshape((-1,)) # return as row vector def _isstatic(self): """True if and only if the system has no dynamics, that is, From 64d0dde4b37675b87edb853f42571ed1b81414e4 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Thu, 11 Mar 2021 09:53:11 -0800 Subject: [PATCH 4/5] another try at fixing failing unit tests --- control/iosys.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 4fd3dd5af..1f33bfc76 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -756,8 +756,17 @@ def _update_params(self, params={}, warning=True): if params and warning: warn("Parameters passed to LinearIOSystems are ignored.") - _rhs = StateSpace.dynamics - _out = StateSpace.output + def _rhs(self, t, x, u): + # Convert input to column vector and then change output to 1D array + xdot = np.dot(self.A, np.reshape(x, (-1, 1))) \ + + np.dot(self.B, np.reshape(u, (-1, 1))) + return np.array(xdot).reshape((-1,)) + + def _out(self, t, x, u): + # Convert input to column vector and then change output to 1D array + y = np.dot(self.C, np.reshape(x, (-1, 1))) \ + + np.dot(self.D, np.reshape(u, (-1, 1))) + return np.array(y).reshape((-1,)) class NonlinearIOSystem(InputOutputSystem): """Nonlinear I/O system. From 23c4b09ae67bca6415cce13b9113362416dc6286 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Fri, 12 Mar 2021 13:41:21 -0800 Subject: [PATCH 5/5] fixes/cleanups suggested by @murrayrm --- control/iosys.py | 9 +++++---- control/statesp.py | 29 ++++++++++------------------- control/tests/statesp_test.py | 14 +++++++++++++- 3 files changed, 28 insertions(+), 24 deletions(-) diff --git a/control/iosys.py b/control/iosys.py index 1f33bfc76..e28de59f2 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -370,7 +370,7 @@ def _rhs(self, t, x, u, params={}): NotImplemented("Evaluation not implemented for system of type ", type(self)) - def dynamics(self, t, x, u, params={}): + def dynamics(self, t, x, u): """Compute the dynamics of a differential or difference equation. Given time `t`, input `u` and state `x`, returns the value of the @@ -401,7 +401,7 @@ def dynamics(self, t, x, u, params={}): ------- dx/dt or x[t+dt] : ndarray """ - return self._rhs(t, x, u, params) + return self._rhs(t, x, u) def _out(self, t, x, u, params={}): """Evaluate the output of a system at a given state, input, and time @@ -415,7 +415,7 @@ def _out(self, t, x, u, params={}): # If no output function was defined in subclass, return state return x - def output(self, t, x, u, params={}): + def output(self, t, x, u): """Compute the output of the system Given time `t`, input `u` and state `x`, returns the output of the @@ -438,7 +438,7 @@ def output(self, t, x, u, params={}): ------- y : ndarray """ - return self._out(t, x, u, params) + return self._out(t, x, u) def set_inputs(self, inputs, prefix='u'): """Set the number/names of the system inputs. @@ -768,6 +768,7 @@ def _out(self, t, x, u): + np.dot(self.D, np.reshape(u, (-1, 1))) return np.array(y).reshape((-1,)) + class NonlinearIOSystem(InputOutputSystem): """Nonlinear I/O system. diff --git a/control/statesp.py b/control/statesp.py index 9ef476e8e..758b91ed9 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -1227,7 +1227,7 @@ def dcgain(self, warn_infinite=False): return self(0, warn_infinite=warn_infinite) if self.isctime() \ else self(1, warn_infinite=warn_infinite) - def dynamics(self, *args): + def dynamics(self, t, x, u=0): """Compute the dynamics of the system Given input `u` and state `x`, returns the dynamics of the state-space @@ -1242,11 +1242,10 @@ def dynamics(self, *args): The inputs `x` and `u` must be of the correct length for the system. - The calling signature is ``out = sys.dynamics(t, x[, u])`` The first argument `t` is ignored because :class:`StateSpace` systems are time-invariant. It is included so that the dynamics can be passed - to most numerical integrators, such as scipy's `integrate.solve_ivp` and - for consistency with :class:`IOSystem` systems. + to most numerical integrators, such as :func:`scipy.integrate.solve_ivp` + and for consistency with :class:`IOSystem` systems. Parameters ---------- @@ -1261,23 +1260,19 @@ def dynamics(self, *args): ------- dx/dt or x[t+dt] : ndarray """ - if len(args) not in (2, 3): - raise ValueError("received" + len(args) + "args, expected 2 or 3") - - x = np.reshape(args[1], (-1, 1)) # force to a column in case matrix + x = np.reshape(x, (-1, 1)) # force to a column in case matrix if np.size(x) != self.nstates: raise ValueError("len(x) must be equal to number of states") - - if len(args) == 2: # received t and x, ignore t + if u is 0: return self.A.dot(x).reshape((-1,)) # return as row vector else: # received t, x, and u, ignore t - u = np.reshape(args[2], (-1, 1)) # force to a column in case matrix + u = np.reshape(u, (-1, 1)) # force to a column in case matrix if np.size(u) != self.ninputs: raise ValueError("len(u) must be equal to number of inputs") return self.A.dot(x).reshape((-1,)) \ + self.B.dot(u).reshape((-1,)) # return as row vector - def output(self, *args): + def output(self, t, x, u=0): """Compute the output of the system Given input `u` and state `x`, returns the output `y` of the @@ -1287,7 +1282,6 @@ def output(self, *args): where A and B are the state-space matrices of the system. - The calling signature is ``y = sys.output(t, x[, u])`` The first argument `t` is ignored because :class:`StateSpace` systems are time-invariant. It is included so that the dynamics can be passed to most numerical integrators, such as scipy's `integrate.solve_ivp` and @@ -1308,17 +1302,14 @@ def output(self, *args): ------- y : ndarray """ - if len(args) not in (2, 3): - raise ValueError("received"+len(args)+"args, expected 2 or 3") - - x = np.reshape(args[1], (-1, 1)) # force to a column in case matrix + x = np.reshape(x, (-1, 1)) # force to a column in case matrix if np.size(x) != self.nstates: raise ValueError("len(x) must be equal to number of states") - if len(args) == 2: # received t and x, ignore t + if u is 0: return self.C.dot(x).reshape((-1,)) # return as row vector else: # received t, x, and u, ignore t - u = np.reshape(args[2], (-1, 1)) # force to a column in case matrix + u = np.reshape(u, (-1, 1)) # force to a column in case matrix if np.size(u) != self.ninputs: raise ValueError("len(u) must be equal to number of inputs") return self.C.dot(x).reshape((-1,)) \ diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 2f86578a4..1eec5eadb 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -773,6 +773,12 @@ def test_dynamics_and_output_siso(self, x, u, sys121): assert_array_almost_equal( sys121.output(0, x, u), sys121.C.dot(x).reshape((-1,)) + sys121.D.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys121.dynamics(0, x), + sys121.A.dot(x).reshape((-1,))) + assert_array_almost_equal( + sys121.output(0, x), + sys121.C.dot(x).reshape((-1,))) # too few and too many states and inputs @pytest.mark.parametrize('x', [0, 1, [], [1, 2, 3], np.atleast_1d(2)]) @@ -799,6 +805,12 @@ def test_dynamics_and_output_mimo(self, x, u, sys222): assert_array_almost_equal( sys222.output(0, x, u), sys222.C.dot(x).reshape((-1,)) + sys222.D.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys222.dynamics(0, x), + sys222.A.dot(x).reshape((-1,))) + assert_array_almost_equal( + sys222.output(0, x), + sys222.C.dot(x).reshape((-1,))) # too few and too many states and inputs @pytest.mark.parametrize('x', [0, 1, [1, 1, 1]]) @@ -807,7 +819,7 @@ def test_error_x_dynamics_mimo(self, x, sys222): sys222.dynamics(0, x) with pytest.raises(ValueError): sys222.output(0, x) - @pytest.mark.parametrize('u', [0, 1, [1, 1, 1]]) + @pytest.mark.parametrize('u', [1, [1, 1, 1]]) def test_error_u_dynamics_mimo(self, u, sys222): with pytest.raises(ValueError): sys222.dynamics(0, (1, 1), u)