def sympy_to_control(tf):
s = sympy.symbols('s')
num, den = sympy.fraction(tf)
num = sympy.poly(num, s)
den = sympy.poly(den, s)
n = np.array(num.all_coeffs(), dtype=float)
d = np.array(den.all_coeffs(), dtype=float)
return control.tf(n, d)
def rlocus(tf, *args, **kwargs):
return control.rlocus(sympy_to_control(tf), *args, **kwargs)
def step_response(tf, *args, **kwargs):
return control.step_response(sympy_to_control(tf), *args, **kwargs)
def forced_response(tf, *args, **kwargs):
return control.forced_response(sympy_to_control(tf), *args, **kwargs)
def wrap_deg(angle):
while angle > 180:
angle -= 360
while angle < -180:
angle += 360
return angle
def zpk(tf):
s = sympy.symbols('s')
num, den = sympy.fraction(tf)
num = sympy.poly(num, s)
den = sympy.poly(den, s)
zeros = sympy.roots(num, multiple=True)
poles = sympy.roots(den, multiple=True)
Gzp = 1
for z in zeros:
Gzp *= (s - z)
for p in poles:
Gzp /= (s - p)
k = sympy.simplify(tf/Gzp)
return zeros, poles, k
def root_locus_problem(G):
s = sympy.symbols('s')
zeros, poles, gain = zpk(G)
print('\n====================================')
print('Part 1')
print('zeros:', zeros)
print('poles:', poles)
print('\n====================================')
print('Part 2')
n = len(poles)
m = len(zeros)
num_asymptotes = n - m
print('num asymptotes:', n-m)
if gain > 0:
angle_start = 180
else:
angle_start = 0
asymptotes = np.array([ wrap_deg((angle_start + 360*j)/num_asymptotes) for j in range(num_asymptotes)])
print('asymptotes deg:', asymptotes)
centroid = (np.sum(poles) -np.sum(zeros))/(n - m)
print('centroid', centroid)
print('\n====================================')
print('Part 3: Break away/in')
K = sympy.symbols('K', real=True)
f_K = sympy.solve(K*G + 1)[0][K]
break_away_in = sympy.solve(f_K.diff(s))
print('candidate break awak/in', break_away_in)
print('\n====================================')
print('Part 4: Imaginary Crossing')
omega = sympy.symbols('omega', real=True)
G_jw = G.subs(s, sympy.I*omega).expand(complex=True).collect(sympy.I)
print('re(G(j omega)):', sympy.re(K*G_jw).simplify(), '= -1')
print('im(G(j omega)):', sympy.im(K*G_jw).simplify(), '= 0')
omega = sympy.symbols('omega', real=True)
print(sympy.solve(sympy.im((K*G).subs(s, sympy.I*omega)), omega))
sol_omega = np.array(sympy.solve(sympy.im((K*G).subs(s, sympy.I*omega)), omega), dtype=float)
try:
omega_val = sol_omega[np.argwhere(sol_omega > 0)[0][0]]
print('omega:', omega_val)
K_val = sympy.solve(sympy.re((K*G_jw)).subs(omega, omega_val).simplify() + 1, K)[0]
print('K:', K_val)
except IndexError as e:
print(e)
print('no imaginary crossing')
print('\n====================================')
print('Part 5: Angle of Arrival/ Departure')
arrival = {}
departure = {}
for i_s, s in enumerate(zeros):
arrival[s] = angle_start
for i_p, p in enumerate(poles):
arrival[s] += wrap_deg(np.rad2deg(np.angle(complex(s - p))))
for i_z, z in enumerate(zeros):
if i_z == i_s:
continue
arrival[s] -= wrap_deg(np.rad2deg(np.angle(complex(s - z))))
for i_s, s in enumerate(poles):
departure[s] = angle_start
for i_p, p in enumerate(poles):
if i_p == i_s:
continue
departure[s] -= wrap_deg(np.rad2deg(np.angle(complex(s - p))))
for i_z, z in enumerate(zeros):
departure[s] += wrap_deg(np.rad2deg(np.angle(complex(s - z))))
for k in arrival.keys():
if arrival[k] > 180:
arrival[k] -= 360
for k in departure.keys():
if departure[k] > 180:
departure[k] -= 360
# account for repeated poles/zeros
num, den = sympy.fraction(G)
zeros_mult = sympy.roots(num)
poles_mult = sympy.roots(den)
print(zeros_mult)
print(poles_mult)
for p in poles_mult.keys():
n = poles_mult[p]
departure[p] /= n
for z in zeros_mult.keys():
n = zeros_mult[z]
arrival[z] /= n
print('poles', poles)
print('zeros', zeros)
print('arrival: ', arrival)
print('departure: ', departure)
print('\n====================================')
print('Part 6: Draw Root Locus')
rlocus(G)