EmioLabs_Mimo/study_closedloop.py at main · AlessandriniAntoine/EmioLabs_Mimo · 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
# %%
import os
import numpy as np
import matplotlib.pyplot as plt

# %% Load Data
data_path = os.path.join(os.path.abspath(""), "data")

data_emio = np.load(os.path.join(data_path, "hardware_closedLoop.npz"))
outputsPos_emio = data_emio["outputsPos"]
motorsPos_emio = data_emio["motorsPos"]
observerStates_emio = data_emio["observerState"]
observerOutputs_emio = data_emio["observerOutput"]
controlMode_emio = data_emio["controlMode"]

nx, ny, nu = (
    observerStates_emio.shape[1],
    outputsPos_emio.shape[1],
    motorsPos_emio.shape[1],
)

# %% outputs
fig, ax = plt.subplots(ny // 2, 2, figsize=(12, 12))
for i in range(ny // 2):
    for j in range(2):
        ax[i, j].plot(outputsPos_emio[:, 2 * i + j], "-r", label="Emio")  #
        ax[i, j].plot(observerOutputs_emio[:, 2 * i + j], "--m", label="Emio (obs)")  #
    ax[i, 0].set_title(f"Marker {i}")
ax[-1, 0].set_xlabel("Time Step")
ax[-1, 1].set_xlabel("Time Step")
ax[0, 0].set_title("Vertical")
ax[0, 1].set_title("Horizontal")
ax[0, 0].legend(loc="upper right")
fig.suptitle("Outputs")
plt.tight_layout()
# %% Motors commands
fig, ax = plt.subplots(nu, 2, figsize=(12, 12))
for i in range(nu):
    ax[i, 0].plot(motorsPos_emio[:, i], "-r", label="Emio")
    ax[i, 1].plot(motorsPos_emio[1:, i] - motorsPos_emio[:-1, i], "-r", label="Emio")
    ax[i, 0].set_ylabel(f"Motor {i}")
ax[0, 0].set_title("Position")
ax[0, 1].set_title("Velocity")
ax[-1, 0].set_xlabel("Time Step")
ax[-1, 1].set_xlabel("Time Step")
ax[0, 0].legend(loc="upper right")
fig.suptitle("Commands")
plt.tight_layout()

# %% Observer states
fig, ax = plt.subplots(nx // 2, 2, figsize=(15, 15))
for i in range(nx // 2):
    for j in range(2):
        ax[i, j].plot(
            observerStates_emio[:, i + j * (nx // 2)],
            color="m",
            linestyle="--",
            marker=",",
            label="Emio (obs)",
        )
        ax[i, j].set_ylabel(f"x_{i + j * (nx // 2) + 1}")
ax[-1, 0].set_xlabel("Time Step")
ax[-1, 1].set_xlabel("Time Step")
ax[0, 0].set_title("Velocity")
ax[0, 1].set_title("Position")
ax[0, 0].legend(loc="upper right")
fig.suptitle("States")
plt.tight_layout()

# %% Individual command
controller = np.load(os.path.join(data_path, f"controller_order{nx // 2}.npz"))
K = controller["feedbackGain"]
individual_cmd_emio = K[np.newaxis, :, :] * observerStates_emio[:, np.newaxis, :]

colors = plt.cm.viridis(np.linspace(0, 1, nx))
fig, ax = plt.subplots(nu, 1, figsize=(12, 12))
for i in range(nu):
    for j in range(nx):
        ax[i].plot(
            individual_cmd_emio[:, i, j],
            ":",
            color=colors[j],
            label=rf"$k{i + 1}{j + 1} * x{j + 1}$",
        )
    ax[i].set_title(rf"Contribution for $u_{i}$")
    ax[i].legend(loc="upper left")
plt.tight_layout()
plt.show()


plt.figure()
plt.plot(motorsPos_emio[:150, 0], label='with mass')
plt.plot(motorsPos_emio[150:300, 0], label='without mass')
plt.legend()
plt.show()