def compare_episode(past, ani=True):
dir_save = list(Path('log').glob("*"))[past]
epi_list = [x for x in dir_save.glob("*")]
env_data_list = [
logging.load(Path(epi_dir, "env_data.h5")) for epi_dir in epi_list
]
agent_data_list = [
logging.load(Path(epi_dir, "agent_data.h5")) for epi_dir in epi_list
]
_, info = logging.load(Path(epi_list[0], "env_data.h5"), with_info=True)
cfg = info['cfg']
if ani == True:
data_num = len(env_data_list)
fig_shape = split_int(data_num)
simple = False
if fig_shape[0] >= 3:
simple=True
fig, _ = plt.subplots(
fig_shape[0],
fig_shape[1],
subplot_kw=dict(projection="3d"),
)
ani = Animator(fig, env_data_list, cfg, simple=simple)
ani.animate()
ani.save(Path(dir_save, "compare-animation.mp4"))
plt.close('all')
return_list = []
for i, data in enumerate(agent_data_list):
G = 0
for r in data['reward'][::-1]:
G = r.item() + cfg.ddpg.discount*G
return_list.append([(i+1)*cfg.epi_eval, G])
return_list = np.array(return_list)
fig, ax = plt.subplots(nrows=1, ncols=1)
ax.plot(return_list[:,0], return_list[:,1], "*")
ax.set_title("Return")
ax.set_ylabel("Return")
ax.set_xlabel("Episode")
ax.grid(True)
fig.savefig(
Path(dir_save, "return.png"),
bbox_inches='tight'
)
plt.close('all')
def plot():
data = logging.load("data/tmp.h5")
time = data["time"]
state = data["state"]
plt.subplot(221)
plt.plot(time, state["system"].squeeze())
plt.legend([r"$x_1$", r"$x_2$"])
plt.subplot(222)
Y = state["memory"]["Y"]
Xi = state["memory"]["Xi"]
Dast = state["memory"]["Dast"]
wast = main.Env.wast
X = np.einsum("bij,jk->bik", Xi, wast) - Dast
# plt.plot(time, state["filter"]["dast"].squeeze(), label=r"$d^\ast$")
plt.plot(time, Y.squeeze(), "k", label=r"$Y$")
plt.plot(time, X.squeeze(), "k--", label=r"$\Xi w^\ast - D^\ast$")
plt.legend()
plt.subplot(223)
plt.plot(time, state["actor_critic"]["wa"].squeeze())
plt.subplot(224)
plt.plot(time, state["actor_critic"]["wc"].squeeze())
plt.show()
def plot_single(path, canvas=[], name=None, **kwargs):
data = logging.load(path)
def create_canvas():
fig, axes = plt.subplots(2, 2, sharex=True, num="states")
for ax in axes.flat:
ax.mod = 1
axes[0, 0].set_ylabel(r"$V_T$ [m/s]")
axes[0, 1].set_ylabel(r"$\alpha$ [deg]")
axes[0, 1].mod = np.rad2deg(1)
axes[1, 0].set_ylabel(r"$q$ [deg/s]")
axes[1, 0].mod = np.rad2deg(1)
axes[1, 1].set_ylabel(r"$\gamma$ [deg]")
axes[1, 1].mod = np.rad2deg(1)
axes[1, 0].set_xlabel("time [s]")
axes[1, 1].set_xlabel("time [s]")
canvas.append((fig, axes))
fig, axes = plt.subplots(2, 2, sharex=True, num="control")
for ax in axes.flat:
ax.mod = 1
axes[0, 0].set_ylabel(r"$\delta_t$")
axes[0, 1].set_ylabel(r"$\delta_e$ [deg]")
axes[0, 1].mod = np.rad2deg(1)
axes[1, 0].set_ylabel(r"$\eta_1$")
axes[1, 1].set_ylabel(r"$\eta_2$")
axes[1, 0].set_xlabel("time [s]")
axes[1, 1].set_xlabel("time [s]")
canvas.append((fig, axes))
return canvas
if not canvas:
canvas = create_canvas()
time = data["time"]
legend_line = []
fig, axes = canvas[0]
for ax, x in zip(axes.flat, data["state"].T):
ln, = ax.plot(time, x * ax.mod, **kwargs)
fig.tight_layout()
legend_line.append(ln)
fig, axes = canvas[1]
for ax, u in zip(axes.flat, data["action"].T):
ln, = ax.plot(time, u * ax.mod, **kwargs)
fig.tight_layout()
legend_line.append(ln)
if name is not None:
for ln in legend_line:
ln.set_label(name)
for window in canvas:
fig, axes = window
axes[0, 0].legend(*axes[-1, -1].get_legend_handles_labels())
return canvas
def plot_hist(path):
# path = os.path.join(
# obj.gan_dir,
# os.path.relpath(os.path.dirname(testfile), obj.test_dir),
# "train_history.h5"
# )
histdata = logging.load(path)
canvas = []
fig, axes = plt.subplots(1, 2, sharey=True, squeeze=False, num="loss")
axes[0, 0].set_ylabel(r"Loss")
axes[0, 0].set_xlabel(r"Epoch")
axes[0, 1].set_xlabel(r"Epoch")
axes[0, 0].set_title("Generator")
axes[0, 1].set_title("Discrimator")
canvas.append((fig, axes))
fig, axes = canvas[0]
ax = axes[0, 0]
ax.plot(histdata["epoch"], histdata["loss_g"])
ax = axes[0, 1]
ax.plot(histdata["epoch"], histdata["loss_d"])
fig.tight_layout()
def _case3(skip_sample):
# Case 3 - Inaccurate basis function
print("Case 3")
print("======")
casedir = os.path.join("data", "case3")
def phia(x):
return x
paramvar, noisescale = np.random.rand(2)
env = Env(paramvar, noisescale * 0, phia=phia)
env.set_envdir(casedir)
if not skip_sample:
_sample(env, 10, tqdm=True)
samplelist = sorted(glob(os.path.join(env.envdir, "sample-*.h5")))
datalist = [logging.load(path) for path in samplelist]
Tlist = [0.02, 0.5, 2]
agentlist = [IFRADP(2, 1, env, phia=phia)]
agentlist += [RADP(2, 1, T, env, phia=phia) for T in Tlist]
for agent in tqdm(agentlist):
agent.set_datalist(datalist)
agent.set_trainpath(env.envdir)
_train(agent)
def _plot_case1():
import matplotlib.pyplot as plt
# Case 1
casedir = os.path.join("data", "case1")
trainlist = glob(os.path.join(casedir, "*", "train-*.h5"))
fig, axes = plt.subplots(2, 1, sharex=True)
draw_true(axes)
for trainpath in trainlist:
data = logging.load(trainpath)
epoch = data["epoch"]
wa = data["params"]["wa"].squeeze()
wc = data["params"]["wc"].squeeze()
axes[0].plot(epoch, wc)
axes[1].plot(epoch, wa)
axes[0].set_ylabel(r"$w_c$")
axes[0].set_xlim(left=0)
axes[1].set_ylabel(r"$w_a$")
axes[1].set_xlabel("Iteration")
set_axes(fig, axes)
def plot_gan(path):
data = logging.load(path)
xlabels = (
r"$V_T$ [m/s]", r"$\alpha$ [deg]", r"$q$ [deg/s]", r"$\theta$ [deg]"
)
xmod = [1, np.rad2deg(1), np.rad2deg(1), np.rad2deg(1)]
ulabels = (
r"$\delta_t$", r"$\delta_e$", r"$\eta_1$", r"$\eta_2$"
)
umod = [1, np.rad2deg(1), 1, 1]
def plot_xu(canvas, i, j, data):
x, u, fake_u = (data[k] for k in ["state", "action", "fake_action"])
x = x[:, j] * xmod[j]
u = u[:, i] * umod[i]
fake_u = fake_u[:, i] * xmod[i]
xmin, xmax = x.min(), x.max()
umin, umax = u.min(), u.max()
fig, axes = canvas[0]
axes[i, 0].set_ylabel(ulabels[i])
axes[-1, 2 * j].set_xlabel(xlabels[j])
axes[-1, 2 * j + 1].set_xlabel(xlabels[j])
axes[i, 2 * j].set_xlim([xmin, xmax])
axes[i, 2 * j + 1].set_xlim([xmin, xmax])
axes[i, 2 * j].set_ylim([umin, umax])
axes[0, 2 * j].set_title("Real")
axes[0, 2 * j + 1].set_title("Fake")
ax = axes[i, 2 * j]
ax.plot(x, u, '.', markersize=2, mew=0, mfc=(0, 0, 0, 1))
ax = axes[i, 2 * j + 1]
ax.plot(x, fake_u, '.', markersize=2, mew=0, mfc=(0, 0, 0, 1))
canvas = []
fig, axes = plt.subplots(
4, 8, sharex=True, sharey=True, squeeze=False, num="real"
)
canvas.append((fig, axes))
for i in range(4):
for j in range(4):
plot_xu(canvas, i, j, data)
fig.tight_layout()
def __init__(self, file_names, keys):
if isinstance(file_names, str):
file_names = (file_names, )
data_all = [logging.load(name) for name in file_names]
self.keys = keys if not isinstance(keys, str) else (keys, )
_data = {
k: torch.cat([torch.tensor(data[k]).float() for data in data_all])
for k in self.keys
}
self.data = _data
self.len = len(self.data[self.keys[0]])
def _plot_case3():
import matplotlib.pyplot as plt
from cycler import cycler
# Case 3
casedir = os.path.join("data", "case3")
trainlist = sorted(glob(os.path.join(casedir, "*", "train-*.h5")))
custom_cycler = (cycler(color=["k"]) * cycler(ls=["-", "--", "-.", ":"]))
def draw_true(axes):
# Draw true parameters
for i in (1, 0.5, 0):
axes[0].axhline(i, c="r", ls="--")
for i in (0, -3):
axes[1].axhline(i, c="r", ls="--")
fig, axes = plt.subplots(2, 1, sharex=True)
draw_true(axes)
legendlines = []
for trainpath, cc in zip(trainlist, custom_cycler):
data, info = logging.load(trainpath, with_info=True)
if info["classname"] == "RADP":
label = rf"RADP ($T = {info['T']}$)"
else:
label = r"IFRADP"
cc = dict(color="b", ls="-", zorder=10)
epoch = data["epoch"]
wa = data["params"]["wa"].squeeze()
wc = data["params"]["wc"].squeeze()
lines = axes[0].plot(epoch, wc, **cc, label=label)
legendlines.append(lines[0])
axes[1].plot(epoch, wa, **cc)
axes[0].set_ylabel(r"$w_c$")
axes[0].set_xlim(left=0)
axes[1].set_ylabel(r"$w_a$")
axes[1].set_xlabel("Iteration")
fig.legend(handles=legendlines,
bbox_to_anchor=(0.1, 0.92, 0.87, .05),
loc='lower center',
ncol=4,
mode="expand",
borderaxespad=0.)
set_axes(fig, axes)
def _plot(obj, testfile):
data = logging.load(testfile)
real_x, real_u, mask, fake_x, fake_u = (data[k].ravel()
for k in ("real_x", "real_u",
"mask", "fake_x",
"fake_u"))
xmin, xmax = real_x.min(), real_x.max()
umin, umax = real_u.min(), real_u.max()
canvas = []
fig, axes = plt.subplots(1, 2, sharey=True, squeeze=False, num="real")
axes[0, 0].set_ylabel(r"$u$")
axes[0, 0].set_xlabel(r"$x$")
axes[0, 1].set_xlabel(r"$x$")
axes[0, 0].set_xlim([xmin, xmax])
axes[0, 1].set_xlim([xmin, xmax])
axes[0, 0].set_ylim([umin, umax])
axes[0, 0].set_title("Real")
axes[0, 1].set_title("Fake")
canvas.append((fig, axes))
fig, axes = canvas[0]
ax = axes[0, 0]
mask = mask.astype(bool)
ax.plot(real_x[mask],
real_u[mask],
'.',
markersize=2,
mew=0,
mfc=(0, 0, 0, 1))
ax.plot(real_x[~mask],
real_u[~mask],
'.',
markersize=2,
mew=0,
mfc=(1, 0, 0, 0.1))
ax = axes[0, 1]
ax.plot(fake_x, fake_u, '.', markersize=2, mew=0, mfc=(0, 0, 0, 1))
fig.tight_layout()
def train_plot(savepath, **kwargs):
data = logging.load(savepath)
epoch = data["i"]
canvas = []
fig, axes = plt.subplots(2, 2, sharex=True, num="hist")
axes[0, 0].set_ylabel(r"$\delta$")
axes[0, 1].set_ylabel(r"G Loss")
axes[1, 0].set_ylabel(r"$w, v$")
axes[1, 1].set_ylabel(r"$\theta$")
axes[1, 0].set_xlabel("Epoch")
axes[1, 1].set_xlabel("Epoch")
canvas.append((fig, axes))
fig, axes = canvas[0]
epoch = data["i"]
axes[0, 0].plot(
epoch,
data["loss"]["delta"].reshape(-1, data["loss"]["delta"][0].size),
**kwargs
)
axes[0, 1].plot(
epoch,
data["loss"]["gan"].reshape(-1, data["loss"]["gan"][0].size),
**kwargs
)
axes[1, 0].plot(
epoch,
data["w"].reshape(-1, data["w"][0].size),
**kwargs
)
axes[1, 0].plot(
epoch,
data["v"].reshape(-1, data["v"][0].size),
**kwargs
)
ln, *_ = axes[1, 1].plot(
epoch,
data["theta"].reshape(
-1, np.multiply(*data["theta"][0].shape)),
**kwargs
)
fig.tight_layout()
def test(obj, **kwargs):
agent = gan.GAN(lr=1e-3, x_size=1, u_size=1, z_size=30)
for weightfile in tqdm.tqdm(sorted(kwargs["weightfiles"])):
tqdm.tqdm.write(f"Using {weightfile} ...")
agent.load(weightfile)
agent.eval()
gandir = os.path.dirname(weightfile)
testpath = os.path.join(
obj.test_dir,
os.path.splitext(os.path.relpath(weightfile, obj.gan_dir))[0] +
".h5")
samplefiles = torch.load(os.path.join(gandir, "sample_path.h5"))
if isinstance(samplefiles, str):
samplefiles = [samplefiles]
data_all = [logging.load(name) for name in samplefiles]
real_x, real_u, mask = [
np.vstack([data[k] for data in data_all])
for k in ("state", "action", "mask")
]
data_gen = DataGen(mode="even")
fake_x = np.zeros_like(real_x)
fake_u = np.zeros_like(real_u)
for i in range(fake_x.size):
fake_x[i] = data_gen()[0]
fake_u = agent.get_action(fake_x)
logging.save(
testpath,
dict(real_x=real_x,
real_u=real_u,
mask=mask,
fake_x=fake_x,
fake_u=fake_u))
tqdm.tqdm.write(f"Test data is saved in {testpath}.")
_plot(obj, testpath)
plt.show()
def _case1(skip_sample):
# Case 1 : Different exploration data (different behavior)
print("Case 1")
print("======")
casedir = os.path.join("data", "case1")
envargs = np.random.rand(10, 2)
for paramvar, noisescale in tqdm(envargs):
env = Env(paramvar, noisescale)
env.set_envdir(casedir)
if not skip_sample:
_sample(env, 20)
# Train
samplelist = sorted(glob(os.path.join(env.envdir, "sample-*.h5")))
datalist = [logging.load(path) for path in samplelist]
agent = RADP(2, 1, 0.05, env)
agent.set_datalist(datalist)
agent.set_trainpath(env.envdir)
_train(agent)
def _plot_sample(testfile):
data = logging.load(testfile)
real_x, real_u, mask = (data[k].ravel()
for k in ("state", "action", "mask"))
xmin, xmax = real_x.min(), real_x.max()
umin, umax = real_u.min(), real_u.max()
canvas = []
fig, axes = plt.subplots(1, 1, squeeze=False, num=testfile)
axes[0, 0].set_ylabel(r"$u$")
axes[0, 0].set_xlabel(r"$x$")
axes[0, 0].set_xlim([xmin, xmax])
axes[0, 0].set_ylim([umin, umax])
canvas.append((fig, axes))
fig, axes = canvas[0]
ax = axes[0, 0]
mask = mask.astype(bool)
ax.plot(real_x[mask],
real_u[mask],
'.',
markersize=2,
mew=0,
mfc=(0, 0, 0, 1))
ax.plot(real_x[~mask],
real_u[~mask],
'.',
markersize=2,
mew=0,
mfc=(1, 0, 0, 0.1))
fig.tight_layout()
return fig
def run(path, **kwargs):
logger = logging.Logger(log_dir=".", file_name=kwargs["out"], max_len=100)
data = logging.load(path)
expname = os.path.basename(path)
envname, agentname, *_ = expname.split("-")
env = getattr(envs, envname)(initial_perturb=[1, 0.0, 0,
np.deg2rad(10)],
dt=0.01,
max_t=40,
solver="rk4",
ode_step_len=1)
agent = getattr(agents, agentname)(env,
lrw=1e-2,
lrv=1e-2,
lrtheta=1e-2,
w_init=0.03,
v_init=0.03,
theta_init=0,
maxlen=100,
batch_size=16)
agent.load_weights(data)
print(f"Runnning {expname} ...")
_run(env, agent, logger, expname, **kwargs)
logger.close()
if kwargs["with_plot"]:
import figures
files = utils.parse_file(kwargs["out"])
canvas = []
for file in tqdm.tqdm(files):
canvas = figures.plot_single(file, canvas=canvas)
figures.show()
def _case2(skip_sample):
# Case 2 - Different integral time step
print("Case 2")
print("======")
casedir = os.path.join("data", "case2")
paramvar, noisescale = np.random.rand(2)
env = Env(paramvar, noisescale * 0)
env.set_envdir(casedir)
if not skip_sample:
_sample(env, 10, tqdm=True)
samplelist = sorted(glob(os.path.join(env.envdir, "sample-*.h5")))
datalist = [logging.load(path) for path in samplelist]
Tlist = [0.02, 0.5, 2]
agentlist = [NDRADP(2, 1, env)]
agentlist += [IFRADP(2, 1, env)]
agentlist += [RADP(2, 1, T, env) for T in Tlist]
for agent in tqdm(agentlist):
agent.set_datalist(datalist)
agent.set_trainpath(env.envdir)
_train(agent)
def close(self):
super().close()
if self.data_callback is not None:
data = logging.load(self.logger.path)
self.data_callback(self, data)
if done:
break
env.close()
logger.close()
return logger.path
T = 0.5
env = Env(T=T, dt=0.1, max_t=10, ode_step_len=4)
savepath = run(env)
import matplotlib.pyplot as plt
data = logging.load(savepath)
canvas = []
fig, axes = plt.subplots(3, 1, sharex=True)
axes[0].set_ylabel(r"$x_1$")
axes[1].set_ylabel(r"$x_2$")
axes[2].set_ylabel(r"$x_3$")
axes[2].set_xlabel("Time [sec]")
canvas.append((fig, axes))
fig, ax = plt.subplots(1, 1)
ax.set_ylabel(r"$u$")
ax.set_xlabel("Time [sec]")
canvas.append((fig, ax))
time = data["time"]
def draw_plot(dir_env_data, dir_agent_data, dir_save):
env_data, info = logging.load(dir_env_data, with_info=True)
agent_data = logging.load(dir_agent_data)
cfg = info['cfg']
time = env_data['time']
load_pos = env_data['load']['pos']
load_vel = env_data['load']['vel']
# load_dcm = data['load']['dcm']
load_att = np.unwrap(env_data['load_att'], axis=0) * 180/np.pi
load_omega = np.unwrap(env_data['load']['omega'], axis=0) * 180/np.pi
quad_moment = env_data['quad_moment']
quad_pos = env_data['quad_pos']
quad_vel = env_data['quad_vel']
quad_att = np.unwrap(env_data['quad_att'], axis=0) * 180/np.pi
quad_att_des = np.unwrap(env_data['quad_att_des'], axis=0) * 180/np.pi
distance_btw_quads = env_data['distance_btw_quads']
distance_btw_quad2anchor = env_data['distance_btw_quad2anchor']
time_agent = agent_data['time']
action = agent_data['action']
reward = agent_data['reward']
# anchor_pos = env_data['anchor_pos']
# check_dynamics = env_data['check_dynamics']
# breakpoint()
for i in range(cfg.quad.num):
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, quad_pos[:,i,0])
ax[1].plot(time, quad_pos[:,i,1])
ax[2].plot(time, quad_pos[:,i,2])
ax[0].set_title(f"Position of Quadrotor {i}")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("X [m]")
ax[1].set_ylabel("Y [m]")
ax[2].set_ylabel("Z [m]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, f"quad_{i}_pos.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, quad_vel[:,i,0])
ax[1].plot(time, quad_vel[:,i,1])
ax[2].plot(time, quad_vel[:,i,2])
ax[0].set_title(f"Velocity of Quadrotor {i}")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("$V_x$ [m/s]")
ax[1].set_ylabel("$V_y$ [m/s]")
ax[2].set_ylabel("$V_z$ [m/s]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, f"quad_{i}_vel.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, quad_moment[:,i,0])
ax[1].plot(time, quad_moment[:,i,1])
ax[2].plot(time, quad_moment[:,i,2])
ax[0].set_title(f"Moment of Quadrotor {i}")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("$M_x$ [Nm]")
ax[1].set_ylabel("$M_y$ [Nm]")
ax[2].set_ylabel("$M_z$ [Nm]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, f"quad_{i}_moment.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
line1, = ax[0].plot(time, quad_att[:,i,0], 'r')
line2, = ax[0].plot(time, quad_att_des[:,i,0], 'b--')
ax[0].legend(handles=(line1, line2), labels=('true', 'des.'))
ax[1].plot(time, quad_att[:,i,1], 'r',
time, quad_att_des[:,i,1], 'b--')
ax[2].plot(time, quad_att[:,i,2], 'r',
time, quad_att_des[:,i,2], 'b--')
ax[0].set_title(f"Euler angle of Quadrotor {i}")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("$\phi$ [deg]")
ax[1].set_ylabel("$\\theta$ [deg]")
ax[2].set_ylabel("$\psi$ [deg]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, f"quad_{i}_att.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, env_data['quads'][f'quad{i:02d}']['omega'][:,0,:])
ax[1].plot(time, env_data['quads'][f'quad{i:02d}']['omega'][:,1,:])
ax[2].plot(time, env_data['quads'][f'quad{i:02d}']['omega'][:,2,:])
ax[0].set_title(f"Angular velocity of Quadrotor {i}")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("$\omega_x$ [deg/s]")
ax[1].set_ylabel("$\omega_y$ [deg/s]")
ax[2].set_ylabel("$\omega_z$ [deg/s]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, f"quad_{i}_omega.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time_agent, action[:,cfg.quad.num*i])
ax[1].plot(time_agent, action[:,cfg.quad.num*i+1]*180/np.pi)
ax[2].plot(time_agent, action[:,cfg.quad.num*i+2]*180/np.pi)
ax[0].set_title(f"Action of Quadrotor {i}")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("Total thrust [N]")
ax[1].set_ylabel("$chi$ [deg]")
ax[2].set_ylabel("$gamma$ [deg]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, f"quad_{i}_action.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, load_pos[:,0])
ax[1].plot(time, load_pos[:,1])
ax[2].plot(time, load_pos[:,2])
ax[0].set_title("Position of Load")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("X [m]")
ax[1].set_ylabel("Y [m]")
ax[2].set_ylabel("Z [m]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, "load_pos.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, load_vel[:,0])
ax[1].plot(time, load_vel[:,1])
ax[2].plot(time, load_vel[:,2])
ax[0].set_title("Velocity of Load")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("$V_x$ [m/s]")
ax[1].set_ylabel("$V_y$ [m/s]")
ax[2].set_ylabel("$V_z$ [m/s]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, "load_vel.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, load_att[:,0])
ax[1].plot(time, load_att[:,1])
ax[2].plot(time, load_att[:,2])
ax[0].set_title("Euler angle of Load")
ax[0].set_ylabel("$\phi$ [deg]")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[1].set_ylabel("$\\theta$ [deg]")
ax[2].set_ylabel("$\psi$ [deg]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, "load_att.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, load_omega[:,0])
ax[1].plot(time, load_omega[:,1])
ax[2].plot(time, load_omega[:,2])
ax[0].set_title("Angular rate of Load")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("$\omega_x$ [deg/s]")
ax[1].set_ylabel("$\omega_y$ [deg/s]")
ax[2].set_ylabel("$\omega_z$ [deg/s]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, "load_omega.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=3, ncols=1)
ax[0].plot(time, distance_btw_quad2anchor[:,0])
ax[1].plot(time, distance_btw_quad2anchor[:,1])
ax[2].plot(time, distance_btw_quad2anchor[:,2])
ax[0].set_title("Distance between quadrotor and anchor")
ax[0].axes.xaxis.set_ticklabels([])
ax[1].axes.xaxis.set_ticklabels([])
ax[0].set_ylabel("Quad 0 [m]")
ax[1].set_ylabel("Quad 1 [m]")
ax[2].set_ylabel("Quad 2 [m]")
ax[2].set_xlabel("time [s]")
[ax[i].grid(True) for i in range(3)]
[ax[i].set_ylim(cfg.link.len[i]-0.1, cfg.link.len[i]+0.1)
for i in range(cfg.quad.num)]
fig.align_ylabels(ax)
fig.savefig(
Path(dir_save, "distance_btw_quad2anchor.png"),
bbox_inches='tight'
)
plt.close('all')
fig, ax = plt.subplots(nrows=1, ncols=1)
line1, = ax.plot(time, distance_btw_quads[:,1], 'r')
line2, = ax.plot(time, distance_btw_quads[:,2], 'g')
line3, = ax.plot(time, distance_btw_quads[:,0], 'b')
line4, = ax.plot(time, cfg.quad.iscollision*np.ones(len(time)), 'k--')
ax.legend(handles=(line1, line2, line3, line4),
labels=(
'quad0<->quad1',
'quad1<->quad2',
'quad2<->quad0',
'collision criteria'
))
ax.set_title("Distance between quadrotors")
ax.set_ylabel('distance [m]')
ax.set_xlabel("time [s]")
ax.grid(True)
fig.savefig(
Path(dir_save, "distance_btw_quads.png"),
bbox_inches='tight'
)
plt.close('all')
fig, _ = plt.subplots(1, 1, subplot_kw=dict(projection="3d"))
ani = Animator(fig, [env_data], cfg)
ani.animate()
ani.save(Path(dir_save, "animation.mp4"))
color="g",
ls="-.",
label="Fixed trim 2, gain 2",
),
interp=dict(
color="r",
label="Interpolated",
),
)
cnamelist = options.keys()
datalist = {}
for p in pathlist:
for c in cnamelist:
if c in p:
datalist[c] = logging.load(p)
# Figure 1
fig, axes = plt.subplots(2, 2)
ylabels = [
r"$V_T$ [m/s]", r"$\alpha$ [deg]", r"$Q$ [deg/s]", r"$\gamma$ [deg]"
]
factor = [1] + [np.rad2deg(1)] * 3
for i, ax in enumerate(axes.flat):
ln = []
for c in cnamelist:
data = datalist[c]
option = options[c]
plt.show()
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("file", nargs="*")
parser.add_argument("--all", action="store_true")
args = parser.parse_args()
color_cycle = cycler(
color=plt.rcParams["axes.prop_cycle"].by_key()["color"])
path_list = args.file
if args.all:
path_list = sorted(glob.glob(os.path.join(BASE_DATA_DIR, "*.h5")))
else:
path_list = sorted(filter(lambda x: x.endswith(".h5"), path_list))
dataset = []
for path, c in zip(path_list, color_cycle()):
dataset.append(logging.load(path))
names = [os.path.basename(os.path.splitext(path)[0]) for path in path_list]
print("Plotting ...")
print("\n".join(path_list))
plot_mult(dataset, color_cycle, names=names)
def get_data(exp, prefix=BASE_DATA_DIR):
path = os.path.join(prefix, exp + ".h5")
return logging.load(path)
def _plot_case2():
import matplotlib.pyplot as plt
from cycler import cycler
# Case 2
casedir = os.path.join("data", "case2")
samplelist = sorted(glob(os.path.join(casedir, "*", "sample-*.h5")))
trainlist = sorted(glob(os.path.join(casedir, "*", "train-*.h5")))
custom_cycler = (cycler(color=["k"]) * cycler(ls=["-", "--", "-.", ":"]))
# plot samples
fig, axes = plt.subplots(3, 1, sharex=True)
for samplepath, cc in zip(samplelist, custom_cycler()):
data = logging.load(samplepath)
t = data["time"]
x = data["state"].squeeze()
u = data["control"].squeeze()
axes[0].plot(t, x[:, 0], **cc)
axes[1].plot(t, x[:, 1], **cc)
axes[2].plot(t, u, **cc)
axes[0].set_ylabel(r"$x_1$")
axes[1].set_ylabel(r"$x_1$")
axes[2].set_ylabel(r"$u$")
axes[2].set_xlabel(r"Time [s]")
set_axes(fig, axes)
fig, axes = plt.subplots(2, 1, sharex=True)
draw_true(axes)
legendlines = []
for trainpath, cc in zip(trainlist, custom_cycler()):
data, info = logging.load(trainpath, with_info=True)
if info["classname"] == "RADP":
label = rf"RADP ($T = {info['T']}$)"
elif info["classname"] == "IFRADP":
label = r"IFRADP"
cc = dict(color="b", ls="-", zorder=10)
elif info["classname"] == "NDRADP":
label = r"NDRADP"
cc = dict(color="g", ls="-", zorder=10)
epoch = data["epoch"]
wa = data["params"]["wa"].squeeze()
wc = data["params"]["wc"].squeeze()
lines = axes[0].plot(epoch, wc, **cc, label=label)
legendlines.append(lines[0])
axes[1].plot(epoch, wa, **cc)
axes[0].set_ylabel(r"$w_c$")
axes[0].set_xlim(left=0)
axes[1].set_ylabel(r"$w_a$")
axes[1].set_xlabel("Iteration")
fig.legend(handles=legendlines,
bbox_to_anchor=(0.1, 0.92, 0.87, .05),
loc='lower center',
ncol=4,
mode="expand",
borderaxespad=0.)
set_axes(fig, axes)
fig, axes = plt.subplots(2, 1)
legendlines = []
for trainpath, cc in zip(trainlist, custom_cycler()):
data, info = logging.load(trainpath, with_info=True)
if info["classname"] == "RADP":
label = rf"RADP ($T = {info['T']}$)"
elif info["classname"] == "IFRADP":
label = r"IFRADP"
cc = dict(color="b", ls="-", zorder=10)
elif info["classname"] == "NDRADP":
label = r"NDRADP"
cc = dict(color="g", ls="-", zorder=10)
epoch = data["epoch"]
wa = data["params"]["wa"].squeeze()
ewa = np.linalg.norm(wa - [-1, -2], axis=1)
wc = data["params"]["wc"].squeeze()
ewc = np.linalg.norm(wc - [0.5, 1, 0], axis=1)
lines = axes[0].plot(epoch, ewc, **cc, label=label)
legendlines.append(lines[0])
axes[1].plot(epoch, ewa, **cc)
axes[0].set_yscale("log")
axes[1].set_yscale("log")
fig.legend(handles=legendlines,
bbox_to_anchor=(0.1, 0.92, 0.87, .05),
loc='lower center',
ncol=4,
mode="expand",
borderaxespad=0.)
set_axes(fig, axes)
def load(self, path):
data = logging.load(path)
self.load_weights(data)
return int(data["epoch"][-1] + 1), int(data["i"][-1] + 1)
def get_data(exp, base=BASE_DATA_DIR):
path = os.path.join(base, exp, "episodic.h5")
data = logging.load(path)
return data
