diff --git a/control/iosys.py b/control/iosys.py index 9b33d2161..a32978b58 100644 --- 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 index 7d3b9fee9..003c43d03 100644 --- 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(