Skip to content

Commit 21aa0a6

Browse files
murrayrmmurrayrm
committed
add examples, update documentation, small refactoring of code
1 parent 6e56b73 commit 21aa0a6

File tree

5 files changed

+259
-70
lines changed

5 files changed

+259
-70
lines changed

‎control/statefbk.py

Lines changed: 67 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -602,7 +602,9 @@ def dlqr(*args, **kwargs):
602602
# Function to create an I/O sytems representing a state feedback controller
603603
def create_statefbk_iosystem(
604604
sys, gain, integral_action=None, estimator=None, type=None,
605-
xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None):
605+
xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None,
606+
gainsched_method='linear', name=None, inputs=None, outputs=None,
607+
states=None):
606608
"""Create an I/O system using a (full) state feedback controller
607609
608610
This function creates an input/output system that implements a
@@ -640,29 +642,25 @@ def create_statefbk_iosystem(
640642
set to a matrix or a function, then additional columns
641643
represent the gains of the integral states of the controller.
642644
643-
If a tuple is given, then it specifies a gain schedule. The
644-
tuple should be of the form
645-
646-
(gains, points[, method])
647-
648-
where gains is a list of gains :math:`K_j` and points is a list of
649-
values :math:`\\mu_j` at which the gains are computed. If `method`
650-
is specified, it is passed to :func:`scipy.interpolate.griddata` to
651-
specify the method of interpolation. Possible values include
652-
`linear`, `nearest`, and `cubic`.
645+
If a tuple is given, then it specifies a gain schedule. The tuple
646+
should be of the form ``(gains, points)`` where gains is a list of
647+
gains :math:`K_j` and points is a list of values :math:`\\mu_j` at
648+
which the gains are computed. The `gainsched_indices` parameter
649+
should be used to specify the scheduling variables.
653650
654651
xd_labels, ud_labels : str or list of str, optional
655652
Set the name of the signals to use for the desired state and inputs.
656653
If a single string is specified, it should be a format string using
657654
the variable ``i`` as an index. Otherwise, a list of strings
658655
matching the size of xd and ud, respectively, should be used.
659656
Default is ``'xd[{i}]'`` for xd_labels and ``'ud[{i}]'`` for
660-
ud_labels.
657+
ud_labels. These settings can also be overriden using the `inputs`
658+
keyword.
661659
662660
integral_action : None, ndarray, or func, optional
663661
If this keyword is specified, the controller can include integral
664-
action in addition to state feedback. If ``integral_action`` is an
665-
ndarray, it will be multiplied by the current and desired state to
662+
action in addition to state feedback. If ``integral_action`` is a
663+
matrix, it will be multiplied by the current and desired state to
666664
generate the error for the internal integrator states of the control
667665
law. If ``integral_action`` is a function ``h``, that function will
668666
be called with the signature h(t, x, u, params) to obtain the
@@ -674,45 +672,59 @@ def create_statefbk_iosystem(
674672
If an estimator is provided, use the states of the estimator as
675673
the system inputs for the controller.
676674
677-
gainsched_indices : list of integers, optional
675+
gainsched_indices : list of int or str, optional
678676
If a gain scheduled controller is specified, specify the indices of
679-
the controller input to use for scheduling the gain. The input to
677+
the controller input to use for scheduling the gain. The input to
680678
the controller is the desired state xd, the desired input ud, and
681679
either the system state x or the system output y (if an estimator is
682-
given).
680+
given). The indices can either be specified as integer offsets into
681+
the input vector or as strings matching the signal names of the
682+
input vector.
683+
684+
gainsched_method : str, optional
685+
The method to use for gain scheduling. Possible values include
686+
`linear` (default), `nearest`, and `cubic`. More information is
687+
available in :func:`scipy.interpolate.griddata`. For points outside
688+
of the convex hull of the scheduling points, the gain at the nearest
689+
point is used.
683690
684691
type : 'linear' or 'nonlinear', optional
685692
Set the type of controller to create. The default for a linear gain
686693
is a linear controller implementing the LQR regulator. If the type
687694
is 'nonlinear', a :class:NonlinearIOSystem is created instead, with
688695
the gain ``K`` as a parameter (allowing modifications of the gain at
689-
runtime). If the gain parameter is a tuple, the a nonlinear,
696+
runtime). If the gain parameter is a tuple, then a nonlinear,
690697
gain-scheduled controller is created.
691698
692699
Returns
693700
-------
694701
ctrl : InputOutputSystem
695702
Input/output system representing the controller. This system takes
696703
as inputs the desired state xd, the desired input ud, and either the
697-
system state x or the system output y (if an estimator is given).
698-
It outputs the controller action u according to the formula u = ud -
699-
K(x - xd). If the keyword `integral_action` is specified, then an
700-
additional set of integrators is included in the control system
701-
(with the gain matrix K having the integral gains appended after the
702-
state gains). If a gain scheduled controller is specified, the gain
703-
(proportional and integral) are evaluated using the input mu.
704+
system state x or the estimated state xhat. It outputs the
705+
controller action u according to the formula u = ud - K(x - xd). If
706+
the keyword `integral_action` is specified, then an additional set
707+
of integrators is included in the control system (with the gain
708+
matrix K having the integral gains appended after the state gains).
709+
If a gain scheduled controller is specified, the gain (proportional
710+
and integral) are evaluated using the scheduling variables specified
711+
by ``gainsched_indices``.
704712
705713
clsys : InputOutputSystem
706714
Input/output system representing the closed loop system. This
707-
systems takes as inputs the desired trajectory (xd, ud) along with
708-
any unassigned gain scheduling values mu and outputs the system
709-
state x and the applied input u (vertically stacked).
710-
711-
Notes
712-
-----
713-
1. If the gain scheduling variable labes are set to the names of system
714-
states, inputs, or outputs or desired states or inputs, then the
715-
scheduling variables are internally connected to those variables.
715+
systems takes as inputs the desired trajectory (xd, ud) and outputs
716+
the system state x and the applied input u (vertically stacked).
717+
718+
Other Parameters
719+
----------------
720+
inputs, outputs : str, or list of str, optional
721+
List of strings that name the individual signals of the transformed
722+
system. If not given, the inputs and outputs are the same as the
723+
original system.
724+
725+
name : string, optional
726+
System name. If unspecified, a generic name <sys[id]> is generated
727+
with a unique integer id.
716728
717729
"""
718730
# Make sure that we were passed an I/O system as an input
@@ -759,8 +771,9 @@ def create_statefbk_iosystem(
759771

760772
elif isinstance(gain, tuple):
761773
# Check for gain scheduled controller
774+
if len(gain) != 2:
775+
raise ControlArgument("gain must be a 2-tuple for gain scheduling")
762776
gains, points = gain[0:2]
763-
method = 'nearest' if len(gain) < 3 else gain[2]
764777

765778
# Stack gains and points if past as a list
766779
gains = np.stack(gains)
@@ -788,8 +801,13 @@ def create_statefbk_iosystem(
788801
# Generate the list of labels using the argument as a format string
789802
ud_labels = [ud_labels.format(i=i) for i in range(sys.ninputs)]
790803

791-
# Create the string of labels for the control system
792-
input_labels = xd_labels + ud_labels + estimator.output_labels
804+
# Create the signal and system names
805+
if inputs is None:
806+
inputs = xd_labels + ud_labels + estimator.output_labels
807+
if outputs is None:
808+
outputs = list(sys.input_index.keys())
809+
if states is None:
810+
states = nintegrators
793811

794812
# Process gainscheduling variables, if present
795813
if gainsched:
@@ -806,21 +824,22 @@ def create_statefbk_iosystem(
806824
# Process scheduling variables
807825
for i, idx in enumerate(gainsched_indices):
808826
if isinstance(idx, str):
809-
gainsched_indices[i] = input_labels.index(gainsched_indices[i])
827+
gainsched_indices[i] = inputs.index(gainsched_indices[i])
810828

811829
# Create interpolating function
812-
if method == 'nearest':
830+
if gainsched_method == 'nearest':
813831
_interp = sp.interpolate.NearestNDInterpolator(points, gains)
814-
_nearest = _interp
815-
elif method == 'linear':
832+
def _nearest(mu):
833+
raise SystemError(f"could not find nearest gain at mu = {mu}")
834+
elif gainsched_method == 'linear':
816835
_interp = sp.interpolate.LinearNDInterpolator(points, gains)
817836
_nearest = sp.interpolate.NearestNDInterpolator(points, gains)
818-
elif method == 'cubic':
837+
elif gainsched_method == 'cubic':
819838
_interp = sp.interpolate.CloughTocher2DInterpolator(points, gains)
820839
_nearest = sp.interpolate.NearestNDInterpolator(points, gains)
821840
else:
822841
raise ControlArgument(
823-
f"unknown gain scheduling method '{method}'")
842+
f"unknown gain scheduling method '{gainsched_method}'")
824843

825844
def _compute_gain(mu):
826845
K = _interp(mu)
@@ -860,9 +879,8 @@ def _control_output(t, states, inputs, params):
860879

861880
params = {} if gainsched else {'K': K}
862881
ctrl = NonlinearIOSystem(
863-
_control_update, _control_output, name='control',
864-
inputs=input_labels, outputs=list(sys.input_index.keys()),
865-
params=params, states=nintegrators)
882+
_control_update, _control_output, name=name, inputs=inputs,
883+
outputs=outputs, states=states, params=params)
866884

867885
elif type == 'linear' or type is None:
868886
# Create the matrices implementing the controller
@@ -879,9 +897,8 @@ def _control_output(t, states, inputs, params):
879897
])
880898

