<commit_before>#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import scipy
from dtk.bicycle import benchmark_state_space_vs_speed, benchmark_matrices


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


if __name__ == '__main__':
    import sys
    import matplotlib.pyplot as plt
    from plot_sim import plot_states

    sim_time = 5 # time for our Whipple simulation [s]

    if len(sys.argv) > 1:
        from load_sim import (load_messages, get_records_from_messages,
                              get_time_vector)
        messages = load_messages(sys.argv[1])
        records = get_records_from_messages(messages)
        t = get_time_vector(records)
        t -= t[0] # redefine zero time for simulator data
        m = messages[0]
        states = records.state[:, 1:]
        fig2, ax2 = plot_states(t, states)

        v = m.model.v
        dt = m.model.dt
        n = int(sim_time/dt)
        x0 = np.array(messages[1].state.x[1:]).reshape((4, 1))
    else:
        v = 5
        dt = 0.005
        n = int(sim_time/dt)
        x0 = np.array(
            [ 0.0031247 ,  0.09299604, -0.03369007, -0.05003717]
            ).reshape((4, 1))

    x = simulate(v, x0, dt, n)
    t = np.array(range(n + 1)) * dt
    fig, ax = plot_states(t, np.hstack((x0, x)).T)
    plt.show()<commit_msg>Fix simulator plot in simulation comparison script<commit_after>#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import scipy
from dtk.bicycle import benchmark_state_space_vs_speed, benchmark_matrices


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


if __name__ == '__main__':
    import sys
    import matplotlib.pyplot as plt
    from plot_sim import plot_states

    sim_time = 1 # time for our Whipple simulation or upper limit [s]

    if len(sys.argv) > 1:
        from load_sim import (load_messages, get_records_from_messages,
                              get_time_vector)
        messages = load_messages(sys.argv[1])
        records = get_records_from_messages(messages)
        t = get_time_vector(records[1:]) # skip first record
        t -= t[0] # redefine zero time for simulator data

        m = messages[0]
        v = m.model.v
        dt = m.model.dt
        if t[-1] < sim_time:
            sim_time = t[-1]
        n = int(sim_time/dt)

        states = records[1:].state[:, 1:]
        fig2, ax2 = plot_states(t[:n], states[:n, :], to_degrees=True)
        fig2.suptitle('Phobos simulator')
        x0 = np.array(messages[1].state.x[1:]).reshape((4, 1))
    else:
        v = 5
        dt = 0.005
        n = int(sim_time/dt)
        x0 = np.array(
            [ 0.0031247 ,  0.09299604, -0.03369007, -0.05003717]
            ).reshape((4, 1))

    x = simulate(v, x0, dt, n)
    t = np.array(range(n + 1)) * dt
    fig, ax = plot_states(t, np.hstack((x0, x)).T, to_degrees=True)
    fig.suptitle('Whipple simulation')

    plt.show()
{'fraction_non_alphanumeric': 0.14819700967458224, 'fraction_numerical': 0.0380386983289358, 'mean_word_length': 2.217114568599717, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5309124', 'n_tokens_mistral': 1855, 'n_tokens_neox': 1727, 'n_words': 418}