Skip to content

Commit 2b13747

Browse files
murrayrmmurrayrm
committed
add create_statefbk_iosystem + unit tests
1 parent 4b0584d commit 2b13747

File tree

9 files changed

+344
-46
lines changed

9 files changed

+344
-46
lines changed

‎control/iosys.py‎

Lines changed: 6 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@
3131
import copy
3232
from warnings import warn
3333

34-
from .namedio import _NamedIOStateObject, _process_signal_list
34+
from .namedio import _NamedIOStateSystem, _process_signal_list
3535
from .statesp import StateSpace, tf2ss, _convert_to_statespace
3636
from .statesp import _ss, _rss_generate
3737
from .xferfcn import TransferFunction
@@ -55,7 +55,7 @@
5555
}
5656

5757

58-
class InputOutputSystem(_NamedIOStateObject):
58+
class InputOutputSystem(_NamedIOStateSystem):
5959
"""A class for representing input/output systems.
6060
6161
The InputOutputSystem class allows (possibly nonlinear) input/output
@@ -139,7 +139,7 @@ def __init__(self, inputs=None, outputs=None, states=None, params={},
139139
140140
"""
141141
# Store the system name, inputs, outputs, and states
142-
_NamedIOStateObject.__init__(
142+
_NamedIOStateSystem.__init__(
143143
self, inputs=inputs, outputs=outputs, states=states, name=name)
144144

145145
# default parameters
@@ -886,7 +886,7 @@ def __init__(self, syslist, connections=[], inplist=[], outlist=[],
886886

887887
# Look for 'input' and 'output' parameter name variants
888888
inputs = _parse_signal_parameter(inputs, 'input', kwargs)
889-
outputs = _parse_signal_parameter(outputs, 'output', kwargs, end=True)
889+
outputs = _parse_signal_parameter(outputs, 'output', kwargs, end=True)
890890

891891
# Convert input and output names to lists if they aren't already
892892
if not isinstance(inplist, (list, tuple)):
@@ -2526,9 +2526,8 @@ def interconnect(syslist, connections=None, inplist=[], outlist=[],
25262526
raise ValueError('check_unused is False, but either '
25272527
+ 'ignore_inputs or ignore_outputs non-empty')
25282528

2529-
if (connections is False
2530-
and not inplist and not outlist
2531-
and not inputs and not outputs):
2529+
if connections is False and not inplist and not outlist \
2530+
and not inputs and not outputs:
25322531
# user has disabled auto-connect, and supplied neither input
25332532
# nor output mappings; assume they know what they're doing
25342533
check_unused = False

‎control/lti.py‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,13 +16,12 @@
1616
from numpy import absolute, real, angle, abs
1717
from warnings import warn
1818
from . import config
19-
from .namedio import _NamedIOObject
2019

2120
__all__ = ['issiso', 'timebase', 'common_timebase', 'timebaseEqual',
2221
'isdtime', 'isctime', 'pole', 'zero', 'damp', 'evalfr',
2322
'freqresp', 'dcgain']
2423

25-
class LTI(_NamedIOObject):
24+
class LTI:
2625
"""LTI is a parent class to linear time-invariant (LTI) system objects.
2726
2827
LTI is the parent to the StateSpace and TransferFunction child classes. It

‎control/namedio.py‎

Lines changed: 16 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,21 @@
11
# namedio.py - internal named I/O object class
22
# RMM, 13 Mar 2022
33
#
4-
# This file implements the _NamedIOObject and _NamedIOStateObject classes,
4+
# This file implements the _NamedIOSystem and _NamedIOStateSystem classes,
55
# which are used as a parent classes for FrequencyResponseData,
66
# InputOutputSystem, LTI, TimeResponseData, and other similar classes to
77
# allow naming of signals.
88

99
import numpy as np
1010

11-
class _NamedIOObject(object):
11+
12+
class _NamedIOSystem(object):
1213
_idCounter = 0
1314

1415
def _name_or_default(self, name=None):
1516
if name is None:
16-
name = "sys[{}]".format(_NamedIOObject._idCounter)
17-
_NamedIOObject._idCounter += 1
17+
name = "sys[{}]".format(_NamedIOSystem._idCounter)
18+
_NamedIOSystem._idCounter += 1
1819
return name
1920

2021
def __init__(
@@ -38,7 +39,7 @@ def __init__(
3839
#:
3940
#: :meta hide-value:
4041
ninputs = 0
41-
42+
4243
#: Number of system outputs.
4344
#:
4445
#: :meta hide-value:
@@ -88,7 +89,7 @@ def find_input(self, name):
8889
return self.input_index.get(name, None)
8990

9091
# Property for getting and setting list of input signals
91-
input_list = property(
92+
input_labels = property(
9293
lambda self: list(self.input_index.keys()), # getter
9394
set_inputs) # setter
9495

@@ -117,26 +118,25 @@ def find_output(self, name):
117118
return self.output_index.get(name, None)
118119

119120
# Property for getting and setting list of output signals
120-
output_list = property(
121+
output_labels = property(
121122
lambda self: list(self.output_index.keys()), # getter
122123
set_outputs) # setter
123124

124125
def issiso(self):
125126
"""Check to see if a system is single input, single output"""
126127
return self.ninputs == 1 and self.noutputs == 1
127128

128-
129-
class _NamedIOStateObject(_NamedIOObject):
129+
130+
class _NamedIOStateSystem(_NamedIOSystem):
130131
def __init__(
131132
self, inputs=None, outputs=None, states=None, name=None):
132133
# Parse and store the system name, inputs, and outputs
133-
_NamedIOObject.__init__(
134+
_NamedIOSystem.__init__(
134135
self, inputs=inputs, outputs=outputs, name=name)
135-
136+
136137
# Parse and store the number of states
137138
self.set_states(states)
138139

139-
140140
#
141141
# Class attributes
142142
#
@@ -151,12 +151,12 @@ def __init__(
151151

152152
def __str__(self):
153153
"""String representation of an input/output system"""
154-
str = _NamedIOObject.__str__(self)
154+
str = _NamedIOSystem.__str__(self)
155155
str += "\nStates (%s): " % self.nstates
156156
for key in self.state_index:
157157
str += key + ", "
158158
return str
159-
159+
160160
def _isstatic(self):
161161
"""Check to see if a system is a static system (no states)"""
162162
return self.nstates == 0
@@ -186,10 +186,11 @@ def find_state(self, name):
186186
return self.state_index.get(name, None)
187187

188188
# Property for getting and setting list of state signals
189-
state_list = property(
189+
state_labels = property(
190190
lambda self: list(self.state_index.keys()), # getter
191191
set_states) # setter
192192

193+
193194
# Utility function to parse a list of signals
194195
def _process_signal_list(signals, prefix='s'):
195196
if signals is None:

‎control/statefbk.py‎

Lines changed: 190 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,8 @@
4646
from .mateqn import care, dare, _check_shape
4747
from .statesp import StateSpace, _ssmatrix, _convert_to_statespace
4848
from .lti import LTI, isdtime, isctime
49+
from .iosys import InputOutputSystem, NonlinearIOSystem, LinearIOSystem, \
50+
interconnect, ss
4951
from .exception import ControlSlycot, ControlArgument, ControlDimension, \
5052
ControlNotImplemented
5153

@@ -69,7 +71,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None):
6971

7072

7173
__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe',
72-
'dlqr', 'dlqe', 'acker']
74+
'dlqr', 'dlqe', 'acker', 'create_statefbk_iosystem']
7375

7476

7577
# Pole placement
@@ -785,6 +787,193 @@ def dlqr(*args, **keywords):
785787
return _ssmatrix(K), _ssmatrix(S), E
786788

787789

790+
# Function to create an I/O sytems representing a state feedback controller
791+
def create_statefbk_iosystem(
792+
sys, K, integral_action=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]',
793+
estimator=None, type='linear'):
794+
"""Create an I/O system using a (full) state feedback controller
795+
796+
This function creates an input/output system that implements a
797+
state feedback controller of the form
798+
799+
u = ud - K_p (x - xd) - K_i integral(C x - C x_d)
800+
801+
It can be called in the form
802+
803+
ctrl, clsys = ct.create_statefbk_iosystem(sys, K)
804+
805+
where ``sys`` is the process dynamics and ``K`` is the state (+ integral)
806+
feedback gain (eg, from LQR). The function returns the controller
807+
``ctrl`` and the closed loop systems ``clsys``, both as I/O systems.
808+
809+
Parameters
810+
----------
811+
sys : InputOutputSystem
812+
The I/O system that represents the process dynamics. If no estimator
813+
is given, the output of this system should represent the full state.
814+
815+
K : ndarray
816+
The state feedback gain. This matrix defines the gains to be
817+
applied to the system. If ``integral_action`` is None, then the
818+
dimensions of this array should be (sys.ninputs, sys.nstates). If
819+
`integral action` is set to a matrix or a function, then additional
820+
columns represent the gains of the integral states of the
821+
controller.
822+
823+
xd_labels, ud_labels : str or list of str, optional
824+
Set the name of the signals to use for the desired state and inputs.
825+
If a single string is specified, it should be a format string using
826+
the variable ``i`` as an index. Otherwise, a list of strings matching
827+
the size of xd and ud, respectively, should be used. Default is
828+
``'xd[{i}]'`` for xd_labels and ``'xd[{i}]'`` for ud_labels.
829+
830+
integral_action : None, ndarray, or func, optional
831+
If this keyword is specified, the controller can include integral
832+
action in addition to state feedback. If ``integral_action`` is an
833+
ndarray, it will be multiplied by the current and desired state to
834+
generate the error for the internal integrator states of the control
835+
law. If ``integral_action`` is a function ``h``, that function will
836+
be called with the signature h(t, x, u, params) to obtain the
837+
outputs that should be integrated. The number of outputs that are
838+
to be integrated must match the number of additional columns in the
839+
``K`` matrix.
840+
841+
estimator : InputOutputSystem, optional
842+
If an estimator is provided, using the states of the estimator as
843+
the system inputs for the controller.
844+
845+
type : 'nonlinear' or 'linear', optional
846+
Set the type of controller to create. The default is a linear
847+
controller implementing the LQR regulator. If the type is 'nonlinear',
848+
a :class:NonlinearIOSystem is created instead, with the gain ``K`` as
849+
a parameter (allowing modifications of the gain at runtime).
850+
851+
Returns
852+
-------
853+
ctrl : InputOutputSystem
854+
Input/output system representing the controller. This system takes
855+
as inputs the desired state xd, the desired input ud, and the system
856+
state x. It outputs the controller action u according to the
857+
formula u = ud - K(x - xd). If the keyword `integral_action` is
858+
specified, then an additional set of integrators is included in the
859+
control system (with the gain matrix K having the integral gains
860+
appended after the state gains).
861+
862+
clsys : InputOutputSystem
863+
Input/output system representing the closed loop system. This
864+
systems takes as inputs the desired trajectory (xd, ud) and outputs
865+
the system state x and the applied input u (vertically stacked).
866+
867+
"""
868+
# Make sure that we were passed an I/O system as an input
869+
if not isinstance(sys, InputOutputSystem):
870+
raise ControlArgument("Input system must be I/O system")
871+
872+
# See whether we were give an estimator
873+
if estimator is not None:
874+
# Check to make sure the estimator is the right size
875+
if estimator.noutputs != sys.nstates:
876+
raise ControlArgument("Estimator output size must match state")
877+
elif sys.noutputs != sys.nstates:
878+
# If no estimator, make sure that the system has all states as outputs
879+
# TODO: check to make sure output map is the identity
880+
raise ControlArgument("System output must be the full state")
881+
else:
882+
# Use the system directly instead of an estimator
883+
estimator = sys
884+
885+
# See whether we should implement integral action
886+
nintegrators = 0
887+
if integral_action is not None:
888+
if not isinstance(integral_action, np.ndarray):
889+
raise ControlArgument("Integral action must pass an array")
890+
elif integral_action.shape[1] != sys.nstates:
891+
raise ControlArgument(
892+
"Integral gain output size must match system input size")
893+
else:
894+
nintegrators = integral_action.shape[0]
895+
C = integral_action
896+
else:
897+
# Create a C matrix with no outputs, just in case update gets called
898+
C = np.zeros((0, sys.nstates))
899+
900+
# Check to make sure that state feedback has the right shape
901+
if not isinstance(K, np.ndarray) or \
902+
K.shape != (sys.ninputs, estimator.noutputs + nintegrators):
903+
raise ControlArgument(
904+
f'Control gain must be an array of size {sys.ninputs}'
905+
f'x {sys.nstates}' +
906+
(f'+{nintegrators}' if nintegrators > 0 else ''))
907+
908+
# Figure out the labels to use
909+
if isinstance(xd_labels, str):
910+
# Gnerate the list of labels using the argument as a format string
911+
xd_labels = [xd_labels.format(i=i) for i in range(sys.nstates)]
912+
913+
if isinstance(ud_labels, str):
914+
# Gnerate the list of labels using the argument as a format string
915+
ud_labels = [ud_labels.format(i=i) for i in range(sys.ninputs)]
916+
917+
# Define the controller system
918+
if type == 'nonlinear':
919+
# Create an I/O system for the state feedback gains
920+
def _control_update(t, x, inputs, params):
921+
# Split input into desired state, nominal input, and current state
922+
xd_vec = inputs[0:sys.nstates]
923+
x_vec = inputs[-estimator.nstates:]
924+
925+
# Compute the integral error in the xy coordinates
926+
return C @ x_vec - C @ xd_vec
927+
928+
def _control_output(t, e, z, params):
929+
K = params.get('K')
930+
931+
# Split input into desired state, nominal input, and current state
932+
xd_vec = z[0:sys.nstates]
933+
ud_vec = z[sys.nstates:sys.nstates + sys.ninputs]
934+
x_vec = z[-sys.nstates:]
935+
936+
# Compute the control law
937+
u = ud_vec - K[:, 0:sys.nstates] @ (x_vec - xd_vec)
938+
if nintegrators > 0:
939+
u -= K[:, sys.nstates:] @ e
940+
941+
return u
942+
943+
ctrl = NonlinearIOSystem(
944+
_control_update, _control_output, name='control',
945+
inputs=xd_labels + ud_labels + estimator.output_labels,
946+
outputs=list(sys.input_index.keys()), params={'K': K},
947+
states=nintegrators)
948+
949+
elif type == 'linear' or type is None:
950+
# Create the matrices implementing the controller
951+
A_lqr = np.zeros((C.shape[0], C.shape[0]))
952+
B_lqr = np.hstack([-C, np.zeros((C.shape[0], sys.ninputs)), C])
953+
C_lqr = -K[:, sys.nstates:]
954+
D_lqr = np.hstack([
955+
K[:, 0:sys.nstates], np.eye(sys.ninputs), -K[:, 0:sys.nstates]
956+
])
957+
958+
ctrl = ss(
959+
A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name='control',
960+
inputs=xd_labels + ud_labels + estimator.output_labels,
961+
outputs=list(sys.input_index.keys()), states=nintegrators)
962+
963+
else:
964+
raise ControlArgument(f"unknown type '{type}'")
965+
966+
# Define the closed loop system
967+
closed = interconnect(
968+
[sys, ctrl] if estimator == sys else [sys, ctrl, estimator],
969+
name=sys.name + "_" + ctrl.name,
970+
inplist=xd_labels + ud_labels, inputs=xd_labels + ud_labels,
971+
outlist=sys.output_labels + sys.input_labels,
972+
outputs=sys.output_labels + sys.input_labels
973+
)
974+
return ctrl, closed
975+
976+
788977
def ctrb(A, B):
789978
"""Controllabilty matrix
790979

‎control/statesp.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@
5959
from scipy.signal import StateSpace as signalStateSpace
6060
from warnings import warn
6161
from .lti import LTI, common_timebase, isdtime, _process_frequency_response
62-
from .namedio import _NamedIOStateObject, _process_signal_list
62+
from .namedio import _NamedIOStateSystem, _process_signal_list
6363
from . import config
6464
from copy import deepcopy
6565

@@ -153,7 +153,7 @@ def _f2s(f):
153153
return s
154154

155155

156-
class StateSpace(LTI, _NamedIOStateObject):
156+
class StateSpace(LTI, _NamedIOStateSystem):
157157
"""StateSpace(A, B, C, D[, dt])
158158
159159
A class for representing state-space models.

0 commit comments

Comments
 (0)