Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
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)
Ejemplo n.º 7
0
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])
Ejemplo n.º 8
0
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])
Ejemplo n.º 9
0
    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
Ejemplo n.º 10
0
    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