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 test_benchmark_to_canonical():
M, C1, K0, K2 = dtkbicycle.benchmark_matrices()
par = dtkbicycle.benchmark_parameters()
bpM, bpC1, bpK0, bpK2 = bicycle.benchmark_par_to_canonical(par)
nptest.assert_allclose(M, bpM)
nptest.assert_allclose(C1, bpC1)
nptest.assert_allclose(K0, bpK0)
nptest.assert_allclose(K2, bpK2)
def test_benchmark_to_canonical():
M, C1, K0, K2 = dtkbicycle.benchmark_matrices()
par = dtkbicycle.benchmark_parameters()
bpM, bpC1, bpK0, bpK2 = bicycle.benchmark_par_to_canonical(par)
nptest.assert_allclose(M, bpM)
nptest.assert_allclose(C1, bpC1)
nptest.assert_allclose(K0, bpK0)
nptest.assert_allclose(K2, bpK2)
def test_benchmark_eigenvalues():
expected = dtkbicycle.benchmark_matrices()
path_to_package = os.path.split(bicycleparameters.__file__)[0]
path_to_data = os.path.join(path_to_package, '..', 'data')
benchmark = bicycleparameters.Bicycle('Benchmark', path_to_data, True,
True)
M, C1, K0, K2 = benchmark.canonical(nominal=True)
nptest.assert_allclose(M, expected[0])
nptest.assert_allclose(C1, expected[1])
nptest.assert_allclose(K0, expected[2])
nptest.assert_allclose(K2, expected[3])
def test_benchmark_eigenvalues():
expected = dtkbicycle.benchmark_matrices()
path_to_package = os.path.split(bicycleparameters.__file__)[0]
path_to_data = os.path.join(path_to_package, '..', 'data')
benchmark = bicycleparameters.Bicycle('Benchmark', path_to_data, True,
True)
M, C1, K0, K2 = benchmark.canonical(nominal=True)
nptest.assert_allclose(M, expected[0])
nptest.assert_allclose(C1, expected[1])
nptest.assert_allclose(K0, expected[2])
nptest.assert_allclose(K2, expected[3])
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_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