bnavigator · Jul 30, 2022 · Jun 18, 2022 · Jul 11, 2022 · Jul 11, 2022 · Jul 15, 2022
diff --git a/control/__init__.py b/control/__init__.py
@@ -72,6 +72,7 @@
 from .config import *
 from .sisotool import *
 from .iosys import *
+from .passivity import *
 
 # Exceptions
 from .exception import *

diff --git a/control/passivity.py b/control/passivity.py
@@ -1,42 +1,81 @@
 '''
 Author: Mark Yeatman  
-Date: May 15, 2022
+Date: July 17, 2022
 '''
 
 import numpy as np
-from control import statesp as ss
+from control import statesp
+from control.exception import ControlArgument, ControlDimension
 
 try:
     import cvxopt as cvx
 except ImportError as e:
     cvx = None
 
+eps = np.nextafter(0, 1)
 
-def ispassive(sys):
+
+def _make_P_basis_matrices(n, rho, nu, make_LMI_matrix_func):
     '''
-    Indicates if a linear time invariant (LTI) system is passive
+    Utility function to make basis matrices for a LMI from a 
+    functional make_LMI_matrix_func and a symmetric matrix P of size n by n
+    representing a parametrized symbolic matrix
+    '''
+    matrix_list = []
+    for i in range(0, n):
+        for j in range(0, n):
+            if j <= i:
+                P = np.zeros((n, n))
+                P[i, j] = 1.0
+                P[j, i] = 1.0
+                matrix_list.append(make_LMI_matrix_func(P, 0, 0, 0).flatten())
+    P = eps*np.eye(n)
+    matrix_list.append(make_LMI_matrix_func(P, rho, nu, 0).flatten())
+    return matrix_list
 
-    Constructs a linear matrix inequality and a feasibility optimization
-    such that if a solution exists, the system is passive.
 
-    The source for the algorithm is: 
-    McCourt, Michael J., and Panos J. Antsaklis. 
-        "Demonstrating passivity and dissipativity using computational methods." ISIS 8 (2013).
+def _P_pos_def_constraint(n):
+    '''
+    Utility function to make basis matrices for a LMI that ensures parametrized symbolic matrix 
+    of size n by n is positive definite.
+    '''
+    matrix_list = []
+    for i in range(0, n):
+        for j in range(0, n):
+            if j <= i:
+                P = np.zeros((n, n))
+                P[i, j] = -1.0
+                P[j, i] = -1.0
+                matrix_list.append(P.flatten())
+    matrix_list.append(np.zeros((n, n)).flatten())
+    return matrix_list
 
-    Parameters
-    ----------
-    sys: A continuous LTI system
-        System to be checked.
 
-    Returns
-    -------
-    bool: 
-        The input system passive.
+def _solve_passivity_LMI(sys, rho=None, nu=None):
+    '''
+    Constructs a linear matrix inequality such that if a solution exists 
+    and the last element of the solution is positive, the system is passive.
+
+    The sources for the algorithm are: 
+
+    McCourt, Michael J., and Panos J. Antsaklis
+        "Demonstrating passivity and dissipativity using computational methods." 
+
+    Nicholas Kottenstette and Panos J. Antsaklis
+        "Relationships Between Positive Real, Passive Dissipative, & Positive Systems" 
+        equation 36.
     '''
     if cvx is None:
         raise ModuleNotFoundError("cvxopt required for passivity module")
 
-    sys = ss._convert_to_statespace(sys)
+    if sys.ninputs != sys.noutputs:
+        raise ControlDimension(
+            "The number of system inputs must be the same as the number of system outputs.")
+
+    if rho is None and nu is None:
+        raise ControlArgument("rho or nu must be given a float value.")
+
+    sys = statesp._convert_to_statespace(sys)
 
     A = sys.A
     B = sys.B
@@ -45,43 +84,126 @@ def ispassive(sys):
 
     # account for strictly proper systems
     [n, m] = D.shape
-    D = D + np.nextafter(0, 1)*np.eye(n, m)
+    D = D + eps * np.eye(n, m)
 
     [n, _] = A.shape
