Project_IMRS/plot_logs.py at master · ryabegor/Project_IMRS · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
import numpy as np
import matplotlib.pyplot as plt


def main():
    data = np.load("sim_logs.npz")

    t = data["t"]
    vx = data["vx"]
    vref = np.ones_like(t)*0.4  # исправлено: целевая скорость 0.4 м/с (из графика в презентации)
    e = 0.4 - vx
    u_raw = data["u_raw"]
    u_sat = data["u_sat"]
    blend = data["blend"]

    wheelL_w = data["wheelL_w"]
    wheelR_w = data["wheelR_w"]
    wheelL_z = data["wheelL_z"]
    wheelR_z = data["wheelL_z"]

    hipL = data["hipL"]
    kneeL = data["kneeL"]
    hipR = data["hipR"]
    kneeR = data["kneeR"]

    # --- НОВЫЙ РАСЧЁТ: ПОЛОЖЕНИЕ И УСКОРЕНИЕ ---
    # 1. Положение (интегрируем скорость)
    x_pos = np.zeros_like(t)
    for i in range(1, len(t)):
        dt = t[i] - t[i-1]
        x_pos[i] = x_pos[i-1] + vx[i-1] * dt

    # 2. Ускорение (дифференцируем скорость)
    accel = np.zeros_like(t)
    for i in range(1, len(t)):
        dt = t[i] - t[i-1]
        if dt > 0:
            accel[i] = (vx[i] - vx[i-1]) / dt
    # Сглаживание ускорения (необязательно, но помогает убрать шум)
    window_size = 5
    if len(accel) > window_size:
        kernel = np.ones(window_size) / window_size
        accel_smooth = np.convolve(accel, kernel, mode='same')
    else:
        accel_smooth = accel

    # --- СТАРЫЕ ГРАФИКИ (оставьте как есть) ---
    plt.figure()
    plt.plot(t, vx, label="v_x")
    plt.plot(t, vref, "--", label="v_ref")
    plt.xlabel("t [s]")
    plt.ylabel("m/s")
    plt.title("Forward speed")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, e, label="e = v_ref - v_x")
    plt.xlabel("t [s]")
    plt.ylabel("m/s")
    plt.title("Speed error")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, u_raw, label="u_raw")
    plt.plot(t, u_sat, label="u_sat")
    plt.xlabel("t [s]")
    plt.ylabel("control")
    plt.title("PID output (raw vs saturated)")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, blend, label="blend (0 wheels -> 1 walk)")
    plt.xlabel("t [s]")
    plt.ylabel("blend")
    plt.title("Mode blend")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, hipL, label="hip_L")
    plt.plot(t, kneeL, label="knee_L")
    plt.plot(t, hipR, label="hip_R")
    plt.plot(t, kneeR, label="knee_R")
    plt.xlabel("t [s]")
    plt.ylabel("angle [rad]")
    plt.title("Leg joint angles")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, wheelL_w, label="wheel_L qvel [rad/s]")
    plt.plot(t, wheelR_w, label="wheel_R qvel [rad/s]")
    plt.xlabel("t [s]")
    plt.ylabel("angular speed [rad/s]")
    plt.title("Wheel angular velocities")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, wheelL_z, label="wheel_L height [m]")
    plt.plot(t, wheelR_z, label="wheel_R height [m]")
    plt.xlabel("t [s]")
    plt.ylabel("z [m]")
    plt.title("Wheel height (site z)")
    plt.legend()
    plt.grid(True)

    # --- НОВЫЕ ГРАФИКИ: ПОЛОЖЕНИЕ И УСКОРЕНИЕ ---
    plt.figure()
    plt.plot(t, x_pos, label="x position", color='green')
    plt.xlabel("t [s]")
    plt.ylabel("x [m]")
    plt.title("Robot position")
    plt.legend()
    plt.grid(True)

    plt.figure()
    plt.plot(t, accel_smooth, label="acceleration", color='red')
    plt.xlabel("t [s]")
    plt.ylabel("a [m/s²]")
    plt.title("Robot acceleration")
    plt.legend()
    plt.grid(True)

    plt.show()


if __name__ == "__main__":
    main()