def loadBlocks(self, s):
for i in Workspace.blocks:
if i.type == 'input':
i.input = np.zeros(len(s['t']))
s['inputs'][i.id] = i
elif i.type == "function":
i.y = np.zeros(len(s['t']))
i.y.fill(np.nan)
s['functions'][i.id] = i
elif i.type == "sum":
i.y = np.zeros(len(s['t']))
i.y.fill(np.nan)
s['sums'][i.id] = i
elif i.type == 'system':
if i.code['type'] == "TF":
if i.code['sub_type'] == "continuous":
i.ss = c.ssdata(
c.c2d(
c.ss(
c.tf(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]))),
s['T']))
else:
i.ss = c.ssdata(
c.ss(
c.tf(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]), s['T'])))
elif i.code['type'] == "SS":
if i.code['sub_type'] == "continuous":
i.ss = c.ssdata(
c.c2d(
c.ss(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]),
json.loads(i.code['self'][2]),
json.loads(i.code['self'][3])), s['T']))
else:
i.ss = c.ssdata(
c.ss(json.loads(i.code['self'][0]),
json.loads(i.code['self'][1]),
json.loads(i.code['self'][2]),
json.loads(i.code['self'][3]), s['T']))
else:
Dialog.alert(
"Alerta",
["Um dos blocos não está configurado como TF ou SS"])
lt = len(s['t'])
lA = len(i.ss[0])
i.x = np.zeros((lt, lA, 1))
i.y = np.zeros(lt)
i.y.fill(np.nan)
s['systems'][i.id] = i
return s
def __init__(self, p=2, T=100, x_0=None):
if p == 2:
A = np.array([[1, 1], [1, 0]])
B = np.array([[0], [1]])
self.n = 2
self.m = 1
elif p == 12:
A = np.array([[-2.5, 1.2, 4.3, 0.1], [0.97, -10.3, 0.4, -6.1],
[-9.2, 1.1, -4.9, 0.3], [1.1, 0.9, -3.4, -0.9]])
B = np.array([[1.1, 0.4, -0.2], [-3.2, 1.4, 0.0], [-0.8, 0.1, 3.0],
[-1.1, -0.9, 5.2]])
self.n = 4
self.m = 3
C = np.eye(A.shape[0])
D = np.zeros((A.shape[0], B.shape[1]))
sysd = control.ss(A, B, C, D).sample(0.1)
self.A, self.B, _, _ = control.ssdata(sysd)
self.Q = np.eye(self.n)
self.R = np.eye(self.m)
self.T = T
self.x_0 = x_0
self.w_Sigma = 0.01 * np.eye(self.n)
self.v_Sigma = 0.01 * np.eye(self.n)
def initialize(self, param_value_dict):
self.k1 = param_value_dict["k1"]
self.k2 = param_value_dict["k2"]
self.luenberger_poles = param_value_dict["Luenberger poles"]
AB = np.array([[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0]])
BB = np.array([[0, 0],
[0, 0],
[0, 0],
[1, 0],
[0, 0],
[0, 1]])
CB = np.array([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
L = place(AB.T, CB.T, self.luenberger_poles).T
cont_observer_system = ss(AB - L @ CB, np.hstack((BB, L)), np.zeros((1, 6)), np.zeros((1, 6)))
# TODO: Remove hardcoded sample time
discrete_observer_system = sample_system(cont_observer_system, 1/60)
self.luenberger_Ad, self.luenberger_Bd, _, _ = ssdata(discrete_observer_system)
self.run_once = False
def LQRControl(plant, Q, R):
plantStateSpace = control.tf2ss(plant)
A, B, C, D = control.ssdata(plantStateSpace)
print(A, B, C, D)
K, P, E = control.lqr(plantStateSpace, Q, R)
output = control.ss(A - B * K, B * K, C, D)
return control.step_response(output, T)
def __init__(self, dt):
self.dt = dt
A, B = self.system()
C = np.eye(A.shape[0])
D = np.zeros((A.shape[0], B.shape[1]))
sys = control.ss(A, B, C, D)
sysd = sys.sample(dt)
A, B, C, D = control.ssdata(sysd)
self.A = A
self.B = B
def get_model(self):
Acont = np.array([[-1 / (self.cr * self.R), 1 / (self.cr * self.R)],
[1 / (self.cw * self.R), -2 / (self.cw * self.R)]])
Bcont = np.array([[1 / self.cr, 0], [0, 1 / (self.cw * self.R)]])
C = np.array([[1, 0]])
D = np.array([[0, 0]])
sys = control.ss(Acont, Bcont, C, D)
sysd = control.sample_system(sys, SIMULATION_TIME_STEP)
Adisc, Bdisc, Cdisc, Ddisc = control.ssdata(sysd)
return Adisc, Bdisc[:, 0], Bdisc[:, 1]
def linearize_system(infile: str) -> str:
with open(infile) as f:
data = load(f)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
config_dict = {
"outputs": [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz",
"ang_rate_x", "ang_rate_y", "ang_rate_z"
]
}
sys = build_nonlin_sys(aircraft, no_wind(), config_dict['outputs'], None)
actuator = actuator_from_dct(data['actuator'])
xeq = state.state
ueq = ctrl.control_input
aircraft_with_actuator = add_actuator(actuator, sys)
states_lin = np.concatenate((ueq, xeq))
linearized_sys = aircraft_with_actuator.linearize(states_lin, ueq)
A, B, C, D = ssdata(linearized_sys)
linsys = StateSpaceMatrices(A, B, C, D)
return linsys
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 12 18:48:36 2018
@author: eddardd
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]')
[time2, yout2] = con.step_response(t_function, time_simulation,
start_condition)
plt.figure(1)
plt.plot(time2, yout2, 'r')
plt.xlabel('time')
plt.ylabel('volts')
plt.figure(2)
plt.plot(t, y_out, 'b*', t, input_u,
'g-') #ploting time x output and time x input
plt.xlabel('time')
plt.ylabel('volts')
plt.show()
print("natural frequency = ", wn)
print("damping constant = ", zetas)
print("system poles = ", poles)
#take the transfer function and now getting an space state
[a, b, c, d] = con.ssdata(t_function)
state_space_2 = con.ss(a, b, c, d)
my_sys = con.minreal(state_space_2)
print("G(s) = ", t_function) #function transfer
print("###################")
print(my_sys) # state space
def test2():
if not test_ode:
return
m1 = 30 / 1000
l1 = 7.6 / 100
r1 = (5 - (10 - 7.6) / 2) / 100
w1 = 10 / 100
d1 = 2.4 / 100
J1 = m1 * (w1**2 + d1**2) / 12
m2 = 44 / 1000
w2 = 25.4 / 100
d2 = 2.4 / 100
J2 = m2 * (w2**2 + d2**2) / 12
r2 = (25.4 / 2 - 1.25) / 100
Jm = 0.004106
km = 0.006039
bm = 0.091503
g = 9.8
bPhi = 0
bTheta = 0
def MK(x, u):
theta, phi, thetaDot, phiDot = x
return (
np.array([[J2 + m2 * r2**2, m2 * r2 * l1 * math.cos(theta - phi)],
[
m2 * r2 * l1 * math.cos(theta - phi),
J1 + Jm + m1 * r1**2 + m2 * l1**2
]]),
np.array([
bTheta * thetaDot + m2 * r2 *
(g * math.sin(theta) + l1 * math.sin(theta - phi) * phiDot**2),
g * (m1 * r1 + m2 * l1) * math.sin(phi) -
m2 * r2 * l1 * math.sin(theta - phi) * thetaDot**2 +
(bm + bPhi) * phiDot - km * u[0]
]))
def ff(t, x, u):
M, K = MK(x, u)
return np.hstack((x[2:4], -la.solve(M, K)))
theta0, phi0 = 0 + math.pi / 6, 0
t0, x0, u0 = 0, np.array([theta0, phi0, 0, 0]), [0]
M, K = MK(x0, u0)
print(M)
print(K)
print(ff(t0, x0, u0))
sys = ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=ff)
tk = 5
uk = [0]
yk = sys.update(tk, uk)
print('1. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
from pyctrl import Controller
controller = Controller()
Ts = 0.01
controller.add_source('clock', Clock(), ['clock'])
condition = Map(function=lambda t: t < T)
controller.add_filter('condition', condition, ['clock'], ['is_running'])
controller.add_signals('tau', 'x')
controller.add_filter(
'ode',
TimeVaryingSystem(model=ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=ff)),
['clock', 'tau'], ['x'])
controller.add_sink('logger', Logger(), ['clock', 'x'])
controller.set_source('clock', reset=True)
T = 5 + Ts
controller.run()
log = controller.get_sink('logger', 'log')
t0 = log['clock'][0, 0]
tk = log['clock'][0, -1]
yk = log['x'][0, -1]
# yk = log[-1,1:]
print('2. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
import control
fc = 7
wc = 2 * math.pi * fc
lpf = control.tf(wc, [1, wc])
ctr = -2 * 100
def gg(t, x, u):
return [x[0]]
Ts = 0.01
Ac, Bc, Cc, Dc = map(np.array, control.ssdata(control.ss(lpf * ctr)))
nc = Ac.shape[0]
def F(t, x, ref):
x, xc = x[0:4], x[4:4 + nc]
y = ref - gg(t, x, [0])
u = max(-100, min(100, Cc.dot(xc) + Dc.dot(y)))
#print(ff(t,x,u))
return np.hstack((ff(t, x, u), Ac.dot(xc) + Bc.dot(y)))
eta = 0
kappa = 0
ref = np.array([eta * math.pi])
theta0 = -20 * math.pi / 180
xx0 = [kappa * math.pi - theta0, eta * math.pi, 0, 0]
xc0 = np.zeros((nc, ))
x0 = np.hstack((xx0, xc0))
t0 = 0
print('F = {}'.format(F(t0, x0, ref)))
sys = ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=F)
tk = 1
uk = np.array([0])
yk = sys.update(tk, uk)
print('1. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
controller.reset()
Ts = 0.01
controller.add_source('clock', Clock(), ['clock'])
condition = Map(function=lambda t: t < T)
controller.add_filter('condition', condition, ['clock'], ['is_running'])
controller.add_signals('ref', 'x')
controller.add_filter(
'ode',
TimeVaryingSystem(model=ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=F)),
['clock', 'ref'], ['x'])
controller.add_sink('logger', Logger(), ['clock', 'x'])
#print(controller.info('all'))
controller.set_source('clock', reset=True)
controller.set_signal('ref', ref)
T = 1 + Ts
controller.run()
log = controller.get_sink('logger', 'log')
t0 = log['clock'][0, 0]
tk = log['clock'][0, -1]
yk = log['x'][0, -1]
#yk = log[-1,1:]
print('2. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
Bc = np.array([[0, np.sqrt(3)/(2*M), -np.sqrt(3)/(2*M)],
[-1/M, 1/(2*M), 1/(2*M)],
[ b/J, b/J, b/J] ])*Be;
Cc = I
Dc = Z
Kc = np.diag([-Cv/M, -Cvn/M, -Cw/J])
Kf = np.diag([(-Fsv + Cv)/M, (-Fsvn + Cvn)/M, (-Fsw + Cw)/J])
nldat = Nldat(Kc=Kc, Kf=Kf)
Ts = 0.06
csys = control.ss(Ac, Bc, Cc, Dc)
dsys = control.c2d(csys, Ts, 'zoh')
[Ad, Bd, Cd, Dd] = control.ssdata(dsys)
#converts the wheel velocities to the states
W2S = np.array([[0, np.sqrt(3) * r / 3.0, -np.sqrt(3) * r / 3.0],
[-2.0 * r / 3.0, r / 3.0, r / 3.0],
[r / (3.0 * b), r / (3.0 * b), r / (3.0 * b)]])
#converts the states to the wheel velocities
S2W = np.array([[0, -1, b],
[np.sqrt(3)/2, 1/2, b],
[-np.sqrt(3)/2, 1/2, b]])/r
if __name__ == '__main__':
data = {'Ac': Ac, 'Bc': Bc, 'Cc': Cc, 'Dc': Dc}
savemat('data/cont_sys.mat', data)
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 10 15:39:20 2018
@author: eddardd
"""
import numpy as np
import control as ctrl
import matplotlib.pyplot as plt
from equations import *
sys_stable = linear_equations(sp=1)
sys_unstable = linear_equations(sp=0)
(A_unstable, _, _, _) = ctrl.ssdata(sys_unstable)
(A_stable, _, _, _) = ctrl.ssdata(sys_stable)
poles_unstable, _ = np.linalg.eig(A_unstable)
poles_stable, _ = np.linalg.eig(A_stable)
fig, ax = plt.subplots(1, 2, figsize=(14, 6), sharey=True)
plt.suptitle('Eigenvalues of linearized system')
ax[0].set_title(r'$\theta_{sp} = 0$')
ax[0].plot(poles_unstable, [0, 0, 0, 0], 'rx')
ax[0].set_xlabel('Real')
ax[0].set_ylabel('Imaginary')
ax[0].grid()
ax[0].axvline()
def test2():
m1 = 30/1000
l1 = 7.6/100
r1 = (5-(10-7.6)/2)/100
w1 = 10/100
d1 = 2.4/100
J1 = m1 * (w1**2 + d1**2) / 12
m2 = 44/1000
w2 = 25.4/100
d2 = 2.4/100
J2 = m2 * (w2**2 + d2**2) / 12
r2 = (25.4/2-1.25)/100
Jm = 0.004106
km = 0.006039
bm = 0.091503
g = 9.8
bPhi = 0
bTheta = 0
def MK(x,u):
theta, phi, thetaDot, phiDot = x
return (np.array([[J2+m2*r2**2, m2*r2*l1*math.cos(theta-phi)],
[m2*r2*l1*math.cos(theta-phi), J1+Jm+m1*r1**2+m2*l1**2]]),
np.array([bTheta*thetaDot+m2*r2*(g*math.sin(theta)+l1*math.sin(theta-phi)*phiDot**2),
g*(m1*r1+m2*l1)*math.sin(phi)-m2*r2*l1*math.sin(theta-phi)*thetaDot**2+(bm+bPhi)*phiDot-km*u[0]]))
def ff(t, x, u):
M, K = MK(x,u)
return np.hstack((x[2:4], -la.solve(M,K)))
theta0, phi0 = 0+math.pi/6, 0
t0, x0, u0 = 0, np.array([theta0,phi0,0,0]), [0]
M,K = MK(x0,u0)
print(M)
print(K)
print(ff(t0,x0,u0))
sys = ODE(t0 = t0, x0 = x0, f = ff)
tk = 5
uk = [0]
yk = sys.update(tk, uk)
print('1. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
from ctrl import Controller
controller = Controller()
Ts = 0.01
controller.add_source('clock',Clock(period = Ts),['clock'])
condition = Condition(lambda t : t < T)
controller.add_filter('condition',condition,['clock'],['is_running'])
controller.add_signals('tau','x')
controller.add_filter('ode',
TimeVarying(ODE(t0 = t0, x0 = x0, f = ff)),
['clock','tau'], ['x'])
controller.add_sink('logger',Logger(),['clock','x'])
controller.set_source('clock',reset=True)
T = 5 + Ts
controller.run()
log = controller.read_sink('logger')
t0 = log[0,0]
tk = log[-1,0]
yk = log[-1,1:]
print('2. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
import control
fc = 7
wc = 2 * math.pi * fc
lpf = control.tf(wc,[1,wc])
ctr = -2*100
def gg(t, x, u):
return [x[0]]
Ts = 0.01
Ac, Bc, Cc, Dc = map(np.array, control.ssdata(control.ss(lpf * ctr)))
nc = Ac.shape[0]
def F(t, x, ref):
x, xc = x[0:4], x[4:4+nc]
y = ref - gg(t,x,[0])
u = max(-100,min(100,Cc.dot(xc)+Dc.dot(y)))
#print(ff(t,x,u))
return np.hstack((ff(t,x,u), Ac.dot(xc)+Bc.dot(y)))
eta = 0
kappa = 0
ref = np.array([eta * math.pi])
theta0 = -20*math.pi/180
xx0 = [kappa*math.pi-theta0,eta*math.pi,0,0]
xc0 = np.zeros((nc,))
x0 = np.hstack((xx0,xc0))
t0 = 0
print('F = {}'.format(F(t0, x0, ref)))
sys = ODE(t0 = t0, x0 = x0, f = F)
tk = 1
uk = np.array([0])
yk = sys.update(tk, uk)
print('1. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
controller.reset()
Ts = 0.01
controller.add_source('clock',Clock(period = Ts),['clock'])
condition = Condition(lambda t : t < T)
controller.add_filter('condition',condition,['clock'],['is_running'])
controller.add_signals('ref','x')
controller.add_filter('ode',
TimeVarying(ODE(t0 = t0, x0 = x0, f = F)),
['clock','ref'], ['x'])
controller.add_sink('logger',Logger(),['clock','x'])
#print(controller.info('all'))
controller.set_source('clock',reset=True)
controller.set_signal('ref',ref)
T = 1 + Ts
controller.run()
log = controller.read_sink('logger')
t0 = log[0,0]
tk = log[-1,0]
yk = log[-1,1:]
print('2. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
def linearize(config: str,
out: str,
template: bool = False,
trim: bool = False):
"""
Linearize the model specified via the config file
"""
if template:
generate_linearize_template()
return
with open(config, 'r') as infile:
data = load(infile)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
iu = [2, 3]
config_dict = {
"outputs": [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz",
"ang_rate_x", "ang_rate_y", "ang_rate_z"
]
}
sys = build_nonlin_sys(aircraft, no_wind(), config_dict['outputs'], None)
idx = [2, 6, 7, 8, 9, 10, 11]
y0 = state.state
iy = [0, 1, 2, 5, 9, 10, 11]
xeq = state.state
ueq = ctrl.control_input
if trim:
print("Finding equillibrium point...")
xeq, ueq = find_eqpt(sys,
state.state,
u0=ctrl.control_input,
idx=idx,
y0=y0,
iy=iy,
iu=iu)
print("Equillibrium point found")
print()
print("Equilibrium state vector")
print(f"x: {xeq[0]: .2e} m, y: {xeq[1]: .2e} m, z: {xeq[2]: .2e} m")
print(
f"roll: {rad2deg(xeq[3]): .1f} deg, pitch: {rad2deg(xeq[4]): .1f} deg"
f", yaw: {rad2deg(xeq[5]): .1f} deg")
print(
f"vx: {xeq[6]: .2e} m/s, vy: {xeq[7]: .2e} m/s, vz: {xeq[8]: .2e} m/s"
)
print(
f"Ang.rates: x: {rad2deg(xeq[9]): .1f} deg/s, y: {rad2deg(xeq[10]): .1f} deg/s"
f", z: {rad2deg(xeq[11]): .1f} deg/s")
print()
print("Equilibrium input control vector")
print(
f"elevator: {rad2deg(ueq[0]): .1f} deg, aileron: {rad2deg(ueq[1]): .1f} deg"
f", rudder: {rad2deg(ueq[2]): .1f} deg, throttle: {ueq[3]: .1f}")
print()
actuator = actuator_from_dct(data['actuator'])
if isinstance(actuator, InputOutputSystem):
aircraft_with_actuator = add_actuator(actuator, sys)
states_lin = np.concatenate((ueq, xeq))
linearized = aircraft_with_actuator.linearize(states_lin, ueq)
else:
linearized = sys.linearize(xeq, ueq)
print("Linearization finished")
A, B, C, D = ssdata(linearized)
print("Found linear state space model on the form")
print()
print(" dx ")
print(" ---- = Ax + Bu")
print(" dt ")
print()
print("With observarion equation")
print()
print("y = Cx + Du")
print()
print("Eigenvalues of A:")
eig = np.linalg.eigvals(A)
print('\n'.join(f'{x:.2e}' for x in eig))
linsys = {
'A': A.tolist(),
'B': B.tolist(),
'C': C.tolist(),
'D': D.tolist(),
'xeq': xeq.tolist(),
'ueq': ueq.tolist()
}
if out is not None:
with open(out, 'w') as outfile:
json.dump(linsys, outfile, indent=2, sort_keys=True)
print(f"Linear model written to {out}")