In [1]:
#%matplotlib notebook
#DEFAULT_FIGSIZE = (8, 6)
%matplotlib inline
DEFAULT_FIGSIZE = (12, 8)
import numpy as np
import scipy.signal
import matplotlib.pyplot as plt
import seaborn as sns
sns.set_style('darkgrid')
from dtk.bicycle import benchmark_state_space_vs_speed, benchmark_matrices
import control
import plot_sim as ps
import matplotlib as mpl
mpl.rcParams['figure.figsize'] = DEFAULT_FIGSIZE
In [2]:
def rudinshapiro(N):
"""
Return first N terms of Rudin-Shapiro sequence
https://en.wikipedia.org/wiki/Rudin-Shapiro_sequence
Confirmed correct output to N = 10000:
https://oeis.org/A020985/b020985.txt
"""
def hamming(x):
"""
Hamming weight of a binary sequence
http://stackoverflow.com/a/407758/125507
"""
return bin(x).count('1')
out = np.empty(N, dtype=int)
for n in range(N):
b = hamming(n << 1 & n)
a = (-1)**b
out[n] = a
return out
s = rudinshapiro(10)
print(s)
np.array(s > 0).astype(float) * np.pi
Out[2]:
In [3]:
plt.close('all')
t = np.arange(0, 10, 0.001)
dt = 0.001
n = int(10/dt)
def multisine(frequencies):
n = len(frequencies)
seq = np.array(rudinshapiro(n) > 0).astype(float)
print('length', n)
print('frequencies', ', '.join(str(f) for f in frequencies))
print('rs sequence', ', '.join(str(s) for s in seq))
u = np.zeros(t.shape)
amplitude = 0.3
for f, s in zip(frequencies, seq):
period = 1/f
if (10/period).is_integer:
print('{} periods for frequency {}'.format(10/period, f))
u += amplitude*np.sin(2*np.pi*f*t + s*np.pi)
return u
u_freq = np.arange(0.2, 10.2, 0.2)
u = multisine(u_freq)
x = np.fft.fft(u)
freq = np.fft.fftfreq(n, dt)
index = (freq > 0) & (freq < 20)
fig, ax = plt.subplots(2, 1)
ax[0].plot(t, u)
ax[1].stem(freq[index], np.abs(x[index]))
plt.show()
In [4]:
def plot_response(log):
t = log.t
u = log.records.input[:, 1] # steer torque
v = log.kollmorgen_command_torque
r = log.states[:, 1] # reference steer angle
y = log.measured_steer_angle
fig, ax = plt.subplots(2, 1, sharex=True)
ax[0].plot(t, u,
label='steer torque')
ax[0].plot(t, v,
label='commanded motor torque')
ax[0].plot(t, log.kollmorgen_applied_torque,
label='actual motor torque')
ax[0].legend()
ax[0].set_ylabel('torque [N-m]')
ax[1].plot(t, r * 180/np.pi,
label='reference encoder angle')
ax[1].plot(t, y * 180/np.pi,
label='measured encoder angle')
ax[1].legend()
ax[1].set_ylabel('angle [deg]')
ax[1].set_xlabel('time [s]')
return fig, ax
plt.close('all')
log = ps.ProcessedRecord('logs/multisine.pb.cobs.gz')
plot_response(log)
plt.show()
In [5]:
# simulate model response to multisine 'u'
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), [5])
A = A[0]
B = B[0]
Bi = B[:, [1]] # steer torque
Ci = np.array([[0, 1, 0, 0]]) # steer angle
D = np.array([[0]])
sys = scipy.signal.StateSpace(A, Bi, Ci, D)
tf = scipy.signal.TransferFunction(sys)
# simulate for 20 seconds
lsim_t = np.arange(0, 20, dt)
lsim_u = np.concatenate((u, u))
_, lsim_y, lsim_x = scipy.signal.lsim(sys, lsim_u, lsim_t)
In [6]:
plt.close('all')
fig, ax = plt.subplots(2, 1, sharex=True)
color = sns.color_palette('Paired', 12)[1::2]
log_u = log.records.input[:, 1] # steer torque
log_r = log.states[:, 1] # reference steer angle
ax[0].plot(lsim_t, lsim_u,
color=color[1],
linestyle='--',
label='python lsim steer torque')
ax[0].plot(log.t, log_u,
color=color[2],
alpha=0.7,
label='simulator steer torque')
ax[0].set_ylabel('torque [Nm]')
ax[0].legend()
ax[1].plot(lsim_t, lsim_y,
color=color[1],
linestyle='--',
label='python lsim steer angle')
ax[1].plot(log.t, log_r,
color=color[2],
alpha=0.7,
label='simulator steer angle')
ax[1].set_ylabel('angle [deg]')
ax[1].legend()
ax[1].set_xlabel('time [s]')
plt.show()
In [7]:
def frequency_response(input_signal, output_signal, last_n=None):
if last_n is not None:
assert last_n > 0
last_n = -last_n
index = slice(last_n, None)
U = np.fft.fft(input_signal[index])
Y = np.fft.fft(output_signal[index])
Suu = U*np.conj(U)
Syu = Y*np.conj(U)
return Syu/Suu
# C = np.power(np.abs(Syu), 2)/(Suu * Syy) # coherence
In [13]:
def plot_frequency_response(log, model_compare=False):
#color = sns.color_palette('deep', 6)
color = sns.color_palette('Paired', 12)[1::2]
log_u = log.records.input[:, 1] # steer torque
log_r = log.states[:, 1] # reference steer angle
log_y = log.measured_steer_angle[:]
log_v = log.kollmorgen_command_torque
log_w = log.kollmorgen_applied_torque
tf_YU = frequency_response(log_u, log_y, n)
tf_RU = frequency_response(log_u, log_r, n)
tf_YR = frequency_response(log_r, log_y, n)
tf_VU = frequency_response(log_u, log_v, n)
tf_WU = frequency_response(log_u, log_w, n)
if model_compare:
# simulated model response
X = frequency_response(lsim_u, lsim_y, n)
# generate frequency response of previously defined model
w, mag, phase = tf.bode(w=2*np.pi*np.logspace(-0.7, 0.7, 100))
index = [i for i, f in enumerate(np.around(freq, 1))
if f in np.around(u_freq, 1)]
fig, ax = plt.subplots(2, 1, sharex=True)
if model_compare:
ax[0].loglog(w/2/np.pi, np.power(10, mag/20),
linestyle='--', color=color[1],
label='R/U: steer torque to reference steer angle (bode)')
ax[0].loglog(freq[index], np.abs(X[index]),
marker='x', color=color[1],
label='R/U: steer torque to reference steer angle (lsim)')
ax[0].loglog(freq[index], np.abs(tf_RU[index]),
marker='x', color=color[2],
label='R/U: steer torque to reference steer angle')
else:
ax[0].loglog(freq[index], np.abs(tf_YU[index]),
marker='x', color=color[0],
label='Y/R: steer torque to measured steer angle')
ax[0].loglog(freq[index], np.abs(tf_YR[index]),
marker='x', color=color[3],
label='Y/R: reference to measured steer angle')
ax[0].loglog(freq[index], np.abs(tf_VU[index]),
marker='x', color=color[4],
label='V/U: steer torque to command torque')
#ax[0].loglog(freq[index], np.abs(tf_WU[index]),
# marker='x', color=color[5],
# label='W/U: steer torque to current torque')
ax[0].legend()
ax[0].set_ylabel('gain')
if model_compare:
ax[1].semilogx(w/2/np.pi, phase,
linestyle='--', color=color[1],
label='R/U: steer torque to reference steer angle (bode)')
ax[1].semilogx(freq[index], np.angle(X[index], deg=True),
marker='x', color=color[1],
label='R/U: steer torque to reference steer angle (lsim)')
ax[1].semilogx(freq[index], np.angle(tf_RU[index], deg=True),
marker='x', color=color[2],
label='R/U: steer torque to reference steer angle')
else:
ax[1].semilogx(freq[index], np.angle(tf_YU[index], deg=True),
marker='x', color=color[0],
label='Y/U: steer torque to measured steer angle')
ax[1].semilogx(freq[index], np.angle(tf_YR[index], deg=True),
marker='x', color=color[3],
label='Y/R: reference to measured steer angle')
ax[1].semilogx(freq[index], np.angle(tf_VU[index], deg=True),
marker='x', color=color[4],
label='V/U: steer torque to command torque')
#ax[1].semilogx(freq[index], np.angle(tf_WU[index], deg=True),
# marker='x', color=color[5],
# label='W/U: steer torque to current torque')
ax[1].legend()
ax[1].set_ylabel('phase [deg]')
ax[1].set_xlabel('frequency [Hz]')
ax[0].set_title('frequency response')
return fig, ax
In [24]:
def plot_roll_frequency_response(log):
#color = sns.color_palette('deep', 6)
color = sns.color_palette('Paired', 12)[1::2]
log_u = log.records.input[:, 1] # steer torque
log_r = log.states[:, 0] # reference roll angle
tf_RU = frequency_response(log_u, log_r, n)
# simulated model response
X = frequency_response(lsim_u, lsim_x[:, 0], n)
# generate frequency response of previously defined model
sys = scipy.signal.StateSpace(A, Bi, np.array([[1, 0, 0, 0]]), D)
tf = scipy.signal.TransferFunction(sys)
w, mag, phase = tf.bode(w=2*np.pi*np.logspace(-0.7, 0.7, 100))
index = [i for i, f in enumerate(np.around(freq, 1))
if f in np.around(u_freq, 1)]
fig, ax = plt.subplots(2, 1, sharex=True)
ax[0].loglog(w/2/np.pi, np.power(10, mag/20),
linestyle='--', color=color[1],
label='R/U: steer torque to reference roll angle (bode)')
ax[0].loglog(freq[index], np.abs(X[index]),
marker='x', color=color[1],
label='R/U: steer torque to reference roll angle (lsim)')
ax[0].loglog(freq[index], np.abs(tf_RU[index]),
marker='x', color=color[2],
label='R/U: steer torque to reference roll angle')
ax[0].legend()
ax[0].set_ylabel('gain')
ax[1].semilogx(w/2/np.pi, phase,
linestyle='--', color=color[1],
label='R/U: steer torque to reference roll angle (bode)')
ax[1].semilogx(freq[index], np.angle(X[index], deg=True),
marker='x', color=color[1],
label='R/U: steer torque to reference roll angle (lsim)')
ax[1].semilogx(freq[index], np.angle(tf_RU[index], deg=True),
marker='x', color=color[2],
label='R/U: steer torque to reference roll angle')
ax[1].legend()
ax[1].set_ylabel('phase [deg]')
ax[1].set_xlabel('frequency [Hz]')
ax[0].set_title('roll frequency response')
return fig, ax
In [14]:
plt.close('all')
plot_frequency_response(log, True)
plt.show()
In [25]:
plt.close('all')
plot_roll_frequency_response(log)
plt.show()
In [9]:
plt.close('all')
plot_frequency_response(log, False)
plt.show()
In [10]:
plt.close('all')
log2 = ps.ProcessedRecord('logs/multisine_kp150_kd3.pb.cobs.gz')
fig, ax = plot_response(log2)
ax[0].plot(t, u, linestyle='--',
label='designed multisine perturbation')
ax[0].legend()
plot_frequency_response(log2, False)
plt.show()
In [11]:
%matplotlib inline
mpl.rcParams['figure.figsize'] = DEFAULT_FIGSIZE
plt.close('all')
log3 = ps.ProcessedRecord('logs/multisine_kp20_kd2.pb.cobs.gz')
fig, ax = plot_response(log3)
ax[0].plot(t, u, linestyle='--',
label='designed multisine perturbation')
ax[0].legend()
plot_frequency_response(log3, False)
plt.show()