-    A = A - np.nextafter(0, 1)*np.eye(n)
-
-    def make_LMI_matrix(P):
-        V = np.vstack((
-            np.hstack((A.T @ P + P@A, P@B)),
-            np.hstack((B.T@P, np.zeros_like(D))))
-        )
-        return V
+    A = A - eps*np.eye(n)
+
+    def make_LMI_matrix(P, rho, nu, one):
+        q = sys.noutputs
+        Q = -rho*np.eye(q, q)
+        S = 1.0/2.0*(one+rho*nu)*np.eye(q)
+        R = -nu*np.eye(m)
+        if sys.isctime():
+            off_diag = P@B - (C.T@S + C.T@Q@D)
+            return np.vstack((
+                np.hstack((A.T @ P + P@A - C.T@Q@C,  off_diag)),
+                np.hstack((off_diag.T, -(D.T@Q@D + D.T@S + S.T@D + R)))
+            ))
+        else:
+            off_diag = A.T@P@B - (C.T@S + C.T@Q@D)
+            return np.vstack((
+                np.hstack((A.T @ P  @ A - P - C.T@Q@C,  off_diag)),
+                np.hstack((off_diag.T, B.T@P@B-(D.T@Q@D + D.T@S + S.T@D + R)))
+            ))
+
+    n = sys.nstates
+
+    # LMI for passivity indices from A,B,C,D
+    sys_matrix_list = list()
+    sys_constants = -np.vstack((
+        np.hstack((np.zeros_like(A),  np.zeros_like(C.T))),
+        np.hstack((np.zeros_like(C), np.zeros_like(D))))
+    )
 
-    matrix_list = []
-    state_space_size = sys.nstates
-    for i in range(0, state_space_size):
-        for j in range(0, state_space_size):
-            if j <= i:
-                P = np.zeros_like(A)
-                P[i, j] = 1.0
-                P[j, i] = 1.0
-                matrix_list.append(make_LMI_matrix(P).flatten())
+    if rho is not None:
+        sys_matrix_list = _make_P_basis_matrices(
+            n, eps, 1.0, make_LMI_matrix)
+        sys_constants = -make_LMI_matrix(np.zeros_like(A), rho, eps, 1.0)
+    else:
+        sys_matrix_list = _make_P_basis_matrices(
+            n, 1.0, eps, make_LMI_matrix)
+        sys_constants = -make_LMI_matrix(np.zeros_like(A), eps, nu, 1.0)
 
-    coefficents = np.vstack(matrix_list).T
+    sys_coefficents = np.vstack(sys_matrix_list).T
 
-    constants = -np.vstack((
-        np.hstack((np.zeros_like(A),  - C.T)),
-        np.hstack((- C, -D - D.T)))
-    )
+    # LMI to ensure P is positive definite
+    P_matrix_list = _P_pos_def_constraint(n)
+    P_coefficents = np.vstack(P_matrix_list).T
+    P_constants = np.zeros((n, n))
 
+    # cost function
     number_of_opt_vars = int(
-        (state_space_size**2-state_space_size)/2 + state_space_size)
+        (n**2-n)/2 + n)
     c = cvx.matrix(0.0, (number_of_opt_vars, 1))
+    c = cvx.matrix(np.append(np.array(c), -1.0))
+
+    Gs = [cvx.matrix(sys_coefficents)] + [cvx.matrix(P_coefficents)]
+    hs = [cvx.matrix(sys_constants)] + [cvx.matrix(P_constants)]
 
     # crunch feasibility solution
     cvx.solvers.options['show_progress'] = False
-    sol = cvx.solvers.sdp(c,
-                          Gs=[cvx.matrix(coefficents)],
-                          hs=[cvx.matrix(constants)])
+    sol = cvx.solvers.sdp(c, Gs=Gs, hs=hs)
+    return sol["x"]
+
 