881899
ctrl = ss(
882-
A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name='control',
883-
inputs=xd_labels + ud_labels + estimator.output_labels,
884-
outputs=list(sys.input_index.keys()), states=nintegrators)
900+
A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name,
901+
inputs=inputs, outputs=outputs, states=states)
885902

886903
else:
887904
raise ControlArgument(f"unknown type '{type}'")
@@ -890,7 +907,7 @@ def _control_output(t, states, inputs, params):
890907
closed = interconnect(
891908
[sys, ctrl] if estimator == sys else [sys, ctrl, estimator],
892909
name=sys.name + "_" + ctrl.name,
893-
inplist=xd_labels + ud_labels, inputs=xd_labels + ud_labels,
910+
inplist=inputs[:-sys.nstates], inputs=inputs[:-sys.nstates],
894911
outlist=sys.output_labels + sys.input_labels,
895912
outputs=sys.output_labels + sys.input_labels
896913
)

‎control/tests/statefbk_test.py

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -799,7 +799,7 @@ def unicycle_output(t, x, u, params):
799799

800800
from math import pi
801801

802-
@pytest.mark.parametrize("method", [None, 'nearest', 'linear', 'cubic'])
802+
@pytest.mark.parametrize("method", ['nearest', 'linear', 'cubic'])
803803
def test_gainsched_unicycle(unicycle, method):
804804
# Speeds and angles at which to compute the gains
805805
speeds = [1, 5, 10]
@@ -818,9 +818,8 @@ def test_gainsched_unicycle(unicycle, method):
818818

