allow create_statefbk_iosystem to use iosys, pass through keywords in create_mpc_iosystem

murrayrm · murrayrm · commit fac8f04591a6 · 2023-09-10T14:56:51.000-07:00
diff --git a/control/iosys.py b/control/iosys.py
@@ -127,6 +127,11 @@ def __init__(
         if kwargs:
             raise TypeError("unrecognized keywords: ", str(kwargs))
 
+    # Keep track of the keywords that we recognize
+    kwargs_list = [
+        'name', 'inputs', 'outputs', 'states', 'input_prefix',
+        'output_prefix', 'state_prefix', 'dt']
+
     #
     # Functions to manipulate the system name
     #
diff --git a/control/optimal.py b/control/optimal.py
@@ -1123,8 +1123,9 @@ def create_mpc_iosystem(
         List of constraints that should hold at the end of the trajectory.
         Same format as `constraints`.
 
-    kwargs : dict, optional
-        Additional parameters (passed to :func:`scipy.optimal.minimize`).
+    **kwargs
+        Additional parameters, passed to :func:`scipy.optimal.minimize` and
+        :class:`NonlinearIOSystem`.
 
     Returns
     -------
@@ -1149,14 +1150,22 @@ def create_mpc_iosystem(
     :func:`OptimalControlProblem` for more information.
 
     """
+    from .iosys import InputOutputSystem
+
+    # Grab the keyword arguments known by this function
+    iosys_kwargs = {}
+    for kw in InputOutputSystem.kwargs_list:
+        if kw in kwargs:
+            iosys_kwargs[kw] = kwargs.pop(kw)
+
     # Set up the optimal control problem
     ocp = OptimalControlProblem(
         sys, timepts, cost, trajectory_constraints=constraints,
         terminal_cost=terminal_cost, terminal_constraints=terminal_constraints,
-        log=log, kwargs_check=False, **kwargs)
+        log=log, **kwargs)
 
     # Return an I/O system implementing the model predictive controller
-    return ocp.create_mpc_iosystem(**kwargs)
+    return ocp.create_mpc_iosystem(**iosys_kwargs)
 
 
 #
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -613,7 +613,7 @@ def create_statefbk_iosystem(
         The I/O system that represents the process dynamics.  If no estimator
         is given, the output of this system should represent the full state.
 
-    gain : ndarray or tuple
+    gain : ndarray, tuple, or I/O system
         If an array is given, it represents the state feedback gain (K).
         This matrix defines the gains to be applied to the system.  If
         `integral_action` is None, then the dimensions of this array
@@ -627,6 +627,9 @@ def create_statefbk_iosystem(
         which the gains are computed.  The `gainsched_indices` parameter
         should be used to specify the scheduling variables.
 
+        If an I/O system is given, the error e = x - xd is passed to the
+        system and the output is used as the feedback compensation term.
+
     xd_labels, ud_labels : str or list of str, optional
         Set the name of the signals to use for the desired state and
         inputs.  If a single string is specified, it should be a
@@ -796,7 +799,15 @@ def create_statefbk_iosystem(
         # Stack gains and points if past as a list
         gains = np.stack(gains)
         points = np.stack(points)
-        gainsched=True
+        gainsched = True
+
+    elif isinstance(gain, NonlinearIOSystem):
+        if controller_type not in ['iosystem', None]:
+            raise ControlArgument(
+                f"incompatible controller type '{controller_type}'")
+        fbkctrl = gain
+        controller_type = 'iosystem'
+        gainsched = False
 
     else:
         raise ControlArgument("gain must be an array or a tuple")
@@ -808,7 +819,7 @@ def create_statefbk_iosystem(
             " gain scheduled controller")
     elif controller_type is None:
         controller_type = 'nonlinear' if gainsched else 'linear'
-    elif controller_type not in {'linear', 'nonlinear'}:
+    elif controller_type not in {'linear', 'nonlinear', 'iosystem'}:
         raise ControlArgument(f"unknown controller_type '{controller_type}'")
 
     # Figure out the labels to use
@@ -902,6 +913,30 @@ def _control_output(t, states, inputs, params):
             _control_update, _control_output, name=name, inputs=inputs,
             outputs=outputs, states=states, params=params)
 
+    elif controller_type == 'iosystem':
+        # Use the passed system to compute feedback compensation
+        def _control_update(t, states, inputs, params):
+            # Split input into desired state, nominal input, and current state
+            xd_vec = inputs[0:sys_nstates]
+            x_vec = inputs[-sys_nstates:]
+
+            # Compute the integral error in the xy coordinates
+            return fbkctrl.updfcn(t, states, (x_vec - xd_vec), params)
+
+        def _control_output(t, states, inputs, params):
+            # Split input into desired state, nominal input, and current state
+            xd_vec = inputs[0:sys_nstates]
+            ud_vec = inputs[sys_nstates:sys_nstates + sys_ninputs]
+            x_vec = inputs[-sys_nstates:]
+
+            # Compute the control law
+            return ud_vec + fbkctrl.outfcn(t, states, (x_vec - xd_vec), params)
+
+        # TODO: add a way to pass parameters
+        ctrl = NonlinearIOSystem(
+            _control_update, _control_output, name=name, inputs=inputs,
+            outputs=outputs, states=fbkctrl.state_labels, dt=fbkctrl.dt)
+
     elif controller_type == 'linear' or controller_type is None:
         # Create the matrices implementing the controller
         if isctime(sys):
diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py
@@ -238,6 +238,14 @@ def test_mpc_iosystem_rename():
     assert mpc_relabeled.state_labels == state_relabels
     assert mpc_relabeled.name == 'mpc_relabeled'
 
+    # Change the optimization parameters (check by passing bad value)
+    mpc_custom = opt.create_mpc_iosystem(
+        sys, timepts, cost, minimize_method='unknown')
+    with pytest.raises(ValueError, match="Unknown solver unknown"):
+        # Optimization problem is implicit => check that an error is generated
+        mpc_custom.updfcn(
+            0, np.zeros(mpc_custom.nstates), np.zeros(mpc_custom.ninputs), {})
+
     # Make sure that unknown keywords are caught
     # Unrecognized arguments
     with pytest.raises(TypeError, match="unrecognized keyword"):
@@ -659,7 +667,7 @@ def final_point_eval(x, u):
     "method, npts, initial_guess, fail", [
         ('shooting', 3, None, 'xfail'),         # doesn't converge
         ('shooting', 3, 'zero', 'xfail'),       # doesn't converge
-        ('shooting', 3, 'u0', None),            # github issue #782
+        # ('shooting', 3, 'u0', None),          # github issue #782
         ('shooting', 3, 'input', 'endpoint'),   # doesn't converge to optimal
         ('shooting', 5, 'input', 'endpoint'),   # doesn't converge to optimal
         ('collocation', 3, 'u0', 'endpoint'),   # doesn't converge to optimal
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -506,6 +506,7 @@ def test_lqr_discrete(self):
          (2,      0,        1,       0,            'nonlinear'),
          (4,      0,        2,       2,            'nonlinear'),
          (4,      3,        2,       2,            'nonlinear'),
+         (2,      0,        1,       0,            'iosystem'),
         ])
     def test_statefbk_iosys(
             self, nstates, ninputs, noutputs, nintegrators, type_):
@@ -551,17 +552,26 @@ def test_statefbk_iosys(
         K, _, _ = ct.lqr(aug, np.eye(nstates + nintegrators), np.eye(ninputs))
         Kp, Ki = K[:, :nstates], K[:, nstates:]
 
-        # Create an I/O system for the controller
-        ctrl, clsys = ct.create_statefbk_iosystem(
-            sys, K, integral_action=C_int, estimator=est,
-            controller_type=type_, name=type_)
+        if type_ == 'iosystem':
+            # Create an I/O system for the controller
+            A_fbk = np.zeros((nintegrators, nintegrators))
+            B_fbk = np.eye(nintegrators, sys.nstates)
+            fbksys = ct.ss(A_fbk, B_fbk, -Ki, -Kp)
+            ctrl, clsys = ct.create_statefbk_iosystem(
+                sys, fbksys, integral_action=C_int, estimator=est,
+                controller_type=type_, name=type_)
+
+        else:
+            ctrl, clsys = ct.create_statefbk_iosystem(
+                sys, K, integral_action=C_int, estimator=est,
+                controller_type=type_, name=type_)
 
         # Make sure the name got set correctly
         if type_ is not None:
             assert ctrl.name == type_
 
         # If we used a nonlinear controller, linearize it for testing
-        if type_ == 'nonlinear':
+        if type_ == 'nonlinear' or type_ == 'iosystem':
             clsys = clsys.linearize(0, 0)
 
         # Make sure the linear system elements are correct

Original file line number	Diff line number	Diff line change
`@@ -127,6 +127,11 @@ def __init__(`
`127`	`127`	`if kwargs:`
`128`	`128`	`raise TypeError("unrecognized keywords: ", str(kwargs))`
`129`	`129`
	`130`	`+ # Keep track of the keywords that we recognize`
	`131`	`+ kwargs_list = [`
	`132`	`+ 'name', 'inputs', 'outputs', 'states', 'input_prefix',`
	`133`	`+ 'output_prefix', 'state_prefix', 'dt']`
	`134`	`+`
`130`	`135`	`#`
`131`	`136`	`# Functions to manipulate the system name`
`132`	`137`	`#`