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 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_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