819819
# Create gain scheduled controller
820820
ctrl, clsys = ct.create_statefbk_iosystem(
821-
unicycle,
822-
(gains, points) if method is None else (gains, points, method),
823-
gainsched_indices=[3, 2])
821+
unicycle, (gains, points),
822+
gainsched_indices=[3, 2], gainsched_method=method)
824823

825824
# Check the gain at the selected points
826825
for speed, angle in points:
@@ -849,8 +848,8 @@ def test_gainsched_unicycle(unicycle, method):
849848
# Make sure that gains are different from 'nearest'
850849
if method is not None and method != 'nearest':
851850
ctrl_nearest, clsys_nearest = ct.create_statefbk_iosystem(
852-
unicycle, (gains, points, 'nearest'),
853-
gainsched_indices=['ud[0]', 2])
851+
unicycle, (gains, points),
852+
gainsched_indices=['ud[0]', 2], gainsched_method='nearest')
854853
nearest_lin = clsys_nearest.linearize(xe, [xd, ud])
855854
assert not np.allclose(
856855
np.sort(clsys_lin.poles()), np.sort(nearest_lin.poles()), rtol=1e-2)
@@ -931,6 +930,11 @@ def test_gainsched_errors(unicycle):
931930
ctrl, clsys = ct.create_statefbk_iosystem(
932931
unicycle, [gains, points], gainsched_indices=[3, 2])
933932

933+
# Wrong number of gain schedule argument
934+
with pytest.raises(ControlArgument, match="gain must be a 2-tuple"):
935+
ctrl, clsys = ct.create_statefbk_iosystem(
936+
unicycle, (gains, speeds, angles), gainsched_indices=[3, 2])
937+
934938
# Mismatched dimensions for gains and points
935939
with pytest.raises(ControlArgument, match="length of gainsched_indices"):
936940
ctrl, clsys = ct.create_statefbk_iosystem(
@@ -944,4 +948,5 @@ def test_gainsched_errors(unicycle):
944948
# Unknown gain scheduling method
945949
with pytest.raises(ControlArgument, match="unknown gain scheduling method"):
946950
ctrl, clsys = ct.create_statefbk_iosystem(
947-
unicycle, (gains, points, 'stuff'), gainsched_indices=[3, 2])
951+
unicycle, (gains, points),
952+
gainsched_indices=[3, 2], gainsched_method='unknown')

0 commit comments

Comments
 (0)