def visualize_data_continuous(root_path):
# load the preprocessed data
data_file = root_path + "/raw_transformed.pkl"
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
# create 4 different plots
fig, axes = plt.subplots(2, 2, figsize=(10, 10))
# first simply plot out all trajectories
for pose in pose_all:
axes[0, 0].plot(pose[:, 0], pose[:, 1])
# then plot dir vs. feedback
dir_out = []
mag_out = []
th_out = []
for pose in pose_all:
# average the last 10 samples
pos_avg = np.mean(pose[-10:, 0:2], axis=0)
dir_out.append(np.arctan2(pos_avg[1], pos_avg[0]))
mag_out.append(np.linalg.norm(pos_avg))
th_out.append(wrap_to_pi(np.mean(pose[-10:, 2])))
dir_in = protocol_data[:, 0] * np.pi / 180.0 - np.pi * 0.5
dir_out = wrap_to_pi(np.asarray(dir_out))
dir_in = wrap_to_pi(dir_in)
axes[0, 1].scatter(np.rad2deg(dir_in), np.rad2deg(dir_out))
# plot mag vs. feedback
axes[1, 0].scatter(np.rad2deg(dir_in), np.asarray(mag_out))
# plot th vs. feedback
axes[1, 1].scatter(dir_out, np.asarray(th_out))
# 3D scatter of dir vs. feedback
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ri = 5.0
ro = 2.0
x = (ri + ro * np.cos(dir_out)) * np.cos(dir_in)
y = (ri + ro * np.cos(dir_out)) * np.sin(dir_in)
z = ro * np.sin(dir_out)
ax.scatter(x, y, z)
plt.show()
def visualize_data_continuous(root_path):
# load the preprocessed data
data_file = root_path + "/raw_transformed.pkl"
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
# create 4 different plots
fig, axes = plt.subplots(2, 2, figsize=(10, 10))
# first simply plot out all trajectories
for pose in pose_all:
axes[0, 0].plot(pose[:, 0], pose[:, 1])
# then plot dir vs. feedback
dir_out = []
mag_out = []
th_out = []
for pose in pose_all:
# average the last 10 samples
pos_avg = np.mean(pose[-10:, 0:2], axis=0)
dir_out.append(np.arctan2(pos_avg[1], pos_avg[0]))
mag_out.append(np.linalg.norm(pos_avg))
th_out.append(wrap_to_pi(np.mean(pose[-10:, 2])))
dir_in = protocol_data[:, 0] * np.pi / 180.0 - np.pi * 0.5
dir_out = wrap_to_pi(np.asarray(dir_out))
dir_in = wrap_to_pi(dir_in)
axes[0, 1].scatter(np.rad2deg(dir_in), np.rad2deg(dir_out))
# plot mag vs. feedback
axes[1, 0].scatter(np.rad2deg(dir_in), np.asarray(mag_out))
# plot th vs. feedback
axes[1, 1].scatter(dir_out, np.asarray(th_out))
# 3D scatter of dir vs. feedback
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ri = 5.0
ro = 2.0
x = (ri + ro * np.cos(dir_out)) * np.cos(dir_in)
y = (ri + ro * np.cos(dir_out)) * np.sin(dir_in)
z = ro * np.sin(dir_out)
ax.scatter(x, y, z)
plt.show()
def load_data(self, data_file):
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
self.X = []
self.y = []
for t, pose, comm in zip(t_all, pose_all, protocol_data):
# first down-sample the data
step = int(self.dt / 0.025)
t = t[::step]
pose = pose[::step, :]
# transform data w.r.t to the initial pose
# assuming that the initial pose is always aligned with world frame
# otherwise the position needs to be rotated
pose -= pose[0]
# convert angles to rad and wrap to pi
pose[:, 2] = np.deg2rad(pose[:, 2])
pose[:, 2] = wrap_to_pi(pose[:, 2])
# convert communication direction to ang
comm[0] *= np.pi * 0.25
comm_vec = np.array([np.cos(comm[0]),
np.sin(comm[0])]) * (comm[1] + 1)
# input (state) is defined as (comm(t'), t-t')
# output is defined as (pose(t+1))
for i in range(len(t) - 1):
x = np.array([comm_vec[0], comm_vec[1], i])
y = pose[i + 1].copy()
self.X.append(x)
self.y.append(y)
def load_data(self, data_file):
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
self.X = []
self.y = []
for t, pose, comm in zip(t_all, pose_all, protocol_data):
# first down-sample the data
step = int(self.dt / 0.025)
t = t[::step]
pose = pose[::step, :]
# transform data w.r.t to the initial pose
# assuming that the initial pose is always aligned with world frame
# otherwise the position needs to be rotated
pose -= pose[0]
# convert angles to rad and wrap to pi
pose[:, 2] = np.deg2rad(pose[:, 2])
pose[:, 2] = wrap_to_pi(pose[:, 2])
# convert communication direction to ang
comm[0] *= np.pi * 0.25
comm_vec = np.array([np.cos(comm[0]), np.sin(comm[0])]) * (comm[1] + 1)
# input (state) is defined as (comm(t'), t-t')
# output is defined as (pose(t+1))
for i in range(len(t) - 1):
x = np.array([comm_vec[0], comm_vec[1], i])
y = pose[i+1].copy()
self.X.append(x)
self.y.append(y)
def load_data(self, data_file):
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
self.X = []
self.y = [[], [], []]
for t, pose, comm in zip(t_all, pose_all, protocol_data):
if int(comm[0]) != self.comm[0] or int(comm[1]) != self.comm[1]:
continue
# first down-sample the data
step = int(self.dt / 0.025)
t = t[::step]
pose = pose[::step, :]
# transform data w.r.t to the initial pose
# assuming that the initial pose is always aligned with world frame
# otherwise the position needs to be rotated
pose -= pose[0]
# convert angles to rad and wrap to pi
pose[:, 2] = np.deg2rad(pose[:, 2])
pose[:, 2] = wrap_to_pi(pose[:, 2])
# convert communication direction to ang
comm[0] *= np.pi * 0.25
comm[0] = wrap_to_pi(comm[0])
# input (state) is defined as (x, y, t-t')
# output is defined as (dx, dy)
for i in range(len(t)-1):
x = np.array([pose[i, 0], pose[i, 1], i * self.dt])
y = pose[i+1] - pose[i]
self.X.append(x)
for dim in range(self.dim):
self.y[dim].append(y[dim])
def load_data(self, data_file):
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
self.X = []
self.y = [[], [], []]
for t, pose, comm in zip(t_all, pose_all, protocol_data):
if int(comm[0]) != self.comm[0] or int(comm[1]) != self.comm[1]:
continue
# first down-sample the data
step = int(self.dt / 0.025)
t = t[::step]
pose = pose[::step, :]
# transform data w.r.t to the initial pose
# assuming that the initial pose is always aligned with world frame
# otherwise the position needs to be rotated
pose -= pose[0]
# convert angles to rad and wrap to pi
pose[:, 2] = np.deg2rad(pose[:, 2])
pose[:, 2] = wrap_to_pi(pose[:, 2])
# convert communication direction to ang
comm[0] *= np.pi * 0.25
comm[0] = wrap_to_pi(comm[0])
# input (state) is defined as (x, y, t-t')
# output is defined as (dx, dy)
for i in range(len(t) - 1):
x = np.array([pose[i, 0], pose[i, 1], i * self.dt])
y = pose[i + 1] - pose[i]
self.X.append(x)
for dim in range(self.dim):
self.y[dim].append(y[dim])
def load_data(self, data_file):
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
self.X = []
self.y = []
for t, pose, comm in zip(t_all, pose_all, protocol_data):
if int(comm[0]) != self.comm[0]:
continue
if int(comm[1]) != self.comm[1]:
continue
# first down-sample the data
step = int(self.dt / 0.025)
t = t[::step]
pose = pose[::step, :]
# transform data w.r.t to the initial pose
# assuming that the initial pose is always aligned with world frame
# otherwise the position needs to be rotated
pose -= pose[0]
# convert angles to rad and wrap to pi
pose[:, 2] = np.deg2rad(pose[:, 2])
pose[:, 2] = wrap_to_pi(pose[:, 2])
# input (state) is defined as (pose(t), comm(t'), t-t')
# output is defined as (pose(t+1))
for i in range(len(t)):
x = np.array([i])
y = pose[i].copy()
self.X.append(x)
self.y.append(y)
def load_data(self, data_file):
with open(data_file) as f:
t_all, pose_all, protocol_data = pickle.load(f)
self.X = []
self.y = []
for t, pose, comm in zip(t_all, pose_all, protocol_data):
if int(comm[0]) != self.comm[0]:
continue
if int(comm[1]) != self.comm[1]:
continue
# first down-sample the data
step = int(self.dt / 0.025)
t = t[::step]
pose = pose[::step, :]
# transform data w.r.t to the initial pose
# assuming that the initial pose is always aligned with world frame
# otherwise the position needs to be rotated
pose -= pose[0]
# convert angles to rad and wrap to pi
pose[:, 2] = np.deg2rad(pose[:, 2])
pose[:, 2] = wrap_to_pi(pose[:, 2])
# input (state) is defined as (pose(t), comm(t'), t-t')
# output is defined as (pose(t+1))
for i in range(len(t)):
x = np.array([i])
y = pose[i].copy()
self.X.append(x)
self.y.append(y)