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)