def fitnessFunction(Kp, Ti, Td):
G = control.tf([Ti * Td, Ti, 1], [Ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
sys = control.feedback(control.series(G, F), 1)
y = control.step(sys)
yout = y[0]
t = y[-1]
# Integrated Square Error
error = 0
for val in y[0]:
error += (val - 1) * (val - 1)
# Overshoot
OS = (yout.max() / yout[-1] - 1) * 100
Tr = 0
Ts = 0
# Rising Time
for i in range(0, len(yout) - 1):
if yout[i] > yout[-1] * .90:
Tr = t[i] - t[0]
# Settling Time
for i in range(2, len(yout) - 1):
if abs(yout[-i] / yout[-1]) > 1.02:
Ts = t[len(yout) - i] - t[0]
return 1 / (OS + Tr + Ts + error * error * 10) * 100000
def fitnessFunction(Kp, Ti, Td):
G = control.tf([Ti*Td, Ti, 1], [Ti, 0])
F = control.tf(1,[1,6,11,6,0])
sys = control.feedback(control.series(G, F), 1)
y = control.step(sys)
yout = y[0]
t = y[-1]
# Integrated Square Error
error = 0
for val in y[0]:
error += (val - 1)*(val - 1)
# Overshoot
OS = (yout.max()/yout[-1]-1)*100
Tr = 0
Ts = 0
# Rising Time
for i in range(0, len(yout) - 1):
if yout[i] > yout[-1]*.90:
Tr = t[i] - t[0]
# Settling Time
for i in range(2, len(yout) - 1):
if abs(yout[-i]/yout[-1]) > 1.02:
Ts = t[len(yout) - i] - t[0]
return 1/(OS + Tr + Ts + error*error*10)*100000
def position():
vmax = 2
kl = 3.1
lmax = (vmax / kl)**2
lrange = np.arange(-lmax, lmax, 0.0001)[np.newaxis].T
vrange = (np.sign(lrange) * kl * (np.abs(lrange))**0.5)
k1 = np.linalg.pinv(lrange) @ vrange
plt.plot(lrange, vrange)
plt.plot(lrange, k1 * lrange)
#print(k1)
#plt.show()
#plt.clf()
Hs = ct.tf([0, 0, k1], [1, 0, 0])
#print(Hs)
Hz = ct.matlab.c2d(Hs, 1, method='zoh')
#print(Hz)
p = [-0.5, .8] #pole locations
z = [-0.5, .999] #zero loations
gain = 2e-7 #gain
freq = 0.001 #at frequency
Fs0 = 1 #sample rate
a, b = fn.generic_biquad(p, z, gain, freq, Fs0)
print('compensator', a, b)
Kz = ct.tf(b, a, 1)
a, b = fn.biquad_lowpass(0.01, 0.5, 20)
print('lowpass', a, b)
a, b = fn.biquad_lowpass(0.005, 0.707, 1)
Fz = ct.tf(b, a, 1)
ct.bode_plot(Hz * Kz * Fz, np.logspace(-4, -1, 1000))
ct.bode_plot(Hz * Kz, np.logspace(-4, -1, 1000))
plt.show()
plt.clf()
sys = ct.feedback(Hz * Kz * Fz, 1)
y, t = ct.step(sys, np.arange(0, 10000, 1))
plt.plot(t, y.T)
sys = ct.feedback(Hz * Kz, 1)
y, t = ct.step(sys, np.arange(0, 10000, 1))
plt.plot(t, y.T)
plt.show()
'''
def draw_step_response_feedback(K,sys):
# Defining feedback system
C = control.tf(K,1)
# Using feedback according to http://nl.mathworks.com/help/control/examples/using-feedback-to-close-feedback-loops.html
controled_sys = control.feedback(C*sys,1)
poles = control.pole(controled_sys)
y,t = control.step(controled_sys)
plt.plot(t,y)
plt.xlabel('t')
plt.ylabel('y(t)')
def update(self, t):
x_eq, u_eq = set_eq(t, self.x_eq_list, self.u_eq_list)
policy_params = dict(x_eq=x_eq, u_eq=u_eq, K=self.K)
self.x, self.u = step(self.x,
policy,
policy_params,
self.dynamics,
self.disturbance,
stepsize=stepsize,
method=step_method)
return self.draw_system(self.x, self.u, x_eq, u_eq, self.ax,
self.artists)
def reset(self):
self.invalid_action_reward = 0
self.current_episode_ticks = 0
for i in range(int(self.wait_before_start / Settings.TICK_LENGTH)):
control.step()
start_speed = control.get_ego_start_speed()
control.add_ego_car(start_speed)
control.step()
self.previous_acceleration = 0
state = prediction.HighwayState.from_sumo()
self.previous_state = state
self.speed_history = [start_speed]
self.control_history = []
self.position_history = [state.ego_position]
self.acceleration_history = [0]
self.jerk_history = [0]
self.state_history = [state]
self.crashed = False
self.merged = False
self.start_time = time.perf_counter()
return dqn.get_state_vector_from_base_state(state)
def stepresponse(Kp, Ti, Td):
G = Kp * control.tf([Ti * Td, Ti, 1], [Ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
system = control.feedback(control.series(G, F), 1)
t = numpy.arange(0, 100, 0.01)
y, T = control.step(system, t)
t_r, t_s, M_p = stepinfo(T, y)
ISE = sum(numpy.power(numpy.subtract(y, 1), 2))
return ISE, t_r, t_s, M_p
def step(self, action):
self.control_history.append(action)
self.current_episode_ticks += 1
self._do_action(action)
control.step()
if control.just_had_collision():
vector_state = np.zeros(self.observation_dim)
reward = self.reward_function(prediction.HighwayState.empty_state(), self.projected_jerk, True, False) + self.invalid_action_reward
self.crashed = True
return vector_state, reward, True, {}
elif control.ego_just_arrived():
vector_state = np.zeros(self.observation_dim)
self.merged = True
reward = self.reward_function(prediction.HighwayState.empty_state(), self.projected_jerk, False, True) + self.invalid_action_reward
return vector_state, reward, True, {}
elif self.current_episode_ticks >= self.max_episode_ticks:
state = prediction.HighwayState.from_sumo()
vector_state = dqn.get_state_vector_from_base_state(state)
current_acceleration = state.ego_acceleration
jerk = (current_acceleration - self.previous_acceleration) / Settings.TICK_LENGTH
reward = self.reward_function(state, jerk, False, False) + self.invalid_action_reward
control.remove_ego_car()
control.step()
return vector_state, reward, True, {}
else:
state = prediction.HighwayState.from_sumo()
vector_state = dqn.get_state_vector_from_base_state(state)
current_acceleration = state.ego_acceleration
jerk = (current_acceleration - self.previous_acceleration) / Settings.TICK_LENGTH
reward = self.reward_function(state, jerk, False, False) + self.invalid_action_reward
self.previous_state = state
self.speed_history.append(state.ego_speed)
self.position_history.append(state.ego_position)
self.acceleration_history.append(current_acceleration)
self.jerk_history.append(jerk)
self.state_history.append(state)
self.previous_acceleration = current_acceleration
return vector_state, reward, False, {}
"""
import numpy as np
import control as ctrl
import matplotlib.pyplot as plt
from equations import *
t0 = 0
tf = 50
n = 5000
sys_unstable = linear_equations(sp=1)
sys_stable = linear_equations(sp=1)
x_unstable, T = ctrl.step(sys_unstable,
np.linspace(t0, tf, n),
X0=[0, 0, 0, 0])
x_stable, T = ctrl.step(sys_stable, np.linspace(t0, tf, n))
fig, ax = plt.subplots(2, 2, figsize=(12, 12))
plt.suptitle('Stable linearized step response summary')
ax[0, 0].set_title('Cart position')
ax[0, 0].plot(np.linspace(t0, tf, len(x_unstable[:, 0])), x_unstable[:, 0],
'k-')
ax[0, 0].set_ylabel('Cart position [meters]')
ax[0, 0].set_xlabel('Time [seconds]')
ax[0, 0].grid()
ax[0, 1].set_title('Pendulum angle')
Q = np.eye(4, 4)
R = np.eye(2, 2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = " + str(np.linalg.matrix_rank(ct.ctrb(Ap, Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap, Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap, Bp, Cp, Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P, t)
#plt.plot(t,stepy)
#ct.impulse(ss_P)
ct.nyquist(ss_P)
#bode plot only implemented for SISO system.omg
#ct.bode(ss_P)
import julia
import matplotlib.pyplot as plt
import control
j = julia.Julia()
j.include("main.jl")
data = j.main("", 0.5, 4, 0.2, "PD")
redata, imdata = data["rlocus"]
tmp = data["Gdz"][0, 0]
n, d = scipy.zpk2tf(tmp.z, tmp.p, tmp, k)
sys = control.tf(n, d)
y, t, x = control.step(feedback(sys, 1))
print(y, t, x)
for i in range(0, len(redata[0])):
plt.plot(redata[:, i], imdata[:, i])
plt.show()
t = np.linspace(0, 3, 301)
# Define a matrix to hold all the responses. Defining it as all zeros and filling it
# later will be significantly faster for large problems and good practice otherwise
resp = np.zeros((len(t), len(damping)))
# loop through the values of damping ratio in the list damping, simluating the system
# and saving the response as column in resp
for index, zeta in enumerate(damping):
# Define the system for this damping ratio
num = wn**2
den = [1, 2 * zeta * wn, wn**2]
sys = control.tf(num, den)
resp[:, index], t_out = control.step(sys, t)
# Now, plot the various responses
# 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')
Stabilization in upward position
"""
import numpy as np
import control as ctrl
import matplotlib.pyplot as plt
from equations import *
sys_unstable = linear_equations(sp = 0)
(A, B, C, D) = ctrl.ssdata(sys_unstable)
K = ctrl.place(A, B, [-3.5, -3.6, -3.7, -3.8])
A_stable = A - np.dot(B,K)
sys_stable = ctrl.ss(A_stable, B, np.eye(4), np.zeros((4,1)))
t0 = 0; tf = 50; n = 5000;
x_stable, T = ctrl.step(sys_stable,np.linspace(t0,tf,n))
fig, ax = plt.subplots(2,2, figsize = (12,12))
plt.suptitle('Stable linearized step response summary')
ax[0,0].set_title('Cart position')
ax[0,0].plot(np.linspace(t0,tf,len(x_stable[:,0])),x_stable[:,0],'k-')
ax[0,0].set_ylabel('Cart position [meters]')
ax[0,0].set_xlabel('Time [seconds]')
ax[0,0].grid()
ax[0,1].set_title('Pendulum angle')
ax[0,1].plot(np.linspace(t0,tf,len(x_stable[:,2])),x_stable[:,2],'k-')
ax[0,1].set_ylabel('Pendulum angle [Radians]')
ax[0,1].set_xlabel('Time [seconds]')
This temporary script file is located here:
/home/ydzhao/.spyder2/.temp.py
"""
import numpy as np
import scipy as sp
import matplotlib.pyplot as plt
import control as control
A,B,C,D=0,1,1,0
agent_model=control.ss(A,B,C,D)
x0=0
x_init=x0
data_u=control.step(agent_model, T=None, X0=x_init, input=0, output=None)
x_init=data_u[0][-1]
t_init=data_u[1][-1]
data_u=control.step(agent_model, T=None, X0=x_init, input=0, output=None)
u=1
T, yout, xout=control.forced_response(agent_model, T=array([0,0.01]), U=1, X0=1)
plt.plot(data_u[1],data_u[0])
plt.show()
#x = np.array([[1, 2, 3], [4, 5, 6]], np.int32)
#T1 = np.arange(0.0, 0.02, 0.001) #NOK con step_response
#T = np.ndarray((1,T1.size), dtype=float)
#T[0,:] = T1
T1 = np.arange(0.0, 0.02, 0.001) #OK con step NOK con step_response
T = np.ndarray((T1.size, 1), dtype=float)
T[:, 0] = T1
#T1 = np.arange(0, 7, 1)
#T = np.ndarray((T1.size,1), dtype=float)
#T[:,0] = T1
#T, yout = ct.step_response(dsys1, T)
t, y = ct.step(dsys1, T)
plt.figure(2)
plt.clf()
plt.plot(omega, 20 * np.log10(abs(mag)), 'b')
plt.ylabel('Amplitude [dB]', color='b')
plt.xlabel('Frequency [rad/sample]')
plt.show()
#plt.figure(3)
#plt.clf()
#t2 = t[0, :]
#plt.plot(t2, y)
#plt.show()
plt.figure(3)
R = 17.2
km = 2.73E-2
J = 9.2E-7
n = 1/187.7
beta = 5.9E-7
# Matrizes do State-Space
A = numpy.matrix([[-R/L, -km/J],[km/L, ((-n**2)*beta)/J]])
B = numpy.matrix([[1], [0]])
C = numpy.matrix([[0, n/J]])
D = numpy.matrix([[0]])
sys = control.ss(A, B, C, D)
t = numpy.arange(0, 0.2, 0.001)
[Y, T] = control.step(sys, t)
wout = 24*Y # escalona a saida
u = ein_max*numpy.ones(len(t))
fig, ax1 = pyplot.subplots()
ax1.set_xlabel('time (s)')
ax1.plot(T, wout)
ax1.set_xlabel('time (s)')
ax1.set_ylabel('$\omega_{out}(t)$ (rad/s)')
ax2 = ax1.twinx()
# TODO: ajustar esse eixo para ser exibido adequadamente ...
ax2.set_ylabel('$e_{in} (t)$ (volts)')
pyplot.title("PMDC motor model")
* c : viscous damping coefficient [N s / m]
* k : spring constant [N / m]
* u : force input [N]
"""
k = 10.0
m = 3.0
c = 3.0
A = np.matrix([[0, 1], [-k / m, -c / m]])
B = np.matrix([[0], [1 / m]])
C = np.matrix([1, 0])
D = 0
sys = control.StateSpace(A, B, C, D)
clf()
suptitle("MSD responses")
(Ys, Ts) = control.step(sys, T=np.linspace(0, 20, 100))
(Yi, Ti) = control.impulse(sys, T=np.linspace(0, 20, 100))
subplot(211)
plot(Ts.T, Ys.T)
title('step')
ylabel('x')
xlabel('time')
subplot(212)
ylabel('x')
title('impulse')
plot(Ti.T, Yi.T)
show()
Q = np.eye(4,4)
R = np.eye(2,2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = "+ str(np.linalg.matrix_rank(ct.ctrb(Ap,Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap,Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap,Bp,Cp,Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P,t)
#plt.plot(t,stepy)
#ct.impulse(ss_P)
ct.nyquist(ss_P)
#bode plot only implemented for SISO system.omg
#ct.bode(ss_P)
