def test_duplicates(self):
nlios = ios.NonlinearIOSystem(lambda t,x,u,params: x, \
lambda t, x, u, params: u*u, \
inputs=1, outputs=1, states=1, name="sys")
# Turn off deprecation warnings
warnings.simplefilter("ignore", category=DeprecationWarning)
warnings.simplefilter("ignore", category=PendingDeprecationWarning)
# Duplicate objects
with warnings.catch_warnings(record=True) as warnval:
# Trigger a warning
ios_series = nlios * nlios
# Verify that we got a warning
self.assertEqual(len(warnval), 1)
self.assertTrue(issubclass(warnval[-1].category, UserWarning))
self.assertTrue("Duplicate object" in str(warnval[-1].message))
# Nonduplicate objects
nlios1 = nlios.copy()
nlios2 = nlios.copy()
with warnings.catch_warnings(record=True) as warnval:
ios_series = nlios1 * nlios2
self.assertEquals(len(warnval), 1)
# when subsystems have the same name, duplicates are
# renamed <subsysname_i>
self.assertTrue("copy of sys_1.x[0]" in ios_series.state_index.keys())
self.assertTrue("copy of sys.x[0]" in ios_series.state_index.keys())
# Duplicate names
iosys_siso = ct.LinearIOSystem(self.siso_linsys)
nlios1 = ios.NonlinearIOSystem(None, \
lambda t, x, u, params: u*u, inputs=1, outputs=1, name="sys")
nlios2 = ios.NonlinearIOSystem(None, \
lambda t, x, u, params: u*u, inputs=1, outputs=1, name="sys")
with warnings.catch_warnings(record=True) as warnval:
# Trigger a warning
iosys = ct.InterconnectedSystem(
(nlios1, iosys_siso, nlios2), inputs=0, outputs=0, states=0)
# Verify that we got a warning
self.assertEqual(len(warnval), 1)
self.assertTrue(issubclass(warnval[-1].category, UserWarning))
self.assertTrue("Duplicate name" in str(warnval[-1].message))
# Same system, different names => everything should be OK
nlios1 = ios.NonlinearIOSystem(None, \
lambda t, x, u, params: u*u, inputs=1, outputs=1, name="nlios1")
nlios2 = ios.NonlinearIOSystem(None, \
lambda t, x, u, params: u*u, inputs=1, outputs=1, name="nlios2")
with warnings.catch_warnings(record=True) as warnval:
iosys = ct.InterconnectedSystem(
(nlios1, iosys_siso, nlios2), inputs=0, outputs=0, states=0)
self.assertEqual(len(warnval), 0)
def test_interconnect_exceptions():
# First make sure the docstring example works
P = ct.tf2io(ct.tf(1, [1, 0]), input='u', output='y')
C = ct.tf2io(ct.tf(10, [1, 1]), input='e', output='u')
sumblk = ct.summing_junction(inputs=['r', '-y'], output='e')
T = ct.interconnect((P, C, sumblk), input='r', output='y')
assert (T.ninputs, T.noutputs, T.nstates) == (1, 1, 2)
# Unrecognized arguments
# LinearIOSystem
with pytest.raises(TypeError, match="unknown parameter"):
P = ct.LinearIOSystem(ct.rss(2, 1, 1), output_name='y')
# Interconnect
with pytest.raises(TypeError, match="unknown parameter"):
T = ct.interconnect((P, C, sumblk), input_name='r', output='y')
# Interconnected system
with pytest.raises(TypeError, match="unknown parameter"):
T = ct.InterconnectedSystem((P, C, sumblk), input_name='r', output='y')
# NonlinearIOSytem
with pytest.raises(TypeError, match="unknown parameter"):
nlios = ct.NonlinearIOSystem(
None, lambda t, x, u, params: u*u, input_count=1, output_count=1)
# Summing junction
with pytest.raises(TypeError, match="input specification is required"):
sumblk = ct.summing_junction()
with pytest.raises(TypeError, match="unknown parameter"):
sumblk = ct.summing_junction(input_count=2, output_count=2)
def closeLoop(tf,k_p,h_multiplier=1):
H=control.TransferFunction([h_multiplier],[1])
H=control.tf2io(H)
H.name='H'
H.set_inputs(['in'])
H.set_outputs(['out'])
D=control.TransferFunction([k_p],[1])
D=control.tf2io(D)
D.name='D'
D.set_inputs(['in'])
D.set_outputs(['out'])
system=control.tf2io(tf)
system.name='system'
system.set_inputs(['in'])
system.set_outputs(['out'])
# +
# in ---->o---->--D-->system----> out
# |- |
# -------H------------
system_MF = control.InterconnectedSystem([H,D,system] ,name='system_MF',
connections=[
['H.in','system.out'],
['D.in','-H.out'],
['system.in','D.out'],
],
inplist=['D.in'],
inputs=['in'],
outlist=['system.out','D.out','-H.out'],
outputs=['out','control','error'])
return system_MF
outputs=('u', 'j_hat', 'b_hat'),
states=3,
name='control',
dt=0
)
# Define the closed-loop system
# x_cl = [plant.x, control.x[0], control.x[1], control.x[2]]
# = [motorx, j_hat, b_hat, e_i]
# u_cl = [xd, xddot]
# y_cl = [motorx, Jhat, Bhat]
IO_CLOSED = control.InterconnectedSystem(
(IO_DC_MOTOR, IO_ADAPTIVE_PI),
connections=(
('plant.u', 'control.u'),
('control.u[2]', 'plant.x')
),
inplist=('control.u[0]', 'control.u[1]'),
outlist=('plant.x', 'control.j_hat', 'control.b_hat', 'control.u'),
dt=0
)
# Set simulation duration and time steps
N_POINTS = 3000
T_F = 60
# Set initial conditions
X0 = np.zeros((4, 1))
X0[0] = 1
X0[1] = J_HAT_0
X0[2] = B_HAT_0
#
# We construct the system using the InterconnectedSystem constructor and using
# signal labels to keep track of everything.
steering = ct.InterconnectedSystem(
# List of subsystems
(trajgen, controller, vehicle),
name='steering',
# Interconnections between subsystems
connections=(('controller.ex', 'trajgen.xd',
'-vehicle.x'), ('controller.ey', 'trajgen.yd', '-vehicle.y'),
('controller.etheta', 'trajgen.thetad', '-vehicle.theta'),
('controller.vd', 'trajgen.vd'), ('controller.phid',
'trajgen.phid'),
('vehicle.v', 'controller.v'), ('vehicle.phi',
'controller.phi')),
# System inputs
inplist=['trajgen.vref', 'trajgen.yref'],
inputs=['yref', 'vref'],
# System outputs
outlist=[
'vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
'controller.phi'
],
outputs=['x', 'y', 'theta', 'v', 'phi'])
# Set up the simulation conditions
yref = 1
T = np.linspace(0, 5, 100)
outputs=('y_r', 'psi_ref', 'delta_ref', 'v_r'))
###############################################################################
# System Connect
###############################################################################
LatSlidingModeControl = ct.InterconnectedSystem(
# List of subsystems
(target, controller, vehicle),
name='LatSlidingModeControl',
# Interconnections between subsystems
connections=(
('target.x_ref', 'vehicle.x'),
('vehicle.v', 'target.v_r'),
('controller.e_y', 'target.y_r', '-vehicle.y'),
('controller.psi_r', 'target.psi_ref'),
('controller.psi', 'vehicle.psi'),
('vehicle.delta_rate', 'controller.delta_rate'),
),
# System inputs
inplist=['target.v_ref'],
inputs=['vref'],
# System outputs
outlist=['vehicle.x', 'vehicle.y', 'vehicle.psi', 'vehicle.delta'],
outputs=['x', 'y', 'psi', 'delta'])
###############################################################################
# Input Output Response
###############################################################################
# time of response
adaptive_output,
inputs=3,
outputs=('u', 'theta', 'k', 'e',
'delta_u', 'u_c', 'e_delta',
'e_u'),
states=4,
name='control',
dt=0)
# Define the closed-loop system
# x_cl = [plant.x_p, ref_model.x_m, control.theta, control.k, control.beta_delta, control.e_delta]
IO_CLOSED = control.InterconnectedSystem(
(IO_PLANT, IO_REF_MODEL, IO_ADAPTIVE),
connections=(('plant.u', 'control.u'), ('control.u[1]', 'plant.x_p'),
('control.u[2]', 'ref_model.x_m')),
inplist=('ref_model.r', 'control.u[0]'),
outlist=('plant.x_p', 'ref_model.x_m', 'control.u', 'control.theta',
'control.k', 'control.e', 'control.u_c', 'control.e_delta',
'control.e_u'),
dt=0)
# Set simulation duration and time steps
N_POINTS = 3000
T_F = 20
# Set initial conditions
X0 = np.zeros((6, 1))
X0[0] = X_P_0
X0[1] = X_M_0
X0[2] = THETA_0
X0[3] = K_0
# System Connect
###############################################################################
LatRearWheelFeedbackControl = ct.InterconnectedSystem(
# List of subsystems
(target, controller, vehicle), name='LatRearWheelFeedbackControl',
# Interconnections between subsystems
connections=(
('target.x','vehicle.x'),
('target.y','vehicle.y'),
('controller.e_x','vehicle.x','-target.x_r'), # e_x:x轴方向偏差
('controller.e_y','vehicle.y','-target.y_r'), # e_y:y轴方向偏差
('controller.e_yaw','vehicle.yaw','-target.yaw_r'), # e_yaw:偏航角偏差
('controller.yaw_ref','target.yaw_r'),
('controller.k_ref','target.k_r'),
('controller.v_ref','target.v_r'),
('vehicle.delta', 'controller.delta'),
('vehicle.v', 'controller.v')
),
# System inputs
inplist=['target.v_ref'],
inputs=['v_ref'],
# System outputs
outlist=['vehicle.x', 'vehicle.y' , 'vehicle.yaw','controller.delta','target.yaw_r'],
outputs=['x', 'y', 'psi', 'delta', 'psi_ref']
)
###############################################################################
# Construct a PI controller with rolloff, as a transfer function
Kp = 0.5 # proportional gain
Ki = 0.1 # integral gain
control_tf = ct.tf2io(
ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]),
name='control', inputs='u', outputs='y')
# Construct the closed loop control system
# Inputs: vref, gear, theta
# Outputs: v (vehicle velocity)
cruise_tf = ct.InterconnectedSystem(
(control_tf, vehicle), name='cruise',
connections=(
['control.u', '-vehicle.v'],
['vehicle.u', 'control.y']),
inplist=('control.u', 'vehicle.gear', 'vehicle.theta'),
inputs=('vref', 'gear', 'theta'),
outlist=('vehicle.v', 'vehicle.u'),
outputs=('v', 'u'))
# Define the time and input vectors
T = np.linspace(0, 25, 101)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta0 = np.zeros(T.shape)
# Now simulate the effect of a hill at t = 5 seconds
plt.figure()
plt.suptitle('Response to change in road slope')
vel_axes = plt.subplot(2, 1, 1)
# x_cl = [plant[5], reference[2], input_filter[4], output_filter[4], controller[12]]
IO_CLOSED = control.InterconnectedSystem(
(IO_PLANT, IO_REF_MODEL, IO_INPUT_FILTER, IO_OUTPUT_FILTER, IO_ADAPTIVE),
connections=(('plant.u[0]', 'control.y[0]'), ('plant.u[1]',
'control.y[1]'),
('input_filter.u[0]', 'control.y[0]'), ('input_filter.u[1]',
'control.y[1]'),
('output_filter.u[0]',
'plant.y[0]'), ('output_filter.u[1]',
'plant.y[1]'), ('control.u[2]',
'input_filter.y[0]'),
('control.u[3]', 'input_filter.y[1]'), ('control.u[4]',
'input_filter.y[2]'),
('control.u[5]', 'input_filter.y[3]'), ('control.u[6]',
'output_filter.y[0]'),
('control.u[7]',
'output_filter.y[1]'), ('control.u[8]',
'output_filter.y[2]'),
('control.u[9]',
'output_filter.y[3]'), ('control.u[10]',
'output_filter.y[4]'),
('control.u[11]', 'output_filter.y[5]'), ('control.u[12]',
'plant.y[0]'),
('control.u[13]',
'plant.y[1]'), ('control.u[14]',
'ref_model.y[0]'), ('control.u[15]',
'ref_model.y[1]')),
inplist=('ref_model.u[0]', 'ref_model.u[1]', 'control.u[0]',
'control.u[1]'),
outlist=('plant.y[0]', 'plant.y[1]', 'ref_model.y[0]', 'ref_model.y[1]'),
dt=0)
name='controlLQR')
io_closed = control.InterconnectedSystem(
# (sysFull, controlLQR),
# (sysPartKF, controlLQR),
# (sysFullNonLin, sysPartKF, controlLQR),
(sysFull, sysPartKF, controlLQR),
connections=(
# ('sysFullNonLin.u', 'controlLQR.f'),
('sysPartKF.u', 'controlLQR.f'),
('sysPartKF.uKF', 'controlLQR.f'),
('controlLQR.x', 'sysPartKF.x_h'),
('controlLQR.xdot', 'sysPartKF.xdot_h'),
('controlLQR.theta', 'sysPartKF.theta_h'),
('controlLQR.thetadot', 'sysPartKF.thetadot_h'),
('sysFull.u', 'controlLQR.f'),
# ('controlLQR.x', 'sysFull.x'),
# ('controlLQR.xdot', 'sysFull.xdot'),
# ('controlLQR.theta', 'sysFull.theta'),
# ('controlLQR.thetadot', 'sysFull.thetadot')
),
inplist=[
'sysPartKF.vd0', 'sysPartKF.vd1', 'sysPartKF.vd2', 'sysPartKF.vd3',
'sysPartKF.vn', 'sysFull.vd0', 'sysFull.vd1', 'sysFull.vd2',
'sysFull.vd3', 'sysFull.vn'
],
# outlist=('sysPartKF.x_h', 'sysPartKF.xdot_h', 'sysPartKF.theta_h', 'sysPartKF.thetadot_h')
# outlist=('sysFull.x', 'sysFull.xdot', 'sysFull.theta', 'sysFull.thetadot')
outlist=('sysFull.x', 'sysFull.xdot', 'sysFull.theta', 'sysFull.thetadot'))
