added dynamics and output to statespace and iosystems

sawyerbfuller · sawyerbfuller · commit e99273a8e84a · 2021-03-10T20:22:50.000-08:00
diff --git 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
@@ -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
@@ -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."""
 
