def test_static_nonlinear_call(satsys):
# Make sure that the saturation system is a static nonlinearity
assert satsys._isstatic()
# Make sure the saturation function is doing the right computation
input = [-2, -1, -0.5, 0, 0.5, 1, 2]
desired = [-1, -1, -0.5, 0, 0.5, 1, 1]
for x, y in zip(input, desired):
assert satsys(x) == y
# Test squeeze properties
assert satsys(0.) == 0.
assert satsys([0.], squeeze=True) == 0.
np.testing.assert_array_equal(satsys([0.]), [0.])
# Test SIMO nonlinearity
def _simofcn(t, x, u, params):
return np.array([np.cos(u), np.sin(u)])
simo_sys = ct.NonlinearIOSystem(None, outfcn=_simofcn, input=1, output=2)
np.testing.assert_array_equal(simo_sys([0.]), [1, 0])
np.testing.assert_array_equal(simo_sys([0.], squeeze=True), [1, 0])
# Test MISO nonlinearity
def _misofcn(t, x, u, params={}):
return np.array([np.sin(u[0]) * np.cos(u[1])])
miso_sys = ct.NonlinearIOSystem(None, outfcn=_misofcn, input=2, output=1)
np.testing.assert_array_equal(miso_sys([0, 0]), [0])
np.testing.assert_array_equal(miso_sys([0, 0], squeeze=True), [0])
def test_sys_naming_convention(self):
"""Enforce generic system names 'sys[i]' to be present when systems are created
without explicit names."""
ct.InputOutputSystem.idCounter = 0
sys = ct.LinearIOSystem(self.mimo_linsys1)
self.assertEquals(sys.name, "sys[0]")
self.assertEquals(sys.copy().name, "copy of sys[0]")
namedsys = ios.NonlinearIOSystem(
updfcn = lambda t, x, u, params: x,
outfcn = lambda t, x, u, params: u,
inputs = ('u[0]', 'u[1]'),
outputs = ('y[0]', 'y[1]'),
states = self.mimo_linsys1.states,
name = 'namedsys')
unnamedsys1 = ct.NonlinearIOSystem(
lambda t,x,u,params: x, inputs=2, outputs=2, states=2
)
unnamedsys2 = ct.NonlinearIOSystem(
None, lambda t,x,u,params: u, inputs=2, outputs=2
)
self.assertEquals(unnamedsys2.name, "sys[2]")
# Unnamed/unnamed connections
uu_series = unnamedsys1 * unnamedsys2
uu_parallel = unnamedsys1 + unnamedsys2
u_neg = - unnamedsys1
uu_feedback = unnamedsys2.feedback(unnamedsys1)
uu_dup = unnamedsys1 * unnamedsys1.copy()
uu_hierarchical = uu_series*unnamedsys1
self.assertEquals(uu_series.name, "sys[3]")
self.assertEquals(uu_parallel.name, "sys[4]")
self.assertEquals(u_neg.name, "sys[5]")
self.assertEquals(uu_feedback.name, "sys[6]")
self.assertEquals(uu_dup.name, "sys[7]")
self.assertEquals(uu_hierarchical.name, "sys[8]")
# Unnamed/named connections
un_series = unnamedsys1 * namedsys
un_parallel = unnamedsys1 + namedsys
un_feedback = unnamedsys2.feedback(namedsys)
un_dup = unnamedsys1 * namedsys.copy()
un_hierarchical = uu_series*unnamedsys1
self.assertEquals(un_series.name, "sys[9]")
self.assertEquals(un_parallel.name, "sys[10]")
self.assertEquals(un_feedback.name, "sys[11]")
self.assertEquals(un_dup.name, "sys[12]")
self.assertEquals(un_hierarchical.name, "sys[13]")
# Same system conflict
with warnings.catch_warnings(record=True) as warnval:
unnamedsys1 * unnamedsys1
self.assertEqual(len(warnval), 1)
def satsys():
satfcn = saturation_class()
def _satfcn(t, x, u, params):
return satfcn(u)
return ct.NonlinearIOSystem(None, outfcn=_satfcn, input=1, output=1)
def __init__(self, T, Ts, l_r=0.5, L=1.0):
"""Constructor.
Parameters
==========
T : int
Timesteps.
Ts : float
Step size in seconds.
l_r : float
Length of hind wheel to bicycle center of gravity.
L : float
Length of bicycle longitudinal dimension from hind to front wheel.
"""
self.T = T
self.Ts = Ts
self.timestamps = np.linspace(0, Ts * T, T + 1)
self.l_r = l_r
self.L = L
self.io_dynamical_system = control.NonlinearIOSystem(
bicycle_kinematics,
None,
inputs=('u_1', 'u_2'),
outputs=('x', 'y', 'psi', 'v'),
states=('x', 'y', 'psi', 'v'),
params={
'l_r': l_r,
'L': L
},
name='bicycle_kinematics')
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 test_signals_naming_convention(self):
"""Enforce generic names to be present when systems are created
without explicit signal names:
input: 'u[i]'
state: 'x[i]'
output: 'y[i]'
"""
ct.InputOutputSystem.idCounter = 0
sys = ct.LinearIOSystem(self.mimo_linsys1)
for statename in ["x[0]", "x[1]"]:
self.assertTrue(statename in sys.state_index)
for inputname in ["u[0]", "u[1]"]:
self.assertTrue(inputname in sys.input_index)
for outputname in ["y[0]", "y[1]"]:
self.assertTrue(outputname in sys.output_index)
self.assertEqual(len(sys.state_index), sys.nstates)
self.assertEqual(len(sys.input_index), sys.ninputs)
self.assertEqual(len(sys.output_index), sys.noutputs)
namedsys = ios.NonlinearIOSystem(
updfcn = lambda t, x, u, params: x,
outfcn = lambda t, x, u, params: u,
inputs = ('u0'),
outputs = ('y0'),
states = ('x0'),
name = 'namedsys')
unnamedsys = ct.NonlinearIOSystem(
lambda t,x,u,params: x, inputs=1, outputs=1, states=1
)
self.assertTrue('u0' in namedsys.input_index)
self.assertTrue('y0' in namedsys.output_index)
self.assertTrue('x0' in namedsys.state_index)
# Unnamed/named connections
un_series = unnamedsys * namedsys
un_parallel = unnamedsys + namedsys
un_feedback = unnamedsys.feedback(namedsys)
un_dup = unnamedsys * namedsys.copy()
un_hierarchical = un_series*unnamedsys
u_neg = - unnamedsys
self.assertTrue("sys[1].x[0]" in un_series.state_index)
self.assertTrue("namedsys.x0" in un_series.state_index)
self.assertTrue("sys[1].x[0]" in un_parallel.state_index)
self.assertTrue("namedsys.x0" in un_series.state_index)
self.assertTrue("sys[1].x[0]" in un_feedback.state_index)
self.assertTrue("namedsys.x0" in un_feedback.state_index)
self.assertTrue("sys[1].x[0]" in un_dup.state_index)
self.assertTrue("copy of namedsys.x0" in un_dup.state_index)
self.assertTrue("sys[1].x[0]" in un_hierarchical.state_index)
self.assertTrue("sys[2].sys[1].x[0]" in un_hierarchical.state_index)
self.assertTrue("sys[1].x[0]" in u_neg.state_index)
# Same system conflict
with warnings.catch_warnings(record=True) as warnval:
same_name_series = unnamedsys * unnamedsys
self.assertEquals(len(warnval), 1)
self.assertTrue("sys[1].x[0]" in same_name_series.state_index)
self.assertTrue("copy of sys[1].x[0]" in same_name_series.state_index)
def __init__(self, params={}):
self.sys = ctrl.NonlinearIOSystem(updfcn=self.pi_update,
outfcn=self.pi_split_accel_brake,
name='driver',
inputs=['v', 'vref'],
outputs=['ya'],
states=['z'],
params=params)
self.sys.name = 'driver'
def sys_dict():
sdict = {}
sdict['ss'] = ct.ss([[-1]], [[1]], [[1]], [[0]])
sdict['tf'] = ct.tf([1], [0.5, 1])
sdict['tfx'] = ct.tf([1, 1], [1]) # non-proper transfer function
sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j], [1, 2, 3])
sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]]))
sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs, sdict['lio']._out,
1, 1, 1)
sdict['arr'] = np.array([[2.0]])
sdict['flt'] = 3.
return sdict
def sys_dict():
sdict = {}
sdict['ss'] = ct.StateSpace([[-1]], [[1]], [[1]], [[0]])
sdict['tf'] = ct.TransferFunction([1], [0.5, 1])
sdict['tfx'] = ct.TransferFunction([1, 1], [1]) # non-proper TF
sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j, 7 + 3j], [1, 2, 3, 4])
sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]]))
sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs,
sdict['lio']._out,
inputs=1,
outputs=1,
states=1)
sdict['arr'] = np.array([[2.0]])
sdict['flt'] = 3.
return sdict
def compute_nonlinear_dynamical_states(initial_state,
T,
Ts,
U,
l_r=0.5,
L=1.0):
"""Compute non-linear dynamical states from bicycle kinematic model.
Parameters
==========
initial_state : ndarray
Initial state [x_0, y_0, psi_0, v_0].
T : int
Timesteps.
Ts : float
Step size in seconds.
U : ndarray
Control inputs of shape (T, 2) [u_1, u_2]
l_r : float
Length of hind wheel to bicycle center of gravity.
L : float
Length of bicycle longitudinal dimension from hind to front wheel.
Returns
=======
ndarray
Trajectory from control of shape (T + 1, 4) with rows [x, y, psi, v].
"""
timestamps = np.linspace(0, Ts * T, T + 1)
mock_inputs = np.concatenate((U, np.array([0, 0])[None]), axis=0).T
io_bicycle_kinematics = control.NonlinearIOSystem(
bicycle_kinematics,
None,
inputs=('u_1', 'u_2'),
outputs=('x', 'y', 'psi', 'v'),
states=('x', 'y', 'psi', 'v'),
params={
'l_r': l_r,
'L': L
},
name='bicycle_kinematics')
_, states = control.input_output_response(io_bicycle_kinematics,
timestamps, mock_inputs,
initial_state)
return states.T
def compute_nonlinear_dynamical_states(initial_state,
T,
Ts,
U,
l_r=0.5,
L=1.0):
"""
Parameters
==========
initial_state : ndarray
Initial state [x_0, y_0, psi_0, v_0, delta_0].
T : int
Timesteps.
Ts : float
Step size in seconds.
U : ndarray
Control inputs of shape (T, 2) [u_1, u_2]
Returns
=======
ndarray
Trajectory from control of shape (T + 1, 5) with rows [x, y, psi, v, delta].
"""
timestamps = np.linspace(0, Ts * T, T + 1)
mock_inputs = np.concatenate((U, np.array([0, 0])[None]), axis=0).T
io_bicycle_kinematics = control.NonlinearIOSystem(
bicycle_kinematics,
None,
inputs=('u_1', 'u_2'),
outputs=('x', 'y', 'psi', 'v', 'delta'),
states=('x', 'y', 'psi', 'v', 'delta'),
params={
'l_r': l_r,
'L': L
},
name='bicycle_kinematics')
_, states = control.input_output_response(io_bicycle_kinematics,
timestamps, mock_inputs,
initial_state)
return states.T
'L': L, # pendulum length
'g': g, # gravity
'd': d, # dampping term
}
tspan = [0, 10] # time for simulation
dt = 0.04
yInit = [0.0, 0.0, 0.0, 0.1] # initial state
xeq = [0.3, 0.0, 0.0, 0.0] # final stable state
# Build non-linear standard system
sysFullNonLin = control.NonlinearIOSystem(cartpendSys_pyCtl,
None,
inputs=('u'),
outputs=('x', 'xdot', 'theta',
'thetadot'),
states=('x', 'xdot', 'theta',
'thetadot'),
params=sysParams,
name='sysFullNonLin')
sysLin = sysFullNonLin.linearize([0.0, 0.0, 0.0, 0.0], 0.0,
params=sysParams) # linearize on up position
A = sysLin.A
B = sysLin.B
BF = np.concatenate([B, Vd, 0.0 * B],
axis=1) # augment inputs to include disturbance and noise
C_full = sysLin.C
C_part = np.array([1, 0, 0, 0], dtype=np.float)
ssFull = control.StateSpace(A, BF, C_full, np.zeros(
# Algebraic relationships
e = x_d - x
e_1 = x_d_dot + LAMBDA * e
e_2 = e + LAMBDA * e_i
# Control law
u = j_hat * e_1 + b_hat * x + K * e_2
return [u, x_state[0], x_state[1]]
IO_ADAPTIVE_PI = control.NonlinearIOSystem(
adaptive_pi_state,
adaptive_pi_output,
inputs=3,
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')
# Return the derivative of the state
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(theta) v
np.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
])
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_update,
vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'phi'),
outputs=('x', 'y', 'theta'))
#
# Gain scheduled controller
#
# For this system we use a simple schedule on the forward vehicle velocity and
# place the poles of the system at fixed values. The controller takes the
# current vehicle position and orientation plus the velocity velocity as
# inputs, and returns the velocity and steering commands.
#
# System state: none
# System input: ex, ey, etheta, vd, phid
# System output: v, phi
def __init__(self, state, lane_width, dt=0.1):
"""
Function to initiate the lateral Model, it will set the parameters.
:param state: dict of system state
:param lane_width: float, width of initial lane in meters [m]
:param dt: timestep increment of the simulation
"""
# Initial lane position should be lane center
self.lane_width = lane_width
self.dt = dt
# Initial steering angle is 0
self.steering_angle = 0
# In lane position is the distance from the right tangent line of the lane
# Initial lane position is middle of the lane
self.in_lane_pos = self.lane_width / 2
# Initial ego vehicle state and parameters
self.lateral_state = {
'heading': np.radians(state['heading']),
'steering_angle': self.steering_angle,
'x_position': state['x_position'],
'y_position': state['y_position'],
'speed': state['speed'],
'lane_id': state['lane_id'],
# Vehicle parameters that are not supposed to update:
# Vehicle dimensions for collision detection
'length': state['length'],
'width': state['width'],
}
self.vehicle_params = {
# Rear wheel offset in meters
'refoffset': 1.5,
# Wheelbase in meters
'wheelbase': 3,
# Maximum steering angle in radians
'maxsteer': 0.5,
}
# Define the vehicle steering dynamics as an input/output system
self.vehicle = ct.NonlinearIOSystem(
self.vehicle_update,
self.vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'delta'),
outputs=('x', 'y'),
params=self.vehicle_params,
)
self.pos_log = {
'x_position': [self.lateral_state['x_position']],
'y_position': [self.lateral_state['y_position']],
'heading': [self.lateral_state['heading']],
'lane_id': [self.lateral_state['lane_id']]
}
omega_2 = [
u_input[1], u_input[3], u_input[5], u_input[7], u_input[9], u_input[11]
]
# Control law
# u = Theta * omega
return [
sum([a * b for a, b in zip(theta_1, omega_1)]),
sum([a * b for a, b in zip(theta_2, omega_2)])
]
IO_ADAPTIVE = control.NonlinearIOSystem(adaptive_state,
adaptive_output,
inputs=16,
outputs=2,
states=12,
name='control',
dt=0)
# Define the closed-loop system
# 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]',
def test_optimal_doc():
"""Test optimal control problem from documentation"""
def vehicle_update(t, x, u, params):
# Get the parameters for the model
l = params.get('wheelbase', 3.) # vehicle wheelbase
phimax = params.get('maxsteer', 0.5) # max steering angle (rad)
# Saturate the steering input
phi = np.clip(u[1], -phimax, phimax)
# Return the derivative of the state
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(theta) v
np.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
])
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_update,
vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'phi'),
outputs=('x', 'y', 'theta'))
# Define the initial and final points and time interval
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Define the cost functions
Q = np.diag([0, 0, 0.1]) # don't turn too sharply
R = np.diag([1, 1]) # keep inputs small
P = np.diag([1000, 1000, 1000]) # get close to final point
traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
# Define the constraints
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
# Solve the optimal control problem
horizon = np.linspace(0, Tf, 3, endpoint=True)
result = opt.solve_ocp(vehicle,
horizon,
x0,
traj_cost,
constraints,
terminal_cost=term_cost,
initial_guess=u0)
# Make sure the resulting trajectory generate a good solution
resp = ct.input_output_response(vehicle,
horizon,
result.inputs,
x0,
t_eval=np.linspace(0, Tf, 10))
t, y = resp
assert (y[0, -1] - xf[0]) / xf[0] < 0.01
assert (y[1, -1] - xf[1]) / xf[1] < 0.01
assert y[2, -1] < 0.1
e = x_p - x_m
e_u = e - e_delta
# Control law
u_c = theta * x_p + k * r
u = u_c if abs(u_c) <= U_MAX else U_MAX * np.sign(u_c)
delta_u = u - u_c
return [u, theta, k, e, delta_u, u_c, e_delta, e_u]
IO_ADAPTIVE = control.NonlinearIOSystem(adaptive_state,
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'),
# u = controller power signal
# returns heater power after saturation
power = x[0]
if u < 0.0:
u = 0
if u > Pmax: #300Watt heating element
u = Pmax
dP = u - power
return dP
# https://python-control.readthedocs.io/en/0.8.3/iosys.html
# Heater system
heater_IO = ctl.NonlinearIOSystem(hupdate,
None,
inputs='p',
outputs='ph',
states='p')
ctl_PID_IO = ctl.iosys.tf2io(conpid) # includes Kd
plant_IO = ctl.LinearIOSystem(ss_mod)
ufb_IO = ctl.iosys.tf2io(ufb) # unity feedback
clsysSat = ctl.feedback(
ctl.series(ctl_PID_IO, heater_IO, plant_IO), ufb_IO,
sign=-1) # negative fb closed loop system w/ Saturation
# Saturation test:
#input = Texp # simple ramp
#for i,iN in enumerate(input):
#input[i] = 2*iN - 250 # expose the saturation
# Return the derivative of the state
return np.array([
math.cos(x[2]) * u[0], # xdot = cos(theta) v
math.sin(x[2]) * u[0], # ydot = sin(theta) v
(u[0] / l) * math.tan(phi) # thdot = v/l tan(phi)
])
def vehicle_output(t, x, u, params):
return x # return x, y, theta (full state)
vehicle = ct.NonlinearIOSystem(vehicle_update,
vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'phi'),
outputs=('x', 'y', 'theta'))
# Initial and final conditions
x0 = [0., -2., 0.]
u0 = [10., 0.]
xf = [100., 2., 0.]
uf = [10., 0.]
Tf = 10
# Define the time horizon (and spacing) for the optimization
horizon = np.linspace(0, Tf, 10, endpoint=True)
# Provide an intial guess (will be extended to entire horizon)
bend_left = [10, 0.01] # slight left veer
# The force F is generated by the engine, whose torque is proportional to
# the rate of fuel injection, which is itself proportional to a control
# signal 0 <= u <= 1 that controls the throttle position. The torque also
# depends on engine speed omega.
def motor_torque(omega, params={}):
# Set up the system parameters
Tm = params.get('Tm', 190.) # engine torque constant
omega_m = params.get('omega_m', 420.) # peak engine angular speed
beta = params.get('beta', 0.4) # peak engine rolloff
return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None)
# Define the input/output system for the vehicle
vehicle = ct.NonlinearIOSystem(
vehicle_update, None, name='vehicle',
inputs=('u', 'gear', 'theta'), outputs=('v'), states=('v'))
# Figure 1.11: A feedback system for controlling the speed of a vehicle. In
# this example, the speed of the vehicle is measured and compared to the
# desired speed. The controller is a PI controller represented as a transfer
# function. In the textbook, the simulations are done for LTI systems, but
# here we simulate the full nonlinear system.
# 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')
# Algebraic relationships
e = x_p - x_m
# Control law
u = theta * x_p + r
# Constant bias
v = V
return [u, v, theta, e]
IO_ADAPTIVE = control.NonlinearIOSystem(adaptive_state,
adaptive_output,
inputs=3,
outputs=('u', 'v', 'theta', 'e'),
states=1,
name='control',
dt=0)
# Define the closed-loop system
# x_cl = [plant.x_p, ref_model.x_m, control.theta]
IO_CLOSED = control.InterconnectedSystem(
(IO_PLANT, IO_REF_MODEL, IO_ADAPTIVE),
connections=(('plant.u', 'control.u'), ('plant.v', 'control.v'),
('control.u[1]', 'plant.x_p'), ('control.u[2]',
'ref_model.x_m')),
inplist=('control.u[0]'),
outlist=('plant.x_p', 'ref_model.x_m', 'control.u', 'control.v',
'control.theta', 'control.e'),
dt=0)
return x[0:2]
# Default vehicle parameters (including nominal velocity)
vehicle_params = {
'refoffset': 1.5,
'wheelbase': 3,
'velocity': 15,
'maxsteer': 0.5
}
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(vehicle_update,
vehicle_output,
states=3,
name='vehicle',
inputs=('v', 'delta'),
outputs=('x', 'y'),
params=vehicle_params)
#
#
#
# ## Vehicle driving on a curvy road (Figure 8.6a)
# ##
# System parameters
wheelbase = vehicle_params['wheelbase'] # coming from part1
v0 = vehicle_params['velocity'] # Dictionary parameters membership key
# Control inputs
return np.array([
np.cos(x[2]) * u[0], # xdot = cos(psi) v
np.sin(x[2]) * u[0], # ydot = sin(psi) v
(u[0] / l) * np.tan(x[3]), # psi_dot = v/l tan(delta)
delta_rate # delta_dot = u
])
def vehicle_output(t, x, u, params):
return x # return x, y, psi (full state)
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(vehicle_update,
vehicle_output,
states=4,
name='vehicle',
inputs=('v', 'delta_rate'),
outputs=('x', 'y', 'psi', 'delta'))
###############################################################################
# Control
###############################################################################
def Sigmoid(x):
return x / (np.fabs(x) + eps)
def Sat(x):
val = 0
if x >= Delta:
val = 1
#
def target_output(t, x, u, params):
ex = [u[1] - icx for icx in target_curvature_sets.x]
ey = [u[2] - icy for icy in target_curvature_sets.y]
d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(ex, ey)]
mind = min(d)
index = d.index(mind)
return np.array([target_curvature_sets.x[index],target_curvature_sets.y[index],target_curvature_sets.yaw[index],target_curvature_sets.k[index],u[0]])
# Define the trajectory generator as an input/output system
target = ct.NonlinearIOSystem(
None, target_output, name='target',
inputs=('v_ref', 'x', 'y'),
outputs=('x_r', 'y_r', 'yaw_r', 'k_r' , 'v_r'))
###############################################################################
# Control
###############################################################################
# angle to -pi-pi
def pi_2_pi(angle):
return (angle + np.pi) % (2 * np.pi) - np.pi
def solve_DARE(A, B, Q, R):
"""
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X = Q
maxiter = 150
# Get the input vector
F1, F2, d = u
# Get the system response from the original dynamics
xdot, ydot, thetadot, xddot, yddot, thddot = \
pvtol_update(t, x, u[0:2], params)
# Now add the wind term
m = params.get('m', 4.) # mass of aircraft
xddot += d / m
return np.array([xdot, ydot, thetadot, xddot, yddot, thddot])
windy_pvtol = ct.NonlinearIOSystem(
windy_update, pvtol_output, name="windy_pvtol",
states = [f'x{i}' for i in range(6)],
inputs = ['F1', 'F2', 'd'],
outputs = [f'x{i}' for i in range(6)]
)
#
# PVTOL dynamics with noise and disturbances
#
def noisy_update(t, x, u, params):
# Get the inputs
F1, F2, Dx, Dy, Nx, Ny, Nth = u
# Get the system response from the original dynamics
xdot, ydot, thetadot, xddot, yddot, thddot = \
pvtol_update(t, x, u[0:2], params)
