def test_discrete(self):
"""Test discrete time functionality"""
# Create some linear and nonlinear systems to play with
linsys = ct.StateSpace(
[[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], [[0]], True)
lnios = ios.LinearIOSystem(linsys)
# Set up parameters for simulation
T, U, X0 = self.T, self.U, self.X0
# Simulate and compare to LTI output
ios_t, ios_y = ios.input_output_response(lnios, T, U, X0)
lin_t, lin_y, lin_x = ct.forced_response(linsys, T, U, X0)
np.testing.assert_array_almost_equal(ios_t, lin_t, decimal=3)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Test MIMO system, converted to discrete time
linsys = ct.StateSpace(self.mimo_linsys1)
linsys.dt = self.T[1] - self.T[0]
lnios = ios.LinearIOSystem(linsys)
# Set up parameters for simulation
T = self.T
U = [np.sin(T), np.cos(T)]
X0 = 0
# Simulate and compare to LTI output
ios_t, ios_y = ios.input_output_response(lnios, T, U, X0)
lin_t, lin_y, lin_x = ct.forced_response(linsys, T, U, X0)
np.testing.assert_array_almost_equal(ios_t, lin_t, decimal=3)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def feedback(tau: float, fb_gain: float) -> None:
"""Simulation of a negative feedback system, using the package 'control'.
'time', 'in_signal', 'num' and 'den' are taken from the global workspace
Parameters
----------
tau : time constant of a first order lag [sec]
fb_gain : feedback gain
"""
# First, define the feedforward transfer function
sys = control.TransferFunction(num, den)
print(sys) # 1 / (tau*s + 1)
# Simulate the response of the feedforward system
t_out, y_out, x_out = control.forced_response(sys, T=time, U=in_signal)
# Then define a feedback-loop, with gain fb_gain
sys_total = control.feedback(sys, fb_gain)
print(sys_total) # 1 / (tau*s + (1+k))
# Simulate the response of the feedback system
t_total, y_total, x_total = control.forced_response(sys_total,
T=time, U=in_signal)
# Show the signals
plt.plot(time, in_signal, '-', label='Input')
plt.plot(t_out, y_out, '--', label='Feedforward')
plt.plot(t_total, y_total, '-.', label='Feedback')
# Format the plot
plt.xlabel('Time [sec]')
plt.title(f"First order lag (tau={tau} sec)")
plt.text(15, 0.8, "Simulated with 'control' ", fontsize=12)
plt.legend()
def test_connect(self):
# Define a couple of (linear) systems to interconnection
linsys1 = self.siso_linsys
iosys1 = ios.LinearIOSystem(linsys1)
linsys2 = self.siso_linsys
iosys2 = ios.LinearIOSystem(linsys2)
# Connect systems in different ways and compare to StateSpace
linsys_series = linsys2 * linsys1
iosys_series = ios.InterconnectedSystem(
(iosys1, iosys2), # systems
((1, 0),), # interconnection (series)
0, # input = first system
1 # output = second system
)
# Run a simulation and compare to linear response
T, U = self.T, self.U
X0 = np.concatenate((self.X0, self.X0))
ios_t, ios_y, ios_x = ios.input_output_response(
iosys_series, T, U, X0, return_x=True)
lti_t, lti_y, lti_x = ct.forced_response(linsys_series, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
# Connect systems with different timebases
linsys2c = self.siso_linsys
linsys2c.dt = 0 # Reset the timebase
iosys2c = ios.LinearIOSystem(linsys2c)
iosys_series = ios.InterconnectedSystem(
(iosys1, iosys2c), # systems
((1, 0),), # interconnection (series)
0, # input = first system
1 # output = second system
)
self.assertTrue(ct.isctime(iosys_series, strict=True))
ios_t, ios_y, ios_x = ios.input_output_response(
iosys_series, T, U, X0, return_x=True)
lti_t, lti_y, lti_x = ct.forced_response(linsys_series, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
# Feedback interconnection
linsys_feedback = ct.feedback(linsys1, linsys2)
iosys_feedback = ios.InterconnectedSystem(
(iosys1, iosys2), # systems
((1, 0), # input of sys2 = output of sys1
(0, (1, 0, -1))), # input of sys1 = -output of sys2
0, # input = first system
0 # output = first system
)
ios_t, ios_y, ios_x = ios.input_output_response(
iosys_feedback, T, U, X0, return_x=True)
lti_t, lti_y, lti_x = ct.forced_response(linsys_feedback, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
def testSimulation(self):
T = range(100)
U = np.sin(T)
# For now, just check calling syntax
# TODO: add checks on output of simulations
tout, yout = step_response(self.siso_ss1d)
tout, yout = step_response(self.siso_ss1d, T)
tout, yout = impulse_response(self.siso_ss1d, T)
tout, yout = impulse_response(self.siso_ss1d)
tout, yout, xout = forced_response(self.siso_ss1d, T, U, 0)
tout, yout, xout = forced_response(self.siso_ss2d, T, U, 0)
tout, yout, xout = forced_response(self.siso_ss3d, T, U, 0)
def calc_average_perf(s, Gp, Gd, Kc, tauI, tauD, tauC, times):
Perf = np.linspace(0, 1, 100)
Gc = Kc * (1 + 1 / (tauI * s) + tauD * s / (tauC * s + 1))
sys_D = Gd / (1 + Gp * Gc)
average_Perf = []
for j in range(0, 100):
Ti = dfs[j]
_, Y, _ = control.forced_response(sys_D, times, Ti)
sys_U = Gc
_, U, _ = control.forced_response(sys_U, times, -Y)
Perf[j] = sum(abs(Y)) + 0.2 * sum(abs(U))
if sum(abs(Y) >= 5) >= 1 or sum(abs(U) >= 5) >= 1:
Perf[j] = 1e6
average_Perf = sum(Perf) / len(Perf)
return average_Perf
def test_nonsquare_bdalg(self):
# Set up parameters for simulation
T = self.T
U2 = [np.sin(T), np.cos(T)]
U3 = [np.sin(T), np.cos(T), T]
X0 = 0
# Set up systems to be composed
linsys_2i3o = ct.StateSpace(
[[-1, 1, 0], [0, -2, 0], [0, 0, -3]], [[1, 0], [0, 1], [1, 1]],
[[1, 0, 0], [0, 1, 0], [0, 0, 1]], np.zeros((3, 2)))
iosys_2i3o = ios.LinearIOSystem(linsys_2i3o)
linsys_3i2o = ct.StateSpace(
[[-1, 1, 0], [0, -2, 0], [0, 0, -3]],
[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
[[1, 0, 1], [0, 1, -1]], np.zeros((2, 3)))
iosys_3i2o = ios.LinearIOSystem(linsys_3i2o)
# Multiplication
linsys_multiply = linsys_3i2o * linsys_2i3o
iosys_multiply = iosys_3i2o * iosys_2i3o
lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U2, X0)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U2, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
linsys_multiply = linsys_2i3o * linsys_3i2o
iosys_multiply = iosys_2i3o * iosys_3i2o
lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U3, X0)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Right multiplication
# TODO: add real tests once conversion from other types is supported
iosys_multiply = ios.InputOutputSystem.__rmul__(iosys_3i2o, iosys_2i3o)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Feedback
linsys_multiply = ct.feedback(linsys_3i2o, linsys_2i3o)
iosys_multiply = iosys_3i2o.feedback(iosys_2i3o)
lin_t, lin_y, lin_x = ct.forced_response(linsys_multiply, T, U3, X0)
ios_t, ios_y = ios.input_output_response(iosys_multiply, T, U3, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Mismatch should generate exception
args = (iosys_3i2o, iosys_3i2o)
self.assertRaises(ValueError, ct.series, *args)
def y_map_function(self,iterator):
# If some combination of 20% done running, checkpoint to console
if iterator in self.milestones:
self.serialized_checkpoint(iterator)
# Create first PRBS outside of loop
u = self.uArray[iterator,:,:]
# The transfer function from the 2nd input to the 1st output is
# (3s + 4) / (6s^2 + 5s + 4).
# num = [[[1., 2.], [3., 4.]], [[5., 6.], [7., 8.]]]
# Iterate over each of the output dimensions and
# add to numerator
allY=np.zeros((self.nstep,self.outDim))
for j in range(0,self.outDim):
# Iterate over each of the input dimensions
# and add to the numerator array
b = [[self.b[iterator,self.outDim*j+i]] for i in range(self.inDim)]
a = [[1.,-self.a[iterator,self.outDim*j+i]] for i in range(self.inDim)]
q = [self.q[iterator,self.outDim*j+i] for i in range(self.inDim)]
bigU=np.transpose(u)
uSim=np.zeros_like(bigU)
for (idx,row) in enumerate(bigU):
uSim[idx,:] = np.concatenate((np.zeros(q[idx]),row[:-q[idx]]))
numTemp=np.array([b]);denTemp=np.array([a])
sys = control.tf(numTemp,denTemp,1)
_,y,_ = control.forced_response(sys,U=uSim,T=self.t)
allY[:,j] = self.gauss_noise(y,self.stdev)
return allY
def testMarkovSignature(self, matarrayout, matarrayin):
U = matarrayin([[1., 1., 1., 1., 1.]])
Y = U
m = 3
H = markov(Y, U, m, transpose=False)
Htrue = np.array([[1., 0., 0.]])
np.testing.assert_array_almost_equal(H, Htrue)
# Make sure that transposed data also works
H = markov(np.transpose(Y), np.transpose(U), m, transpose=True)
np.testing.assert_array_almost_equal(H, np.transpose(Htrue))
# Generate Markov parameters without any arguments
H = markov(Y, U, m)
np.testing.assert_array_almost_equal(H, Htrue)
# Test example from docstring
T = np.linspace(0, 10, 100)
U = np.ones((1, 100))
T, Y = forced_response(tf([1], [1, 0.5], True), T, U)
H = markov(Y, U, 3, transpose=False)
# Test example from issue #395
inp = np.array([1, 2])
outp = np.array([2, 4])
mrk = markov(outp, inp, 1, transpose=False)
# Make sure MIMO generates an error
U = np.ones((2, 100)) # 2 inputs (Y unchanged, with 1 output)
with pytest.raises(ControlMIMONotImplemented):
markov(Y, U, m)
def creation_example():
"""Example for creation of a FSR model with python-control."""
# creating a step response for our model
sys = ctrl.tf([1], [100, 1])
sim_duration = 2000
time = np.arange(sim_duration)
_, output = ctrl.step_response(sys, time)
# creating the model
model = ps.FsrModel(output, t=time)
# creating a test input signal
u = np.ones(sim_duration)
for i in range(len(u)):
if i < 500:
u[i] = 0.001 * i
# getting response from our model
t1, y1 = ps.forced_response(model, time, u)
# simulating a reference with control
t2, y2, _ = ctrl.forced_response(sys, time, u)
# show everything
ax = plt.subplot(111)
plt.plot(t1, y1, label="Model response")
plt.plot(t2, y2, label="Reference response")
plt.plot(t1, u, label="Input signal")
ax.legend()
plt.title("pystrem example")
plt.show()
def step_system(xopt, label, ax):
kp, tz1, tz2, tz3, tp = xopt
Co = kp * (tz1 * s + 1) * (tz2 * s + 1) * (tz3 * s +
1) / (s * s * (tp * s + 1))
To = control.minreal(Co * Go / (1 + Co * Go), verbose=False)
_, yout = control.forced_response(To, T, U=ramp, squeeze=True)
ax.plot(T, yout, label=label)
def err(self, params, div):
num = params[:div]
den = params[div:]
est_sys = control.tf(num, den)
tsim, ysim, xsim = control.forced_response(est_sys, T=self.t, U=self.u)
numpy.nan_to_num(self.history.append(sum((self.y - ysim)**2)))
return self.history[-1]
def solve(problem_spec):
""" solution of coprime decomposition
:param problem_spec: ProblemSpecification object
:return: solution_data: output value of the system and controller function
"""
s, t, T = sp.symbols("s, t, T")
transfer_func = problem_spec.transfer_func()
z_func, n_func = transfer_func.expand().as_numer_denom(
) # separate numerator and denominator
# tracking controller
# numerator and denominator of controller
cd_res = cd.coprime_decomposition(z_func, n_func, problem_spec.pol)
tf_k = (cd_res.f_func * z_func) / (cd_res.h_func * n_func) # open_loop
z_o, n_o = sp.simplify(tf_k).expand().as_numer_denom()
n_coeffs_o = [float(c) for c in st.coeffs(z_o, s)
] # coefficients of numerator of open_loop
d_coeffs_o = [float(c) for c in st.coeffs(n_o, s)
] # coefficients of denominator of open_loop
# feedback
close_loop = control.feedback(control.tf(n_coeffs_o, d_coeffs_o))
# simulate system with controller with initial error (190K instead of 200K).
y = control.forced_response(close_loop, problem_spec.tt, problem_spec.yr,
problem_spec.x0_1)
solution_data = SolutionData()
solution_data.yy = y[1]
solution_data.controller_n = cd_res.f_func
solution_data.controller_d = cd_res.h_func
solution_data.controller_ceoffs = cd_res.c_coeffs
return solution_data
def test_forced_response_legacy(self):
# Define a system for testing
sys = ct.rss(2, 1, 1)
T = np.linspace(0, 10, 10)
U = np.sin(T)
"""Make sure that legacy version of forced_response works"""
ct.config.use_legacy_defaults("0.8.4")
# forced_response returns x by default
t, y = ct.step_response(sys, T)
t, y, x = ct.forced_response(sys, T, U)
ct.config.use_legacy_defaults("0.9.0")
# forced_response returns input/output by default
t, y = ct.step_response(sys, T)
t, y = ct.forced_response(sys, T, U)
t, y, x = ct.forced_response(sys, T, U, return_x=True)
def main():
m = 1500 # Mass. Gives it a bit delay in the beginning.
k = 450 # Static gain. Tune so end values are similar to experimental data.
c = 240 # Time constant. Higher is slower. Damping.
# Plant transfer function. Static gain is numerator. Denominator is c * s + 1.
plant = control.tf([k], [m, c, 1])
plant_ss = control.ss(plant)
Kp = 2
Ki = 0
Kd = 1
# PID controller transfer function. C(s) = Kp + Ki/s + Kd s = (Kd s^2 + Kp s + Ki) / s
controller = control.tf([Kd, Kp, Ki], [1, 0])
sys = control.feedback(plant, controller)
# Plot the step responses
setpoint = 150
n = 100 # seconds
fig, ax = plt.subplots(tight_layout=True)
T = np.arange(0, n, 1) # 0 -- n seconds, in steps of 1 second
u = np.full(
n, fill_value=setpoint) # input vector. Step response so single value.
T, y_out, x_out = control.forced_response(sys, T, u)
ax.plot(T, y_out, label=str(setpoint))
ax.plot(T, x_out[0], label='x0')
ax.plot(T, x_out[1], label='x1')
ax.legend()
plt.show()
def graficado(valor_ajustado_de_la_deslizadera_del_tiempo, tamaño_impulso,
tamaño_escalon, tamaño_rampa, boton_pulsado, G0):
fig.clf()
retardo = eval(input_del_retardo_en_la_ventana.get())
if boton_pulsado == 'Impulso':
t, y = control.impulse_response(
G0 * tamaño_impulso,
T=(valor_ajustado_de_la_deslizadera_del_tiempo))
if boton_pulsado == 'Escalon':
t, y = control.step_response(
G0 * tamaño_escalon,
T=(valor_ajustado_de_la_deslizadera_del_tiempo))
if boton_pulsado == 'Rampa':
t, y = control.step_response(
G0 * tamaño_rampa / s,
T=(valor_ajustado_de_la_deslizadera_del_tiempo))
if boton_pulsado == 'Arbitraria':
t, y, xout = control.forced_response(G0,
T=(t_experimental),
U=(u_experimental))
t_a_dibujar, y_a_dibujar = ajusta_el_retardo_segun_el_input_de_la_ventana(
t, y, retardo)
fig.add_subplot(111).plot(t_a_dibujar, y_a_dibujar)
canvas.draw()
def speed_control(sys, ref_speeds, curr_speeds, init_values):
'''
Effects: get the reference speed, current (measured) speed and initial values
Use the difference
e = ref_speeds - curr_speeds
as the input for the PI controller, derive the new throttle
Parameters
----------
sys : control.ss
state space controller
ref_speeds : list of float
the desired speed we need
curr_speeds : list of float
the current speed
init_values : the initial_values of the system
DESCRIPTION.
Returns
-------
throttle : float type
DESCRIPTION.
'''
U0 = np.array(ref_speeds) - np.array(curr_speeds)
#print(U0)
_, y0, x0 = control.forced_response(
sys, U=U0,
X0=init_values[0]) # y0 is the next values, x0 is the state evolution
# see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response
init_values.append(x0[-1])
throttle = y0[-1]
return throttle, init_values
def test_nonlinear_iosys(self):
# Create a simple nonlinear I/O system
nlsys = ios.NonlinearIOSystem(predprey)
T = self.T
# Start by simulating from an equilibrium point
X0 = [0, 0]
ios_t, ios_y = ios.input_output_response(nlsys, T, 0, X0)
np.testing.assert_array_almost_equal(ios_y, np.zeros(np.shape(ios_y)))
# Now simulate from a nonzero point
X0 = [0.5, 0.5]
ios_t, ios_y = ios.input_output_response(nlsys, T, 0, X0)
#
# Simulate a linear function as a nonlinear function and compare
#
# Create a single input/single output linear system
linsys = self.siso_linsys
# Create a nonlinear system with the same dynamics
nlupd = lambda t, x, u, params: \
np.reshape(linsys.A * np.reshape(x, (-1, 1)) + linsys.B * u, (-1,))
nlout = lambda t, x, u, params: \
np.reshape(linsys.C * np.reshape(x, (-1, 1)) + linsys.D * u, (-1,))
nlsys = ios.NonlinearIOSystem(nlupd, nlout)
# Make sure that simulations also line up
T, U, X0 = self.T, self.U, self.X0
lti_t, lti_y, lti_x = ct.forced_response(linsys, T, U, X0)
ios_t, ios_y = ios.input_output_response(nlsys, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
def getGamma( lc ):
A,b = de.lstSqMultiPt( ThetaH[:,:], ThetaA.reshape((-1,1)), df[:,1:], (df[:,0]-.05).reshape((-1,1)), etaP.reshape((-1,))/lc, t, .0125 )[:2]
M = A.tocsr()
# preconditioning with Jacobi preconditioner
D = ss.dia_matrix( (1./np.power(M.multiply(M).sum(0),.5),0), (M.shape[1],M.shape[1]) )
G = (D*M.T*M*D).todense()
# Tikhonov matrix for regularization
gamma = D*((G)**-1*D*(M.T*b))
reg = co.coupledOde( Lambda.shape[1]-1, etaP.reshape((-1,)), lc, np.ones(Lambda.shape[1]-1), np.ones(1), np.ones(1)*.05, np.array(gamma[:A.shape[1]]).reshape((-1,)) )
sys = control.ss(reg.A().detach(),np.eye(reg.A().shape[0]),np.eye(reg.A().shape[0]),np.zeros(reg.A().shape))
ut = np.vstack(([[0]],np.array(gamma[A.shape[1]:]).reshape((-1,1))))
tp = np.hstack(([0],t))
x0 = np.ones((2236,1))
x0[-1] = 0.05
rv = control.forced_response(sys,np.arange(0,32+5e-2,5e-2),X0=x0)
reft,refft,header = ph.parseH( 'Min4data_2iso_0919.csv', do_squeeze=False )
reft = np.vstack([x.reshape((1,-1)) for x in reft])
refft = np.vstack([x.reshape((1,-1)) for x in refft])
residual = rv[1][:-1,(np.power( 2, [0,2,3,4,5] )/5e-2).astype(np.int)] - refft[:,list(range(1,refft.shape[1]))+[0]].T
mask = (1 - np.isnan(residual)).nonzero()
flatResidual = residual[mask]
return gamma,flatResidual,rv
def speed_control(env, sys, ref_speeds, curr_speeds, init_values):
'''
Parameters
----------
env : CARLA_ENV
DESCRIPTION.
sys : control.ss
speed controller
ref_speeds : list of float
the desired speed we need
curr_speeds : list of float
the current speed
init_values : the initial_values of the system
DESCRIPTION.
Returns
-------
throttle : float type
DESCRIPTION.
'''
U0 = np.array(ref_speeds) - np.array(curr_speeds)
#print(U0)
_, y0, x0 = control.forced_response(sys, U=U0, X0=init_values[0])
init_values.append(x0[-1])
throttle = y0[-1]
return throttle, init_values
def test_import_export(self):
sys = ctrl.tf([1], [100, 1])
sim_duration = 2000
time = np.zeros(sim_duration)
i = 0
while (i < sim_duration):
time[i] = i
i += 1
_, output = ctrl.step_response(sys, time)
m = ps.FsrModel(output, t=time)
# testing our model with a test signal
test_sgnl_len = int(2500)
u = np.zeros(test_sgnl_len)
for i in range(test_sgnl_len):
u[i] = 1
time = np.zeros(test_sgnl_len)
for i in range(test_sgnl_len):
time[i] = i
_, comp, _ = ctrl.forced_response(sys, time, u)
# 'rb' and 'wb' to avoid problems under Windows!
with open("temp", "w") as fh:
ps.export_csv(m, fh)
with open("temp", "r") as fh:
m = ps.import_csv(fh)
os.remove("temp")
_, y = ps.forced_response(m, time, u)
self.assertTrue(np.allclose(y, comp, rtol=1e-2), "import/export broke")
def get_reference_speed(distance_sys,ref_distance, curr_distance, init_values):
'''
Parameters
----------
distance_sys : control.ss
state space controller.
ref_distance : float
the reference distance.
curr_distance : float
current distance between two vehicles.
init_values : the initial_values of the system
Returns
-------
None.
'''
U0 = np.array(ref_distance) - np.array(curr_distance)
print(U0)
_,y0,x0 = control.forced_response(distance_sys,U = U0,X0 = init_values[0]) # y0 is the next values, x0 is the state evolution
# see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response
init_values.append(x0[-1])
ref_speed = y0[-1]
return ref_speed, init_values
def test_double_integrator(self):
# Define a second order integrator
sys = ct.StateSpace([[-1, 1], [0, -2]], [[0], [1]], [[1, 0]], 0)
flatsys = fs.LinearFlatSystem(sys)
# Define the endpoints of a trajectory
x1 = [0, 0]; u1 = [0]; T1 = 1
x2 = [1, 0]; u2 = [0]; T2 = 2
x3 = [0, 1]; u3 = [0]; T3 = 3
x4 = [1, 1]; u4 = [1]; T4 = 4
# Define the basis set
poly = fs.PolyFamily(6)
# Plan trajectories for various combinations
for x0, u0, xf, uf, Tf in [
(x1, u1, x2, u2, T2), (x1, u1, x3, u3, T3), (x1, u1, x4, u4, T4)]:
traj = fs.point_to_point(flatsys, x0, u0, xf, uf, Tf, basis=poly)
# Verify that the trajectory computation is correct
x, u = traj.eval([0, Tf])
np.testing.assert_array_almost_equal(x0, x[:, 0])
np.testing.assert_array_almost_equal(u0, u[:, 0])
np.testing.assert_array_almost_equal(xf, x[:, 1])
np.testing.assert_array_almost_equal(uf, u[:, 1])
# Simulate the system and make sure we stay close to desired traj
T = np.linspace(0, Tf, 100)
xd, ud = traj.eval(T)
t, y, x = ct.forced_response(sys, T, ud, x0)
np.testing.assert_array_almost_equal(x, xd, decimal=3)
def test_IT2_forced_response(self):
sys = ctrl.tf([100], [50, 1000, 150, 0]) # IT2
_, o = ctrl.step_response(sys, self.time)
m = ps.FsrModel(o, t=self.time, optimize=False)
_, y = ps.forced_response(m, self.t, self.u)
_, o, _ = ctrl.forced_response(sys, self.t, self.u)
self.assertTrue(np.allclose(y, o, rtol=1e-2), "it2 response broke")
def test_PT2_forced_response(self):
sys = ctrl.tf([1], [1000, 10, 1]) # PT2 with overshooting
_, o = ctrl.step_response(sys, self.time)
m = ps.FsrModel(o, t=self.time)
_, y = ps.forced_response(m, self.t, self.u)
_, o, _ = ctrl.forced_response(sys, self.t, self.u)
self.assertTrue(np.allclose(y, o, rtol=1e-2), "pt2 response broke")
def fun(iterator):
sys = control.tf([
np.random.randint(10),
], [np.random.randint(10), 1.])
_, yEnd, _ = control.forced_response(sys,
U=uArray[iterator, :],
T=np.linspace(0, 100, 100))
return yEnd
def make_step(sys, u, t):
ti, yi = control.forced_response(sys,
U=u,
T=[PrevStep.t, t],
X0=PrevStep.y * PLANT_DENOMINATOR[0])
PrevStep.t = ti[-1]
PrevStep.y = yi[-1]
return PrevStep.t, PrevStep.y
def __init__(self, sys, sid):
self.sid = sid
self.sys = sys
self.time = [0,1,2,3,4,5]
self.u = [0,0,0,0,0,0]
self.count = len(self.time)
T, yout, _ = control.forced_response(self.sys, self.time, self.u)
socketio.emit('process', {'T': T.tolist(), 'yout': yout.tolist(), 'uout': self.u}, room=self.sid)
def test_bdalg_functions(self):
"""Test block diagram functions algebra on I/O systems"""
# Set up parameters for simulation
T = self.T
U = [np.sin(T), np.cos(T)]
X0 = 0
# Set up systems to be composed
linsys1 = self.mimo_linsys1
linio1 = ios.LinearIOSystem(linsys1)
linsys2 = self.mimo_linsys2
linio2 = ios.LinearIOSystem(linsys2)
# Series interconnection
linsys_series = ct.series(linsys1, linsys2)
iosys_series = ct.series(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_series, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Make sure that systems don't commute
linsys_series = ct.series(linsys2, linsys1)
lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
self.assertFalse((np.abs(lin_y - ios_y) < 1e-3).all())
# Parallel interconnection
linsys_parallel = ct.parallel(linsys1, linsys2)
iosys_parallel = ct.parallel(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_parallel, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_parallel, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Negation
linsys_negate = ct.negate(linsys1)
iosys_negate = ct.negate(linio1)
lin_t, lin_y, lin_x = ct.forced_response(linsys_negate, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_negate, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Feedback interconnection
linsys_feedback = ct.feedback(linsys1, linsys2)
iosys_feedback = ct.feedback(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_feedback, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_feedback, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def test_all_pass_forced_response(self):
sys = ctrl.tf([-50, 1], [1000, 10, 1]) # all-pass
_, o = ctrl.step_response(sys, self.time)
m = ps.FsrModel(o, t=self.time)
_, y = ps.forced_response(m, self.t, self.u)
_, o, _ = ctrl.forced_response(sys, self.t, self.u)
self.assertTrue(np.allclose(y, o, rtol=1e-1),
"all-pass response broke")
def step(self, action, sys):
# Update time and action vectors
self.time.append(self.count)
self.u.append(action)
# Evaluate and send response
T, yout, uout = control.forced_response(self.sys, self.time, self.u)
socketio.emit('process', {'T': T[-1].tolist(), 'yout': yout[-1].tolist(), 'uout': self.u[-1]}, room=self.sid)
self.inc_count()
def showStepResponse(self, ax=0):
T, yout, xout = control.forced_response(self.sys, self.time, [functions.step(self.time)])
if ax != 0:
ax.plot(T,yout, label='step response')
ax.legend()
else:
fig, ax1 = plt.subplots(nrows=1, ncols=1)
ax1.plot(T,yout, label='step response')
ax1.legend()
plt.show()
def showTimeResponse(self, ax=0):
T, yout, xout = control.forced_response(self.sys, self.time, self.fct(self.time))
if ax != 0:
ax.plot(T,yout, label='time response')
ax.legend()
else:
fig, ax1 = plt.subplots(nrows=1, ncols=1)
ax1.plot(T,yout, label='time response')
ax1.legend()
plt.show()
#--------- Uncomment below to compare the response to the exact function with --------
#--------- our n-term fourier approximation --------
#
wn = (2*pi)**2
z = 0.1
# Define the system to use in simulation - in transfer function form here
num = [2.*z*wn,wn**2];
den = [1,2.*z*wn,wn**2];
sys = control.tf(num,den);
# run the simulation - first with the exact input
[T_out,yout_exact,xout_exact] = control.forced_response(sys,t,y)
# run the simulation - utilize the built-in initial condition response function
[T_approx,yout_approx,xout_approx] = control.forced_response(sys,t,approx)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = figure(figsize=(6,4))
ax = gca()
subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18)
setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
# ltext = leg.get_texts()
# plt.setp(ltext,fontsize=18)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# save the figure as a high-res pdf in the current folder
# plt.savefig('mpc_command.pdf')
# show the figure
plt.show()
# Now, let's verify the results using a full simulation of the model
x0 = np.array([[x1_init],[x1_dot_init]])
time_out, y_out, x_out = control.forced_response(sys, time, u_total, x0)
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96)
# Change the axis units font
plt.setp(ax.get_ymajorticklabels(),fontsize=18)
plt.setp(ax.get_xmajorticklabels(),fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ki = 10 # The integral gain # Define the closed-loop transfer function numPID = [kd,kp,ki] denPID = [m,(c + kd),(k + kp),ki] sysPID = control.tf(numPID,denPID) # Let's start it at t=0.5s for clarity in plotting Xd = zeros(500) # Define an array of all zeros Xd[50:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s) Xd[200:] = 0.6 # Make the elements after 2.5s = 0.5 # run the simulation - utilize the built-in forced_response function [T_PID,yout_PID,xout_PID] = control.forced_response(sysPID,t,Xd) # Calculate the error terms error = Xd - yout_PID error_deriv = r_[0,diff(error)] error_int = trapz(error,T_PID) # Calculate the "derivative on measurement" term error_measurement = -r_[0,diff(yout_PID)] # Calculate the force input (the output of the PID controller) force = kp*error + ki*error_int + kd*error_deriv # Direct application # Force from "derivative on measurement" method force_measurement = kp*error + ki*error_int + kd*error_measurement
ylabel('Force (N)',family='CMU Serif',fontsize=22,weight='bold',labelpad=10)
# xlim(0,4)
ylim(-1.25,1.25)
plot(t,F,color='blue',linewidth=2,label='Response')
# uncomment below save the figure as a high-res pdf in the current folder
savefig('BangCoastBang_Force.png')
#show the figure
#show()
# run the simulation - utilize the built-in forced_response function
[T,yout,xout] = control.forced_response(sys,t,F,hmax=0.01)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = figure(figsize=(6,4))
ax = gca()
subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18)
setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
sampling_multiple = dt / new_dt
# Define the new time vector
time = np.arange(0, stop_time, new_dt)
sampling_offset = np.ones(int(sampling_multiple),)
u_newDt = np.kron(u_total, sampling_offset)
u_newDt = u_newDt[int(sampling_multiple):]
# Convert the system to digital using the faster sampling rate.
new_digital_sys = control.sample_system(sys, new_dt)
# Now, simulate the systema at the new higher sampling rate
t_out, y_out, x_out = control.forced_response(new_digital_sys, time, u_newDt)
# I'm including a message here, so that I can tell from the terminal when it's
# done running. Otherwise, the plot windows tend to end up hidden behind others
# and I have to dig around to get them.
input("\nDone solving... press enter to plot the results.")
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96)
# Change the axis units font
plt.setp(ax.get_ymajorticklabels(),fontsize=18)
plt.setp(ax.get_xmajorticklabels(),fontsize=18)
# Set up simulation parameters
t = r_[0:5:500j] # time for simulation, 0-5s with 500 points in-between
# ----- Open-Loop input the step response -----------------------------------------------
#
# Define the open-loop system to use in simulation - in transfer function form here
num = [1]
den = [m, c, k]
sys = control.tf(num, den)
# Let's start it at t=0.5s for clarity in plotting
F = zeros(500) # Define an array of all zeros
F[25:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s)
# run the simulation - utilize the built-in forced_response function
[T, yout, xout] = control.forced_response(sys, t, F)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = figure(figsize=(6, 4))
ax = gca()
subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96)
setp(ax.get_ymajorticklabels(), family="CMU Serif", fontsize=18)
setp(ax.get_xmajorticklabels(), family="CMU Serif", fontsize=18)
ax.spines["right"].set_color("none")
ax.spines["top"].set_color("none")
ax.xaxis.set_ticks_position("bottom")
ax.yaxis.set_ticks_position("left")
ax.grid(True, linestyle=":", color="0.75")
ax.set_axisbelow(True)
def input_sim(self,u=0,T_intval=0.01):
T, yout, xout=control.forced_response(self.agent_model, np.array([0,T_intval]), u, self.state)
self.state=yout[-1]
den = [1.,0.,wn**2];
sys = control.tf(num,den);
# Set up simulation parameters
t = r_[0:5:0.001] # time for simulation, 0-5s with 500 points in-between
#----- Look at a step response --------------------------------------------------------
# Let's start it at t=0.5s for
U = zeros(5000) # Define an array of all zeros
U[500:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s)
# run the simulation - utilize the built-in initial condition response function
[T_un,yout_un,xout_un] = control.forced_response(sys,t,U)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = figure(figsize=(6,4))
ax = gca()
subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18)
setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
ylabel('Force (N)',family='CMU Serif',fontsize=22,weight='bold',labelpad=10)
xlim(0,4)
ylim(-1.25*fmax,1.25*fmax)
plot(t,F,color='blue',linewidth=2,label='Response')
# uncomment below save the figure as a high-res pdf in the current folder
savefig('BangBang_Force_Flexible.png')
#show the figure
#show()
# run the simulation - utilize the built-in forced_response function
[T,yout,xout] = control.forced_response(sys,t,F)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = figure(figsize=(6,4))
ax = gca()
subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18)
setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
#
# The next thing that may be of interest is the step response of the system. I know that the system is unstable but the step response can possibly reveal other information. I will use the control toolbox's `forced_response` function so that we can control the magnitude of the step input. We will simulate the system for 5 seconds and set a step input of 2 degrees.
# In[5]:
time = np.linspace(0.0, 5.0, num=1001)
# In[6]:
delta = np.deg2rad(2.0) * np.ones_like(time)
# In[7]:
time, theta, state = cn.forced_response(theta_delta, T=time, U=delta)
# Now, I'll create a reusable function for plotting a SISO input/output time history.
# In[8]:
def plot_siso_response(time, input, output, title='Time Response',
x_lab='Time [s]', x_lim=None,
input_y_lab='Input', input_y_lim=None,
output_y_lab='Output', output_y_lim=None,
subplots=True):
"""Plots a time history of the input and output of a SISO system."""
xaxis = gr.XAxis(title=x_lab, range=x_lim)
plot(T,yout,color="blue",linewidth=2)
# save the figure as a high-res pdf in the current folder
savefig('Step_Resp.pdf')
# show the figure
show()
#----- We could also manually input the step response -----
# Let's start it at t=0.5s for
U = np.zeros(500) # Define an array of all zeros
U[50:] = 1 # Make all elements of this array index>50 = 1 (all after 0.5s)
# run the simulation - utilize the built-in initial condition response function
[T_man,yout_man,xout_man] = control.forced_response(sys,t,U)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = figure(figsize=(6,4))
ax = gca()
subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
setp(ax.get_ymajorticklabels(),family='CMU Serif',fontsize=18)
setp(ax.get_xmajorticklabels(),family='CMU Serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)