add param processing to create_statefbk_iosystem

Author: murrayrm

control/statefbk.py

@@ -586,7 +586,8 @@ def create_statefbk_iosystem(
         controller_type=None, xd_labels=None, ud_labels=None, ref_labels=None,
         feedfwd_pattern='trajgen', gainsched_indices=None,
         gainsched_method='linear', control_indices=None, state_indices=None,
-        name=None, inputs=None, outputs=None, states=None, **kwargs):
+        name=None, inputs=None, outputs=None, states=None, params=None,
+        **kwargs):
     r"""Create an I/O system using a (full) state feedback controller.
 
     This function creates an input/output system that implements a
@@ -751,6 +752,10 @@ def create_statefbk_iosystem(
         System name. If unspecified, a generic name <sys[id]> is generated
         with a unique integer id.
 
+    params : dict, optional
+        System parameter values.  By default, these will be copied from
+        `sys` and `ctrl`, but can be overriden with this keyword.
+
     Examples
     --------
     >>> import control as ct
@@ -773,7 +778,8 @@ def create_statefbk_iosystem(
     if not isinstance(sys, NonlinearIOSystem):
         raise ControlArgument("Input system must be I/O system")
 
-    # Process (legacy) keywords
+    # Process keywords
+    params = sys.params if params is None else params
     controller_type = _process_legacy_keyword(
         kwargs, 'type', 'controller_type', controller_type)
     if kwargs:
@@ -970,10 +976,10 @@ def _control_output(t, states, inputs, params):
 
             return u
 
-        params = {} if gainsched else {'K': K}
+        ctrl_params = {} if gainsched else {'K': K}
         ctrl = NonlinearIOSystem(
             _control_update, _control_output, name=name, inputs=inputs,
-            outputs=outputs, states=states, params=params)
+            outputs=outputs, states=states, params=ctrl_params)
 
     elif controller_type == 'iosystem' and feedfwd_pattern == 'trajgen':
         # Use the passed system to compute feedback compensation
@@ -1061,7 +1067,8 @@ def _control_output(t, states, inputs, params):
         [sys, ctrl] if estimator == sys else [sys, ctrl, estimator],
         name=sys.name + "_" + ctrl.name, add_unused=True,
         inplist=inplist, inputs=input_labels,
-        outlist=outlist, outputs=output_labels
+        outlist=outlist, outputs=output_labels,
+        params= ctrl.params | params
     )
     return ctrl, closed
 

control/tests/statefbk_test.py

@@ -930,9 +930,10 @@ def unicycle_update(t, x, u, params):
 
     return ct.NonlinearIOSystem(
         unicycle_update, None,
-        inputs = ['v', 'phi'],
-        outputs = ['x', 'y', 'theta'],
-        states = ['x_', 'y_', 'theta_'])
+        inputs=['v', 'phi'],
+        outputs=['x', 'y', 'theta'],
+        states=['x_', 'y_', 'theta_'],
+        params={'a': 1})        # only used for testing params
 
 from math import pi
 
@@ -1194,3 +1195,40 @@ def test_create_statefbk_errors():
 
     with pytest.raises(ControlArgument, match="feedfwd_pattern != 'refgain'"):
         ct.create_statefbk_iosystem(sys, K, Kf, feedfwd_pattern='trajgen')
+
+
+def test_create_statefbk_params(unicycle):
+    # Speeds and angles at which to compute the gains
+    speeds = [1, 5, 10]
+    angles = np.linspace(0, pi/2, 4)
+    points = list(itertools.product(speeds, angles))
+
+    # Gains for each speed (using LQR controller)
+    Q = np.identity(unicycle.nstates)
+    R = np.identity(unicycle.ninputs)
+    gain, _, _ = ct.lqr(unicycle.linearize([0, 0, 0], [5, 0]), Q, R)
+
+    #
+    # Schedule on desired speed and angle
+    #
+
+    # Create a linear controller
+    ctrl, clsys = ct.create_statefbk_iosystem(unicycle, gain)
+    assert [k for k in ctrl.params.keys()] == []
+    assert [k for k in clsys.params.keys()] == ['a']
+    assert clsys.params['a'] == 1
+
+    # Create a nonlinear controller
+    ctrl, clsys = ct.create_statefbk_iosystem(
+        unicycle, gain, controller_type='nonlinear')
+    assert [k for k in ctrl.params.keys()] == ['K']
+    assert [k for k in clsys.params.keys()] == ['K', 'a']
+    assert clsys.params['a'] == 1
+
+    # Override the default parameters
+    ctrl, clsys = ct.create_statefbk_iosystem(
+        unicycle, gain, controller_type='nonlinear', params={'a': 2, 'b': 1})
+    assert [k for k in ctrl.params.keys()] == ['K']
+    assert [k for k in clsys.params.keys()] == ['K', 'a', 'b']
+    assert clsys.params['a'] == 2
+    assert clsys.params['b'] == 1

examples/steering-gainsched.py

@@ -46,10 +46,10 @@ def vehicle_output(t, x, u, params):
     return x                            # return x, y, theta (full state)
 
 # Define the vehicle steering dynamics as an input/output system
-vehicle = ct.NonlinearIOSystem(
+vehicle = ct.nlsys(
     vehicle_update, vehicle_output, states=3, name='vehicle',
-    inputs=('v', 'phi'),
-    outputs=('x', 'y', 'theta'))
+    inputs=('v', 'phi'), outputs=('x', 'y', 'theta'),
+    params={'wheelbase': 3, 'maxsteer': 0.5})
 
 #
 # Gain scheduled controller
@@ -89,10 +89,12 @@ def control_output(t, x, u, params):
     return  np.array([v, phi])
 
 # Define the controller as an input/output system
-controller = ct.NonlinearIOSystem(
+controller = ct.nlsys(
     None, control_output, name='controller',        # static system
     inputs=('ex', 'ey', 'etheta', 'vd', 'phid'),    # system inputs
-    outputs=('v', 'phi')                            # system outputs
+    outputs=('v', 'phi'),                           # system outputs
+    params={'longpole': -2, 'latpole1': -1/2 + sqrt(-7)/2,
+            'latpole2': -1/2 - sqrt(-7)/2, 'wheelbase': 3}
 )
 
 #
@@ -113,7 +115,7 @@ def trajgen_output(t, x, u, params):
     return np.array([vref * t, yref, 0, vref, 0])
 
 # Define the trajectory generator as an input/output system
-trajgen = ct.NonlinearIOSystem(
+trajgen = ct.nlsys(
     None, trajgen_output, name='trajgen',
     inputs=('vref', 'yref'),
     outputs=('xd', 'yd', 'thetad', 'vd', 'phid'))
@@ -156,10 +158,13 @@ def trajgen_output(t, x, u, params):
     inplist=['trajgen.vref', 'trajgen.yref'],
     inputs=['yref', 'vref'],
 
-    #  System outputs
+    # System outputs
     outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
              'controller.phi'],
-    outputs=['x', 'y', 'theta', 'v', 'phi']
+    outputs=['x', 'y', 'theta', 'v', 'phi'],
+
+    # Parameters
+    params=trajgen.params | vehicle.params | controller.params,
 )
 
 # Set up the simulation conditions
@@ -220,7 +225,8 @@ def trajgen_output(t, x, u, params):
 # Create the gain scheduled system
 controller, _ = ct.create_statefbk_iosystem(
     vehicle, (gains, points), name='controller', ud_labels=['vd', 'phid'],
-    gainsched_indices=['vd', 'theta'], gainsched_method='linear')
+    gainsched_indices=['vd', 'theta'], gainsched_method='linear',
+    params=vehicle.params | controller.params)
 
 # Connect everything together (note that controller inputs are different)
 steering = ct.interconnect(
@@ -245,10 +251,13 @@ def trajgen_output(t, x, u, params):
     inplist=['trajgen.vref', 'trajgen.yref'],
     inputs=['yref', 'vref'],
 
-    #  System outputs
+    # System outputs
     outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
              'controller.phi'],
-    outputs=['x', 'y', 'theta', 'v', 'phi']
+    outputs=['x', 'y', 'theta', 'v', 'phi'],
+
+    # Parameters
+    params=steering.params
 )
 
 # Plot the results to compare to the previous case