Merge pull request #798 from adswid/fix-find_eqpt

murrayrm · web-flow · commit 23a7791cbcc3 · 2022-11-25T17:37:32.000-08:00
Fix find_eqpt when y0 is None
diff --git a/control/iosys.py b/control/iosys.py
@@ -2138,10 +2138,15 @@ def rootfun(z):
             dx = sys._rhs(t, x, u) - dx0
             if dtime:
                 dx -= x           # TODO: check
-            dy = sys._out(t, x, u) - y0
 
-            # Map the results into the constrained variables
-            return np.concatenate((dx[deriv_vars], dy[output_vars]), axis=0)
+            # If no y0 is given, don't evaluate the output function
+            if y0 is None:
+                return dx[deriv_vars]
+            else:
+                dy = sys._out(t, x, u) - y0
+
+                # Map the results into the constrained variables
+                return np.concatenate((dx[deriv_vars], dy[output_vars]), axis=0)
 
         # Set the initial condition for the root finding algorithm
         z0 = np.concatenate((x[state_vars], u[input_vars]), axis=0)
diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py
@@ -822,6 +822,17 @@ 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
+        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],
+            idx=[2, 3, 4, 5], ix=[0, 1, 2, 3], return_result=True)
+        assert result.success
+        np.testing.assert_array_almost_equal(
+            nlsys_full._out(0, xeq, ueq)[[2, 3]], [0.1, 0.1], decimal=5)
+        np.testing.assert_array_almost_equal(
+            nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5)
+
         # Fix one input and vary the other
         nlsys_full = ios.NonlinearIOSystem(pvtol_full, None)
         xeq, ueq, result = ios.find_eqpt(
