def simulate(v, x0, dt, n, u=None):
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), [v])
A = np.squeeze(A)
B = np.squeeze(B)
M = np.zeros((6, 6))
M[:4, :4] = A
M[:4, 4:] = B
M *= dt
Md = scipy.linalg.expm(M)
Md_zero = Md[4:, :4]
Md_eye = Md[4:, 4:]
if not np.array_equal(Md_zero, np.zeros(Md_zero.shape)):
print('WARNING: Failure in system discretization')
print(Md_zero)
print('should equal 0')
if not np.array_equal(Md_eye, np.eye(2)):
print('WARNING: Failure in system discretization')
print(Md_eye)
print('should equal I')
Ad = Md[:4, :4]
Bd = Md[:4, 4:]
if u is None:
u = np.zeros((2, n))
x = np.zeros((4, n))
for i in range(n):
x[:, i:i + 1] = np.dot(Ad, x0) + np.dot(Bd, u[:, i:i + 1])
x0 = x[:, i:i + 1]
return x
def compute_whipple_lqr_gain(velocity):
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), velocity)
Q = np.diag([1e5, 1e3, 1e3, 1e2])
R = np.eye(2)
gains = [control.lqr(Ai, Bi, Q, R)[0] for Ai, Bi in zip(A, B)]
return gains
def compute_whipple_lqr_gain(velocity):
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), velocity)
Q = np.diag([1e5, 1e3, 1e3, 1e2])
R = np.eye(2)
gains = [control.lqr(Ai, Bi, Q, R)[0] for Ai, Bi in zip(A, B)]
return gains
def simulate(v, x0, dt, n, u=None):
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(), [v])
A = np.squeeze(A)
B = np.squeeze(B)
M = np.zeros((6, 6))
M[:4, :4] = A
M[:4, 4:] = B
M *= dt
Md = scipy.linalg.expm(M)
Md_zero = Md[4:, :4]
Md_eye = Md[4:, 4:]
if not np.array_equal(Md_zero, np.zeros(Md_zero.shape)):
print('WARNING: Failure in system discretization')
print(Md_zero)
print('should equal 0')
if not np.array_equal(Md_eye, np.eye(2)):
print('WARNING: Failure in system discretization')
print(Md_eye)
print('should equal I')
Ad = Md[:4, :4]
Bd = Md[:4, 4:]
if u is None:
u = np.zeros((2, n))
x = np.zeros((4, n))
for i in range(n):
x[:, i:i+1] = np.dot(Ad, x0) + np.dot(Bd, u[:, i:i+1])
x0 = x[:, i:i+1]
return x
def plot_states(self, degrees=False, **kwargs):
"""Plot states as generated from the model simulation (ground truth).
"""
fig, ax = plt.subplots(3, 1, sharex=True, **kwargs)
if degrees:
scale = 180/np.pi
angle_unit = 'deg'
else:
scale = 1
angle_unit = 'rad'
has_model = self.messages[0].model.HasField('v')
if has_model:
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(),
[v])
A = np.squeeze(A)
B = np.squeeze(B)
C = np.eye(4)
D = np.zeros((4, 2))
u = self.records.input.reshape((-1, 2, 1))
system = scipy.signal.lti(A, B, C, D)
_, _, lsim_state = scipy.signal.lsim(system, u, self.t)
# plot angles
self._plot_line(ax[0], 'roll angle', scale)
self._plot_line(ax[0], 'steer angle', scale)
if has_model:
ax[0].plot(self.t, scale*lsim_state[:, 0],
label='roll angle (lsim)',
color=self._color('roll angle'),
alpha=0.8, linestyle='--')
ax[0].plot(self.t, scale*lsim_state[:, 1],
label='steer angle (lsim)',
color=self._color('steer angle'),
alpha=0.8, linestyle='--')
ax[0].set_ylabel('angle [{}]'.format(angle_unit))
ax[0].set_xlabel('time [s]')
#ax[0].axhline(0, color='black')
ax[0].legend()
# plot angular rates
self._plot_line(ax[1], 'roll rate', scale)
self._plot_line(ax[1], 'steer rate', scale)
if has_model:
ax[1].plot(self.t, scale*lsim_state[:, 2],
label='roll rate (lsim)',
color=self._color('roll rate'),
alpha=0.8, linestyle='--')
ax[3].plot(self.t, scale*lsim_state[:, 3],
label='steer rate (lsim)',
color=self._color('steer rate'),
alpha=0.8, linestyle='--')
ax[1].set_ylabel('rate [{}/s]'.format(angle_unit))
ax[1].set_xlabel('time [s]')
#ax[1].axhline(0, color='black')
ax[1].legend()
# plot torques
self._plot_line(ax[2], 'steer torque')
ax[2].plot(self.t, self.kollmorgen_command_torque,
label='commanded torque', color=self.colors[11], alpha=0.8)
ax[2].set_ylabel('torque [Nm]')
ax[2].set_xlabel('time [s]')
#ax[2].axhline(0, color='black')
ax[2].legend()
return fig, ax
def plot(canon, H, riders, environments, idMats):
filename = ''
for rider in riders:
filename += '-' + rider
for env in environments:
filename += '-' + env.replace(' ', '')
filename = 'canonical-id-plots/' + filename[1:]
print filename
v0 = 0.
vf = 10.
num = 100
mM, mC1, mK0, mK2, mH = csi.mean_canon(riders, canon, H)
speeds, mAs, mBs = bicycle.benchmark_state_space_vs_speed(mM, mC1, mK0, mK2,
v0=v0, vf=vf, num=num)
w, v = control.eigen_vs_parameter(mAs)
mEigenvalues, mEigenvectors = control.sort_modes(w, v)
iM, iC1, iK0, iK2, iH = idMats
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
v0=v0, vf=vf, num=num)
w, v = control.eigen_vs_parameter(iAs)
iEigenvalues, iEigenvectors = control.sort_modes(w, v)
aAs, aBs, aSpeed = csi.mean_arm(riders)
w, v = control.eigen_vs_parameter(aAs)
aEigenvalues, aEigenvectors = control.sort_modes(w, v)
rlfig = plt.figure()
ax = rlfig.add_subplot(1, 1, 1)
ax.plot(speeds, iEigenvalues.real, 'k-')
ax.plot(speeds, abs(iEigenvalues.imag), 'k--')
ax.plot(speeds, mEigenvalues.real, 'b-')
ax.plot(speeds, abs(mEigenvalues.imag), 'b--')
ax.plot(aSpeed, aEigenvalues.real, 'r-')
ax.plot(aSpeed, abs(aEigenvalues.imag), 'r--')
ax.set_ylim((-15, 10))
ax.set_xlabel('Speed [m/s]')
ax.set_ylabel('Real and Imaginary Parts of the Eigenvalues [1/s]')
rlfig.savefig(filename + '-eig.png')
rlfig.clf()
# bode plots
speeds = [2.0, 3.0, 5.8, 9.0]
null, mAs, mBs = bicycle.benchmark_state_space_vs_speed(mM, mC1, mK0, mK2,
speeds)
null, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
speeds)
C = np.array([[1.0, 0.0, 0.0, 0.0]])
D = np.array([[0.0, 0.0]])
systems = []
inputNames = ['$T_\phi$', '$T_\delta$']
stateNames = ['$\phi$', '$\delta$', '$\dot{\phi}$', '$\dot{\delta}$']
outputNames = ['$\phi$']
for A, B in zip(mAs, mBs):
systems.append(control.StateSpace(A, B, C, D, name='Whipple',
stateNames=stateNames, inputNames=inputNames,
outputNames=outputNames))
for A, B in zip(iAs, iBs):
systems.append(control.StateSpace(A, B, C, D, name='Canon ID',
stateNames=stateNames, inputNames=inputNames,
outputNames=outputNames))
C = np.zeros((1, 19))
C[0, 3] = 1
D = 0.
indices = [20, 30, 58, 90]
for A, B in zip(aAs[indices], aBs[indices]):
systems.append(control.StateSpace(A, B[:, [0, 2]], C, D, name='Arms',
inputNames=inputNames, outputNames=outputNames))
w = np.logspace(-1, 2)
linestyles = ['-', '--', '-.', ':'] * 3
colors = ['k'] * len(speeds) + ['b'] * len(speeds) + ['r'] * len(speeds)
bode = control.Bode(w, *tuple(systems), linestyles=linestyles, colors=colors)
bode.plot()
bode.figs[0].savefig(filename + '-Tphi.png')
bode.figs[1].savefig(filename + '-Tdel.png')
bode.figs[0].clf()
bode.figs[1].clf()
def plot_states(self, degrees=False, **kwargs):
"""Plot states as generated from the model simulation (ground truth).
"""
fig, ax = plt.subplots(3, 1, sharex=True, **kwargs)
if degrees:
scale = 180 / np.pi
angle_unit = 'deg'
else:
scale = 1
angle_unit = 'rad'
has_model = self.messages[0].model.HasField('v')
if has_model:
_, A, B = benchmark_state_space_vs_speed(*benchmark_matrices(),
[v])
A = np.squeeze(A)
B = np.squeeze(B)
C = np.eye(4)
D = np.zeros((4, 2))
u = self.records.input.reshape((-1, 2, 1))
system = scipy.signal.lti(A, B, C, D)
_, _, lsim_state = scipy.signal.lsim(system, u, self.t)
# plot angles
self._plot_line(ax[0], 'roll angle', scale)
self._plot_line(ax[0], 'steer angle', scale)
if has_model:
ax[0].plot(self.t,
scale * lsim_state[:, 0],
label='roll angle (lsim)',
color=self._color('roll angle'),
alpha=0.8,
linestyle='--')
ax[0].plot(self.t,
scale * lsim_state[:, 1],
label='steer angle (lsim)',
color=self._color('steer angle'),
alpha=0.8,
linestyle='--')
ax[0].set_ylabel('angle [{}]'.format(angle_unit))
ax[0].set_xlabel('time [s]')
#ax[0].axhline(0, color='black')
ax[0].legend()
# plot angular rates
self._plot_line(ax[1], 'roll rate', scale)
self._plot_line(ax[1], 'steer rate', scale)
if has_model:
ax[1].plot(self.t,
scale * lsim_state[:, 2],
label='roll rate (lsim)',
color=self._color('roll rate'),
alpha=0.8,
linestyle='--')
ax[3].plot(self.t,
scale * lsim_state[:, 3],
label='steer rate (lsim)',
color=self._color('steer rate'),
alpha=0.8,
linestyle='--')
ax[1].set_ylabel('rate [{}/s]'.format(angle_unit))
ax[1].set_xlabel('time [s]')
#ax[1].axhline(0, color='black')
ax[1].legend()
# plot torques
self._plot_line(ax[2], 'steer torque')
ax[2].plot(self.t,
self.kollmorgen_command_torque,
label='commanded torque',
color=self.colors[11],
alpha=0.8)
ax[2].set_ylabel('torque [Nm]')
ax[2].set_xlabel('time [s]')
#ax[2].axhline(0, color='black')
ax[2].legend()
return fig, ax
def plot(canon, H, riders, environments, idMats):
filename = ''
for rider in riders:
filename += '-' + rider
for env in environments:
filename += '-' + env.replace(' ', '')
filename = '../plots/' + filename[1:]
print filename
v0 = 0.
vf = 10.
num = 100
# identified
iM, iC1, iK0, iK2, iH = idMats
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
v0=v0, vf=vf, num=num)
w, v = control.eigen_vs_parameter(iAs)
iEigenvalues, iEigenvectors = control.sort_modes(w, v)
# whipple model (mean)
wM, wC1, wK0, wK2, wH = cbi.mean_canon(riders, canon, H)
speeds, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM, wC1, wK0, wK2,
v0=v0, vf=vf, num=num)
w, v = control.eigen_vs_parameter(wAs)
wEigenvalues, wEigenvectors = control.sort_modes(w, v)
# arm model (mean)
aAs, aBs, aSpeed = cbi.mean_arm(riders)
indices = np.int32(np.round(speeds * 10))
w, v = control.eigen_vs_parameter(aAs[indices])
aEigenvalues, aEigenvectors = control.sort_modes(w, v)
# eigenvalue plot
rlfig = cbi.plot_rlocus_parts(speeds, iEigenvalues, wEigenvalues,
aEigenvalues)
rlfig.savefig(filename + '-eig.png')
# root locus
v0 = 0.
vf = 10.
num = 20
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
v0=v0, vf=vf, num=num)
iEig, null = control.eigen_vs_parameter(iAs)
speeds, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM, wC1, wK0, wK2,
v0=v0, vf=vf, num=num)
wEig, null = control.eigen_vs_parameter(wAs)
indices = np.int32(np.round(speeds * 10))
aEig, null = control.eigen_vs_parameter(aAs[indices])
rlcfig = cbi.plot_rlocus(speeds, iEig, wEig, aEig)
rlcfig.savefig(filename + '-rlocus.png')
# bode plots
speeds = np.array([2.0, 4.0, 6.0, 9.0])
null, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
speeds)
null, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM, wC1, wK0, wK2,
speeds)
figs = cbi.plot_bode(speeds, iAs, iBs, wAs, wBs, aAs, aBs)
figs[0].savefig(filename + '-Tphi.png')
figs[1].savefig(filename + '-Tdel.png')
# load M, C1, K0, K2 for each rider
canon = cbi.load_benchmark_canon(allRiders)
# load the H lateral force vector for each rider
H = cbi.lateral_force_contribution(allRiders)
# Eigenvalues versus speed plot.
v0 = 0.
vf = 10.
num = 100
# identified for all rider and all environments
iM, iC1, iK0, iK2, iH = idMat['A-A']
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM,
iC1,
iK0,
iK2,
v0=v0,
vf=vf,
num=num)
w, v = control.eig_of_series(iAs)
iEigenvalues, iEigenvectors = control.sort_modes(w, v)
# whipple model (mean)
wM, wC1, wK0, wK2, wH = cbi.mean_canon(allRiders, canon, H)
speeds, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM,
wC1,
wK0,
wK2,
v0=v0,
vf=vf,
num=num)
# Load in precomputed results from identify.py.
results_directory = '../data'
with open(os.path.join(results_directory, 'id-matrices.p'), 'r') as f:
id_matrices = cPickle.load(f)
# Eigenvalues versus speed parameters.
v0 = 0.
vf = 10.
num = 100
# Load the identified model from data for rider L and in pavillion floor and
# generate the eigenvalues an eigenvectors as a function of speed.
iM, iC1, iK0, iK2, iH = id_matrices['L-P']
speeds, iAs, iBs = bicycle.benchmark_state_space_vs_speed(iM, iC1, iK0, iK2,
v0=v0, vf=vf,
num=num)
w, v = control.eig_of_series(iAs)
iEigenvalues, iEigenvectors = control.sort_modes(w, v)
# Load the Whipple model M, C1, K0, K2, H from first principles and generate
# the eigenvalues and eigenvectors as a function of speed.
wM, wC1, wK0, wK2 = cbi.load_benchmark_canon(['Luke'])['Luke']
wH = cbi.lateral_force_contribution(['Luke'])['Luke']
speeds, wAs, wBs = bicycle.benchmark_state_space_vs_speed(wM, wC1, wK0, wK2,
v0=v0, vf=vf,
num=num)
w, v = control.eig_of_series(wAs)
wEigenvalues, wEigenvectors = control.sort_modes(w, v)
# Load the Arm model state space for each rider from first principles and