-    return (sol["x"] is not None)
+def get_output_fb_index(sys):
+    '''
+    Returns the output feedback passivity index for the input system. This is largest gain that can be placed in
+    positive feedback with a system such that the new interconnected system is passive.
+    Parameters
+    ----------
+    sys: An LTI system
+        System to be checked.
+
+    Returns
+    -------
+    float: 
+        The OFP index 
+    '''
+    sol = _solve_passivity_LMI(sys, nu=eps)
+    if sol is None:
+        return -np.inf
+    else:
+        return sol[-1]
+
+
+def get_input_ff_index(sys):
+    '''
+    Returns the input feedforward passivity (IFP) index for the input system. This is the largest gain that can be 
+    placed in negative parallel interconnection with a system such that the new interconnected system is passive.
+    Parameters
+    ----------
+    sys: An LTI system
+        System to be checked.
+
+    Returns
+    -------
+    float: 
+        The IFP index 
+    '''
+
+    sol = _solve_passivity_LMI(sys, rho=eps)
+    if sol is None:
+        return -np.inf
+    else:
+        return sol[-1]
+
+
+def ispassive(sys):
+    '''
+    Indicates if a linear time invariant (LTI) system is passive
+
+    Parameters
+    ----------
+    sys: An LTI system
+        System to be checked.
+
+    Returns
+    -------
+    bool: 
+        The input system is passive.
+    '''
+    output_fb_index = get_output_fb_index(sys)
+    input_ff_index = get_input_ff_index(sys)
+    return output_fb_index >= 0 or input_ff_index >= 0
diff --git a/control/tests/passivity_test.py b/control/tests/passivity_test.py
@@ -4,14 +4,14 @@
 '''
 import pytest
 import numpy
-from control import ss, passivity, tf
+from control import ss, passivity, tf, sample_system, parallel, feedback
 from control.tests.conftest import cvxoptonly
-
+from control.exception import ControlArgument, ControlDimension
 
 pytestmark = cvxoptonly
 
 
-def test_ispassive():
+def test_ispassive_ctime():
     A = numpy.array([[0, 1], [-2, -2]])
     B = numpy.array([[0], [1]])
     C = numpy.array([[-1, 2]])
@@ -28,6 +28,69 @@ def test_ispassive():
     assert(not passivity.ispassive(sys))
 
 
+def test_ispassive_dtime():
+    A = numpy.array([[0, 1], [-2, -2]])
+    B = numpy.array([[0], [1]])
+    C = numpy.array([[-1, 2]])
+    D = numpy.array([[1.5]])
+    sys = ss(A, B, C, D)
+    sys = sample_system(sys, 1, method='bilinear')
+    assert(passivity.ispassive(sys))
+
+
+def test_passivity_indices_ctime():
+    sys = tf([1, 1, 5, 0.1], [1, 2, 3, 4])
+
+    iff_index = passivity.get_input_ff_index(sys)
+    ofb_index = passivity.get_output_fb_index(sys)
+
+    assert(isinstance(ofb_index, float))
+
+    sys_ff = parallel(sys, -iff_index)
+    sys_fb = feedback(sys, ofb_index, sign=1)
+
+    assert(sys_ff.ispassive())
+    assert(sys_fb.ispassive())
+
+    sys_ff = parallel(sys, -iff_index-1e-6)
+    sys_fb = feedback(sys, ofb_index+1e-6,  sign=1)
+
+    assert(not sys_ff.ispassive())
+    assert(not sys_fb.ispassive())
+
+
+def test_passivity_indices_dtime():
+    sys = tf([1, 1, 5, 0.1], [1, 2, 3, 4])
+    sys = sample_system(sys, Ts=0.1)
+    iff_index = passivity.get_input_ff_index(sys)
+    ofb_index = passivity.get_output_fb_index(sys)
+
+    assert(isinstance(iff_index, float))
+
+    sys_ff_nu = parallel(sys, -iff_index)
+    sys_fb_rho = feedback(sys, ofb_index-1e-6,  sign=1)
+
+    assert(sys_ff_nu.ispassive())
+    assert(sys_fb_rho.ispassive())
+
+    sys_ff_nu = parallel(sys, -iff_index-1e-6)
+    sys_fb_rho = feedback(sys, ofb_index, sign=1)
+
+    assert(not sys_ff_nu.ispassive())
+    assert(not sys_fb_rho.ispassive())
+
+
+def test_system_dimension():
+    A = numpy.array([[0, 1], [-2, -2]])
+    B = numpy.array([[0], [1]])
+    C = numpy.array([[-1, 2], [0, 1]])
+    D = numpy.array([[1.5], [1]])
+    sys = ss(A, B, C, D)
+
+    with pytest.raises(ControlDimension):
+        passivity.ispassive(sys)
+
+
 A_d = numpy.array([[-2, 0], [0, 0]])
 A = numpy.array([[-3, 0], [0, -2]])
 B = numpy.array([[0], [1]])
@@ -41,11 +104,8 @@ def test_ispassive():
      ((A_d, B, C, D), True),
      ((A*1e12, B, C, D*0), True),
      ((A, B*0, C*0, D), True),
-     ((A*0, B, C, D), True),
-     ((A*0, B*0, C*0, D*0), True)])
+     ((A*0, B, C, D), True)])
 def test_ispassive_edge_cases(test_input, expected):
-
-    # strictly proper
     A = test_input[0]
     B = test_input[1]
     C = test_input[2]
@@ -54,6 +114,26 @@ def test_ispassive_edge_cases(test_input, expected):
     assert(passivity.ispassive(sys) == expected)
 
 
+def test_ispassive_all_zeros():
+    A = numpy.array([[0]])
+    B = numpy.array([[0]])
+    C = numpy.array([[0]])
+    D = numpy.array([[0]])
+    sys = ss(A, B, C, D)
+
+    with pytest.raises(ValueError):
+        passivity.ispassive(sys)
+
+def test_rho_and_nu_are_none():
+    A = numpy.array([[0]])
+    B = numpy.array([[0]])
+    C = numpy.array([[0]])
+    D = numpy.array([[0]])
+    sys = ss(A, B, C, D)
+
+    with pytest.raises(ControlArgument):
+        passivity._solve_passivity_LMI(sys)
+
 def test_transfer_function():
     sys = tf([1], [1, 2])
     assert(passivity.ispassive(sys))

diff --git a/doc/control.rst b/doc/control.rst
@@ -88,6 +88,7 @@ Control system analysis
     pzmap
     root_locus
     sisotool
+    passivity
 
 Matrix computations
 ===================