From 8780bdce261582d7b8b11b9b203a99fc226d8527 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 29 Nov 2024 08:38:22 -0800 Subject: [PATCH 1/5] add feedfwd keyword arguments (no functionality yet) --- control/statefbk.py | 89 +++++++++++++++++++++++++++++++-------------- 1 file changed, 62 insertions(+), 27 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 16eeb36ee..a38fb294a 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -39,24 +39,25 @@ # # $Id$ -# External packages and modules +import warnings + import numpy as np import scipy as sp -import warnings from . import statesp -from .mateqn import care, dare, _check_shape -from .statesp import StateSpace, _ssmatrix, _convert_to_statespace, ss +from .config import _process_legacy_keyword +from .exception import ControlArgument, ControlDimension, \ + ControlNotImplemented, ControlSlycot +from .iosys import _process_indices, _process_labels, isctime, isdtime from .lti import LTI -from .iosys import isdtime, isctime, _process_indices, _process_labels +from .mateqn import _check_shape, care, dare from .nlsys import NonlinearIOSystem, interconnect -from .exception import ControlSlycot, ControlArgument, ControlDimension, \ - ControlNotImplemented -from .config import _process_legacy_keyword +from .statesp import StateSpace, _convert_to_statespace, _ssmatrix, ss # Make sure we have access to the right slycot routines try: from slycot import sb03md57 + # wrap without the deprecation warning def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): ret = sb03md57(A, U, C, dico, job, fact, trana, ldwork) @@ -581,8 +582,9 @@ def dlqr(*args, **kwargs): # Function to create an I/O sytems representing a state feedback controller def create_statefbk_iosystem( - sys, gain, integral_action=None, estimator=None, controller_type=None, - xd_labels=None, ud_labels=None, gainsched_indices=None, + sys, gain, feedfwd_gain=None, integral_action=None, estimator=None, + controller_type=None, xd_labels=None, ud_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): r"""Create an I/O system using a (full) state feedback controller. @@ -592,7 +594,7 @@ def create_statefbk_iosystem( .. math:: u = u_d - K_p (x - x_d) - K_i \int(C x - C x_d) - It can be called in the form:: + by calling ctrl, clsys = ct.create_statefbk_iosystem(sys, K) @@ -608,6 +610,18 @@ def create_statefbk_iosystem( where :math:`\mu` represents the scheduling variable. + Alternatively, a control of the form + + .. math:: u = k_f r - K_p x - K_i \int(C x - r) + + can be created by calling + + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, kf, feedfwd_pattern='refgain') + + In either form, an estimator can also be used to compute the estimated + state from the input and output measurements. + Parameters ---------- sys : NonlinearIOSystem @@ -640,6 +654,15 @@ def create_statefbk_iosystem( ud_labels. These settings can also be overridden using the `inputs` keyword. + feedfwd_pattern : str, optional + If set to 'refgain', the reference gain design pattern is used to + create the controller instead of the trajectory generation pattern. + + feedfwd_gain : array_like, optional + Specify the feedforward gain, `k_f`. Used only for the reference + gain design pattern. If not given and if `sys` is a `StateSpace` + (linear) system, will be computed as -1/(C (A-BK)^{-1}) B. + integral_action : ndarray, optional If this keyword is specified, the controller can include integral action in addition to state feedback. The value of the @@ -841,20 +864,26 @@ def create_statefbk_iosystem( raise ControlArgument(f"unknown controller_type '{controller_type}'") # Figure out the labels to use - xd_labels = _process_labels( - xd_labels, 'xd', ['xd[{i}]'.format(i=i) for i in range(sys_nstates)]) - ud_labels = _process_labels( - ud_labels, 'ud', ['ud[{i}]'.format(i=i) for i in range(sys_ninputs)]) - - # Create the signal and system names - if inputs is None: - inputs = xd_labels + ud_labels + estimator.output_labels + if feedfwd_pattern == 'trajgen': + xd_labels = _process_labels(xd_labels, 'xd', [ + 'xd[{i}]'.format(i=i) for i in range(sys_nstates)]) + ud_labels = _process_labels(ud_labels, 'ud', [ + 'ud[{i}]'.format(i=i) for i in range(sys_ninputs)]) + + # Create the signal and system names + if inputs is None: + inputs = xd_labels + ud_labels + estimator.output_labels + elif feedfwd_pattern == 'refgain': + raise NotImplementedError("reference gain pattern not yet implemented") + else: + raise NotImplementedError(f"unknown pattern '{feedfwd_pattern}'") + if outputs is None: outputs = [sys.input_labels[i] for i in control_indices] if states is None: states = nintegrators - # Process gainscheduling variables, if present + # Process gain scheduling variables, if present if gainsched: # Create a copy of the scheduling variable indices (default = xd) gainsched_indices = _process_indices( @@ -897,7 +926,7 @@ def _compute_gain(mu): return K # Define the controller system - if controller_type == 'nonlinear': + if controller_type == 'nonlinear' and feedfwd_pattern == 'trajgen': # Create an I/O system for the state feedback gains def _control_update(t, states, inputs, params): # Split input into desired state, nominal input, and current state @@ -931,7 +960,7 @@ 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': + elif controller_type == 'iosystem' and feedfwd_pattern == 'trajgen': # 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 @@ -955,7 +984,7 @@ def _control_output(t, states, inputs, params): _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: + elif controller_type in 'linear' and feedfwd_pattern == 'trajgen': # Create the matrices implementing the controller if isctime(sys): # Continuous time: integrator @@ -973,6 +1002,12 @@ def _control_output(t, states, inputs, params): A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name, inputs=inputs, outputs=outputs, states=states) + elif feedfwd_pattern == 'refgain': + if controller_type not in ['linear', 'iosystem']: + raise ControlArgument( + "refgain design pattern only supports linear controllers") + raise NotImplementedError("reference gain pattern not yet implemented") + else: raise ControlArgument(f"unknown controller_type '{controller_type}'") @@ -1020,7 +1055,7 @@ def ctrb(A, B, t=None): bmat = _ssmatrix(B) n = np.shape(amat)[0] m = np.shape(bmat)[1] - + if t is None or t > n: t = n @@ -1042,7 +1077,7 @@ def obsv(A, C, t=None): Dynamics and output matrix of the system t : None or integer maximum time horizon of the controllability matrix, max = A.shape[0] - + Returns ------- O : 2D array (or matrix) @@ -1062,14 +1097,14 @@ def obsv(A, C, t=None): cmat = _ssmatrix(C) n = np.shape(amat)[0] p = np.shape(cmat)[0] - + if t is None or t > n: t = n # Construct the observability matrix obsv = np.zeros((t * p, n)) obsv[:p, :] = cmat - + for k in range(1, t): obsv[k * p:(k + 1) * p, :] = np.dot(obsv[(k - 1) * p:k * p, :], amat) From 391d29c159655d31f22550cfef98f1dd9b74ae00 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 29 Nov 2024 10:37:31 -0800 Subject: [PATCH 2/5] update StateSpace size checks/error messages to be more informative --- control/statesp.py | 27 +++++++++++++--------- control/tests/statesp_test.py | 43 ++++++++++++++++++----------------- 2 files changed, 38 insertions(+), 32 deletions(-) diff --git a/control/statesp.py b/control/statesp.py index bfe5f996b..7af9008f4 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -252,6 +252,12 @@ def __init__(self, *args, **kwargs): D = np.zeros((C.shape[0], B.shape[1])) D = _ssmatrix(D) + # If only direct term is present, adjust sizes of C and D if needed + if D.size > 0 and B.size == 0: + B = np.zeros((0, D.shape[1])) + if D.size > 0 and C.size == 0: + C = np.zeros((D.shape[0], 0)) + # Matrices defining the linear system self.A = A self.B = B @@ -268,7 +274,7 @@ def __init__(self, *args, **kwargs): # Process iosys keywords defaults = args[0] if len(args) == 1 else \ - {'inputs': D.shape[1], 'outputs': D.shape[0], + {'inputs': B.shape[1], 'outputs': C.shape[0], 'states': A.shape[0]} name, inputs, outputs, states, dt = _process_iosys_keywords( kwargs, defaults, static=(A.size == 0)) @@ -295,16 +301,15 @@ def __init__(self, *args, **kwargs): # Check to make sure everything is consistent # # Check that the matrix sizes are consistent - if A.shape[0] != A.shape[1] or self.nstates != A.shape[0]: - raise ValueError("A must be square.") - if self.nstates != B.shape[0]: - raise ValueError("A and B must have the same number of rows.") - if self.nstates != C.shape[1]: - raise ValueError("A and C must have the same number of columns.") - if self.ninputs != B.shape[1] or self.ninputs != D.shape[1]: - raise ValueError("B and D must have the same number of columns.") - if self.noutputs != C.shape[0] or self.noutputs != D.shape[0]: - raise ValueError("C and D must have the same number of rows.") + def _check_shape(matrix, expected, name): + if matrix.shape != expected: + raise ValueError( + f"{name} is the wrong shape; " + f"expected {expected} instead of {matrix.shape}") + _check_shape(A, (self.nstates, self.nstates), "A") + _check_shape(B, (self.nstates, self.ninputs), "B") + _check_shape(C, (self.noutputs, self.nstates), "C") + _check_shape(D, (self.noutputs, self.ninputs), "D") # # Final processing diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index cb200c4ab..549c0612a 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -121,29 +121,30 @@ def test_constructor(self, sys322ABCD, dt, argfun): np.testing.assert_almost_equal(sys.D, sys322ABCD[3]) assert sys.dt == dtref - @pytest.mark.parametrize("args, exc, errmsg", - [((True, ), TypeError, - "(can only take in|sys must be) a StateSpace"), - ((1, 2), TypeError, "1, 4, or 5 arguments"), - ((np.ones((3, 2)), np.ones((3, 2)), - np.ones((2, 2)), np.ones((2, 2))), - ValueError, "A must be square"), - ((np.ones((3, 3)), np.ones((2, 2)), - np.ones((2, 3)), np.ones((2, 2))), - ValueError, "A and B"), - ((np.ones((3, 3)), np.ones((3, 2)), - np.ones((2, 2)), np.ones((2, 2))), - ValueError, "A and C"), - ((np.ones((3, 3)), np.ones((3, 2)), - np.ones((2, 3)), np.ones((2, 3))), - ValueError, "B and D"), - ((np.ones((3, 3)), np.ones((3, 2)), - np.ones((2, 3)), np.ones((3, 2))), - ValueError, "C and D"), - ]) + @pytest.mark.parametrize( + "args, exc, errmsg", + [((True, ), TypeError, "(can only take in|sys must be) a StateSpace"), + ((1, 2), TypeError, "1, 4, or 5 arguments"), + ((np.ones((3, 2)), np.ones((3, 2)), + np.ones((2, 2)), np.ones((2, 2))), ValueError, + "A is the wrong shape; expected \(3, 3\)"), + ((np.ones((3, 3)), np.ones((2, 2)), + np.ones((2, 3)), np.ones((2, 2))), ValueError, + "B is the wrong shape; expected \(3, 2\)"), + ((np.ones((3, 3)), np.ones((3, 2)), + np.ones((2, 2)), np.ones((2, 2))), ValueError, + "C is the wrong shape; expected \(2, 3\)"), + ((np.ones((3, 3)), np.ones((3, 2)), + np.ones((2, 3)), np.ones((2, 3))), ValueError, + "D is the wrong shape; expected \(2, 2\)"), + ((np.ones((3, 3)), np.ones((3, 2)), + np.ones((2, 3)), np.ones((3, 2))), ValueError, + "D is the wrong shape; expected \(2, 2\)"), + ]) def test_constructor_invalid(self, args, exc, errmsg): """Test invalid input to StateSpace() constructor""" - with pytest.raises(exc, match=errmsg): + + with pytest.raises(exc, match=errmsg) as w: StateSpace(*args) with pytest.raises(exc, match=errmsg): ss(*args) From 9ed48173b1b48e77bf9fdeeb9d667e576ada7919 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 29 Nov 2024 17:04:10 -0800 Subject: [PATCH 3/5] initial implementation, docstrings for refgain pattern --- control/statefbk.py | 90 +++++++++++++++++++++++----------- control/tests/statefbk_test.py | 49 +++++++++++++++--- 2 files changed, 104 insertions(+), 35 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index a38fb294a..de23f6d0d 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -583,7 +583,7 @@ def dlqr(*args, **kwargs): # Function to create an I/O sytems representing a state feedback controller def create_statefbk_iosystem( sys, gain, feedfwd_gain=None, integral_action=None, estimator=None, - controller_type=None, xd_labels=None, ud_labels=None, + 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): @@ -645,24 +645,15 @@ def create_statefbk_iosystem( 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 format - string using the variable `i` as an index. Otherwise, a list of - strings matching the size of `x_d` and `u_d`, respectively, should - be used. Default is "xd[{i}]" for xd_labels and "ud[{i}]" for - ud_labels. These settings can also be overridden using the - `inputs` keyword. - - feedfwd_pattern : str, optional - If set to 'refgain', the reference gain design pattern is used to - create the controller instead of the trajectory generation pattern. - feedfwd_gain : array_like, optional Specify the feedforward gain, `k_f`. Used only for the reference gain design pattern. If not given and if `sys` is a `StateSpace` (linear) system, will be computed as -1/(C (A-BK)^{-1}) B. + feedfwd_pattern : str, optional + If set to 'refgain', the reference gain design pattern is used to + create the controller instead of the trajectory generation pattern. + integral_action : ndarray, optional If this keyword is specified, the controller can include integral action in addition to state feedback. The value of the @@ -703,17 +694,19 @@ def create_statefbk_iosystem( Returns ------- ctrl : NonlinearIOSystem - Input/output system representing the controller. This system - takes as inputs the desired state `x_d`, the desired input - `u_d`, and either the system state `x` or the estimated state - `xhat`. It outputs the controller action `u` according to the - formula `u = u_d - K(x - x_d)`. If the keyword - `integral_action` is specified, then an additional set of - integrators is included in the control system (with the gain - matrix `K` having the integral gains appended after the state - gains). If a gain scheduled controller is specified, the gain - (proportional and integral) are evaluated using the scheduling - variables specified by `gainsched_indices`. + Input/output system representing the controller. For the 'trajgen' + design patter (default), this system takes as inputs the desired + state `x_d`, the desired input `u_d`, and either the system state + `x` or the estimated state `xhat`. It outputs the controller + action `u` according to the formula `u = u_d - K(x - x_d)`. For + the 'refgain' design patter, the system takes as inputs the + reference input `r` and the system or estimated state. If the + keyword `integral_action` is specified, then an additional set of + integrators is included in the control system (with the gain matrix + `K` having the integral gains appended after the state gains). If + a gain scheduled controller is specified, the gain (proportional + and integral) are evaluated using the scheduling variables + specified by `gainsched_indices`. clsys : NonlinearIOSystem Input/output system representing the closed loop system. This @@ -739,6 +732,15 @@ def create_statefbk_iosystem( specified as either integer offsets or as estimator/system output signal names. If not specified, defaults to the system states. + xd_labels, ud_labels, ref_labels : str or list of str, optional + Set the name of the signals to use for the desired state and inputs + or the reference inputs (for the 'refgain' design pattern). If a + single string is specified, it should be a format string using the + variable `i` as an index. Otherwise, a list of strings matching + the size of `x_d` and `u_d`, respectively, should be used. Default + is "xd[{i}]" for xd_labels and "ud[{i}]" for ud_labels. These + settings can also be overridden using the `inputs` keyword. + inputs, outputs, states : str, or list of str, optional List of strings that name the individual signals of the transformed system. If not given, the inputs, outputs, and states are the same @@ -835,6 +837,10 @@ def create_statefbk_iosystem( # Check for gain scheduled controller if len(gain) != 2: raise ControlArgument("gain must be a 2-tuple for gain scheduling") + elif feedfwd_pattern != 'trajgen': + raise NotImplementedError( + "Gain scheduling is not implemented for pattern " + f"'{feedfwd_pattern}'") gains, points = gain[0:2] # Stack gains and points if past as a list @@ -842,7 +848,7 @@ def create_statefbk_iosystem( points = np.stack(points) gainsched = True - elif isinstance(gain, NonlinearIOSystem): + elif isinstance(gain, NonlinearIOSystem) and feedfwd_pattern != 'refgain': if controller_type not in ['iosystem', None]: raise ControlArgument( f"incompatible controller type '{controller_type}'") @@ -874,7 +880,10 @@ def create_statefbk_iosystem( if inputs is None: inputs = xd_labels + ud_labels + estimator.output_labels elif feedfwd_pattern == 'refgain': - raise NotImplementedError("reference gain pattern not yet implemented") + ref_labels = _process_labels(ref_labels, 'r', [ + f'r[{i}]' for i in range(sys_ninputs)]) + if inputs is None: + inputs = ref_labels + estimator.output_labels else: raise NotImplementedError(f"unknown pattern '{feedfwd_pattern}'") @@ -1006,7 +1015,32 @@ def _control_output(t, states, inputs, params): if controller_type not in ['linear', 'iosystem']: raise ControlArgument( "refgain design pattern only supports linear controllers") - raise NotImplementedError("reference gain pattern not yet implemented") + + if feedfwd_gain is None: + raise ControlArgument( + "'feedfwd_gain' required for reference gain pattern") + + # Check to make sure the reference gain is valid + Kf = np.atleast_2d(feedfwd_gain) + if Kf.ndim != 2 or Kf.shape[0] != sys.ninputs or \ + Kf.shape[1] != sys.ninputs: + raise ControlArgument("feedfwd_gain is not the right shape") + + # Create the matrices implementing the controller + # [r, x]->[u]: u = k_f r - K_p x - K_i \int(C x - r) + if isctime(sys): + # Continuous time: integrator + A_lqr = np.zeros((C.shape[0], C.shape[0])) + else: + # Discrete time: summer + A_lqr = np.eye(C.shape[0]) + B_lqr = np.hstack([-np.eye(C.shape[0], sys_ninputs), C]) + C_lqr = -K[:, sys_nstates:] # integral gain (opt) + D_lqr = np.hstack([Kf, -K]) + + ctrl = ss( + A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name, + inputs=inputs, outputs=outputs, states=states) else: raise ControlArgument(f"unknown controller_type '{controller_type}'") diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 2d96ad225..651e19b43 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -56,7 +56,7 @@ def testCtrbT(self): Wctrue = np.array([[5., 6.], [7., 8.]]) Wc = ctrb(A, B, t=t) np.testing.assert_array_almost_equal(Wc, Wctrue) - + def testObsvSISO(self): A = np.array([[1., 2.], [3., 4.]]) C = np.array([[5., 7.]]) @@ -70,7 +70,7 @@ def testObsvMIMO(self): Wotrue = np.array([[5., 6.], [7., 8.], [23., 34.], [31., 46.]]) Wo = obsv(A, C) np.testing.assert_array_almost_equal(Wo, Wotrue) - + def testObsvT(self): A = np.array([[1., 2.], [3., 4.]]) C = np.array([[5., 6.], [7., 8.]]) @@ -128,15 +128,14 @@ def testGramRc(self): C = np.array([[4., 5.], [6., 7.]]) D = np.array([[13., 14.], [15., 16.]]) sys = ss(A, B, C, D) - Rctrue = np.array([[4.30116263, 5.6961343], - [0., 0.23249528]]) + Rctrue = np.array([[4.30116263, 5.6961343], [0., 0.23249528]]) Rc = gram(sys, 'cf') np.testing.assert_array_almost_equal(Rc, Rctrue) sysd = ct.c2d(sys, 0.2) Rctrue = np.array([[1.91488054, 2.53468814], [0. , 0.10290372]]) Rc = gram(sysd, 'cf') - np.testing.assert_array_almost_equal(Rc, Rctrue) + np.testing.assert_array_almost_equal(Rc, Rctrue) @slycotonly def testGramWo(self): @@ -149,7 +148,7 @@ def testGramWo(self): Wo = gram(sys, 'o') np.testing.assert_array_almost_equal(Wo, Wotrue) sysd = ct.c2d(sys, 0.2) - Wotrue = np.array([[ 1305.369179, -440.046414], + Wotrue = np.array([[ 1305.369179, -440.046414], [ -440.046414, 333.034844]]) Wo = gram(sysd, 'o') np.testing.assert_array_almost_equal(Wo, Wotrue) @@ -184,7 +183,7 @@ def testGramRo(self): Rotrue = np.array([[ 36.12989315, -12.17956588], [ 0. , 13.59018097]]) Ro = gram(sysd, 'of') - np.testing.assert_array_almost_equal(Ro, Rotrue) + np.testing.assert_array_almost_equal(Ro, Rotrue) def testGramsys(self): sys = tf([1.], [1., 1., 1.]) @@ -1143,3 +1142,39 @@ def test_gainsched_errors(unicycle): ctrl, clsys = ct.create_statefbk_iosystem( unicycle, (gains, points), gainsched_indices=[3, 2], gainsched_method='unknown') + + +@pytest.mark.parametrize("ninputs, Kf", [ + (1, 1), (1, None), + (2, np.diag([1, 1])), (2, None), +]) +def test_refgain_pattern(ninputs, Kf): + sys = ct.rss(2, 2, ninputs, strictly_proper=True) + sys.C = np.eye(2) + + K, _, _ = ct.lqr(sys.A, sys.B, np.eye(sys.nstates), np.eye(sys.ninputs)) + if Kf is None: + # Make sure we get an error if we don't specify Kf + with pytest.raises(ControlArgument, match="'feedfwd_gain' required"): + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, Kf, feedfwd_pattern='refgain') + + # Now compute the gain to give unity zero frequency gain + C = np.eye(ninputs, sys.nstates) + Kf = -np.linalg.inv( + C @ np.linalg.inv(sys.A - sys.B @ K) @ sys.B) + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, Kf, feedfwd_pattern='refgain') + + np.testing.assert_almost_equal( + C @ clsys(0)[0:sys.nstates], np.eye(ninputs)) + + else: + ctrl, clsys = ct.create_statefbk_iosystem( + sys, K, Kf, feedfwd_pattern='refgain') + + manual = ct.feedback(sys, K) * Kf + np.testing.assert_almost_equal(clsys.A, manual.A) + np.testing.assert_almost_equal(clsys.B, manual.B) + np.testing.assert_almost_equal(clsys.C[:sys.nstates, :], manual.C) + np.testing.assert_almost_equal(clsys.D[:sys.nstates, :], manual.D) From 96b7b734ae2746f021a04b22e039284093c7033f Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 1 Dec 2024 21:59:19 -0800 Subject: [PATCH 4/5] documentation updates --- control/statefbk.py | 5 +++-- control/tests/statesp_test.py | 2 +- doc/iosys.rst | 32 ++++++++++++++++++++++++++++---- 3 files changed, 32 insertions(+), 7 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index de23f6d0d..6782fc673 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -610,7 +610,7 @@ def create_statefbk_iosystem( where :math:`\mu` represents the scheduling variable. - Alternatively, a control of the form + Alternatively, a controller of the form .. math:: u = k_f r - K_p x - K_i \int(C x - r) @@ -652,7 +652,8 @@ def create_statefbk_iosystem( feedfwd_pattern : str, optional If set to 'refgain', the reference gain design pattern is used to - create the controller instead of the trajectory generation pattern. + create the controller instead of the trajectory generation + ('trajgen') pattern. integral_action : ndarray, optional If this keyword is specified, the controller can include integral diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 549c0612a..1798c524c 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -127,7 +127,7 @@ def test_constructor(self, sys322ABCD, dt, argfun): ((1, 2), TypeError, "1, 4, or 5 arguments"), ((np.ones((3, 2)), np.ones((3, 2)), np.ones((2, 2)), np.ones((2, 2))), ValueError, - "A is the wrong shape; expected \(3, 3\)"), + "A is the wrong shape; expected \(3, 3\)"), ((np.ones((3, 3)), np.ones((2, 2)), np.ones((2, 3)), np.ones((2, 2))), ValueError, "B is the wrong shape; expected \(3, 2\)"), diff --git a/doc/iosys.rst b/doc/iosys.rst index f2dfbff4d..e67f4dc9c 100644 --- a/doc/iosys.rst +++ b/doc/iosys.rst @@ -470,6 +470,29 @@ closed loop systems `clsys`, both as I/O systems. The input to the controller is the vector of desired states :math:`x_\text{d}`, desired inputs :math:`u_\text{d}`, and system states :math:`x`. +The above design pattern is referred to as the "trajectory generation" +('trajgen') pattern, since it assumes that the input to the controller is a +feasible trajectory :math:`(x_\text{d}, u_\text{d})`. Alternatively, a +controller using the "reference gain" pattern can be created, which +implements a state feedback controller of the form + +.. math:: + + u = k_\text{f}\, r - K x, + +where :math:`r` is the reference input and :math:`k_\text{f}` is the +feedforward gain (normally chosen so that the steady state output +:math:`y_\text{ss}` will be equal to :math:`r`). + +A reference gain controller can be created with the command:: + + ctrl, clsys = ct.create_statefbk_iosystem(sys, K, kf, feedfwd_pattern='refgain') + +This reference gain design pattern is described in more detail in Section +7.2 of FBS2e (Stabilization by State Feedback) and the trajectory +generation design pattern is described in Section 8.5 (State Space +Controller Design). + If the full system state is not available, the output of a state estimator can be used to construct the controller using the command:: @@ -507,10 +530,11 @@ must match the number of additional columns in the `K` matrix. If an estimator is specified, :math:`\hat x` will be used in place of :math:`x`. -Finally, gain scheduling on the desired state, desired input, or -system state can be implemented by setting the gain to a 2-tuple -consisting of a list of gains and a list of points at which the gains -were computed, as well as a description of the scheduling variables:: +Finally, for the trajectory generation design pattern, gain scheduling on +the desired state, desired input, or system state can be implemented by +setting the gain to a 2-tuple consisting of a list of gains and a list of +points at which the gains were computed, as well as a description of the +scheduling variables:: ctrl, clsys = ct.create_statefbk_iosystem( sys, ([g1, ..., gN], [p1, ..., pN]), gainsched_indices=[s1, ..., sq]) From bb52b70a6dc88c0899d79410a6c038bacba65e07 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Fri, 6 Dec 2024 20:57:29 -0800 Subject: [PATCH 5/5] addressed @slivingston review comments --- control/statefbk.py | 9 +++++++-- control/tests/statefbk_test.py | 20 ++++++++++++++++++-- doc/iosys.rst | 13 +++++++------ 3 files changed, 32 insertions(+), 10 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 6782fc673..1dd0be325 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -696,11 +696,11 @@ def create_statefbk_iosystem( ------- ctrl : NonlinearIOSystem Input/output system representing the controller. For the 'trajgen' - design patter (default), this system takes as inputs the desired + design pattern (default), this system takes as inputs the desired state `x_d`, the desired input `u_d`, and either the system state `x` or the estimated state `xhat`. It outputs the controller action `u` according to the formula `u = u_d - K(x - x_d)`. For - the 'refgain' design patter, the system takes as inputs the + the 'refgain' design pattern, the system takes as inputs the reference input `r` and the system or estimated state. If the keyword `integral_action` is specified, then an additional set of integrators is included in the control system (with the gain matrix @@ -779,6 +779,11 @@ def create_statefbk_iosystem( if kwargs: raise TypeError("unrecognized keywords: ", str(kwargs)) + # Check for consistency of positional parameters + if feedfwd_gain is not None and feedfwd_pattern != 'refgain': + raise ControlArgument( + "feedfwd_gain specified but feedfwd_pattern != 'refgain'") + # Figure out what inputs to the system to use control_indices = _process_indices( control_indices, 'control', sys.input_labels, sys.ninputs) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 651e19b43..3928fb725 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -1145,8 +1145,10 @@ def test_gainsched_errors(unicycle): @pytest.mark.parametrize("ninputs, Kf", [ - (1, 1), (1, None), - (2, np.diag([1, 1])), (2, None), + (1, 1), + (1, None), + (2, np.diag([1, 1])), + (2, None), ]) def test_refgain_pattern(ninputs, Kf): sys = ct.rss(2, 2, ninputs, strictly_proper=True) @@ -1178,3 +1180,17 @@ def test_refgain_pattern(ninputs, Kf): np.testing.assert_almost_equal(clsys.B, manual.B) np.testing.assert_almost_equal(clsys.C[:sys.nstates, :], manual.C) np.testing.assert_almost_equal(clsys.D[:sys.nstates, :], manual.D) + + +def test_create_statefbk_errors(): + sys = ct.rss(2, 2, 1, strictly_proper=True) + sys.C = np.eye(2) + K = -np.ones((1, 4)) + Kf = 1 + + K, _, _ = ct.lqr(sys.A, sys.B, np.eye(sys.nstates), np.eye(sys.ninputs)) + with pytest.raises(NotImplementedError, match="unknown pattern"): + ct.create_statefbk_iosystem(sys, K, feedfwd_pattern='mypattern') + + with pytest.raises(ControlArgument, match="feedfwd_pattern != 'refgain'"): + ct.create_statefbk_iosystem(sys, K, Kf, feedfwd_pattern='trajgen') diff --git a/doc/iosys.rst b/doc/iosys.rst index e67f4dc9c..0554683f3 100644 --- a/doc/iosys.rst +++ b/doc/iosys.rst @@ -57,7 +57,7 @@ Example To illustrate the use of the input/output systems module, we create a model for a predator/prey system, following the notation and parameter -values in FBS2e. +values in `Feedback Systems `_. We begin by defining the dynamics of the system @@ -129,7 +129,8 @@ system and computing the linearization about that point. We next compute a controller that stabilizes the equilibrium point using eigenvalue placement and computing the feedforward gain using the number of -lynxes as the desired output (following FBS2e, Example 7.5): +lynxes as the desired output (following `Feedback Systems +`_, Example 7.5): .. code-block:: python @@ -488,10 +489,10 @@ A reference gain controller can be created with the command:: ctrl, clsys = ct.create_statefbk_iosystem(sys, K, kf, feedfwd_pattern='refgain') -This reference gain design pattern is described in more detail in Section -7.2 of FBS2e (Stabilization by State Feedback) and the trajectory -generation design pattern is described in Section 8.5 (State Space -Controller Design). +This reference gain design pattern is described in more detail in +Section 7.2 of `Feedback Systems `_ (Stabilization +by State Feedback) and the trajectory generation design pattern is +described in Section 8.5 (State Space Controller Design). If the full system state is not available, the output of a state estimator can be used to construct the controller using the command::