elevator task, remove t265 recording

agilexrobotics · Nov 27, 2023 · 739ff73 · 739ff73
1 parent acc201e
commit 739ff73
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 13 deletions.
diff --git a/aloha_scripts/constants.py b/aloha_scripts/constants.py
@@ -63,6 +63,12 @@
         'num_episodes': 50,
         'episode_len': 400,
         'camera_names': ['cam_high', 'cam_left_wrist', 'cam_right_wrist']
+    },
+    'aloha_mobile_elevator':{
+        'dataset_dir': DATA_DIR + '/aloha_mobile_elevator',
+        'num_episodes': 50,
+        'episode_len': 8500,
+        'camera_names': ['cam_high', 'cam_left_wrist', 'cam_right_wrist']
     }
 }
 

diff --git a/aloha_scripts/real_env.py b/aloha_scripts/real_env.py
@@ -52,7 +52,7 @@ def __init__(self, init_node, setup_robots=True, setup_base=False):
         if setup_base:
             self.setup_base()
 
-        self.setup_t265()
+        # self.setup_t265()
         self.setup_dxl()
 
         self.recorder_left = Recorder('left', init_node=False)
@@ -109,6 +109,7 @@ def get_images(self):
         return self.image_recorder.get_images()
 
     def get_base_vel_t265(self):
+        raise NotImplementedError
         frames = self.pipeline.wait_for_frames()
         pose_frame = frames.get_pose_frame()
         pose = pose_frame.get_pose_data()
@@ -160,7 +161,7 @@ def get_observation(self, get_tracer_vel=False):
         obs['qvel'] = self.get_qvel()
         obs['effort'] = self.get_effort()
         obs['images'] = self.get_images()
-        obs['base_vel_t265'] = self.get_base_vel_t265()
+        # obs['base_vel_t265'] = self.get_base_vel_t265()
         obs['base_vel'] = self.get_base_vel()
         if get_tracer_vel:
             obs['tracer_vel'] = self.get_tracer_vel()

diff --git a/aloha_scripts/record_episodes.py b/aloha_scripts/record_episodes.py
@@ -103,7 +103,7 @@ def capture_one_episode(dt, max_timesteps, camera_names, dataset_dir, dataset_na
         timesteps.append(ts)
         actions.append(action)
         actual_dt_history.append([t0, t1, t2])
-        time.sleep(DT - (time.time() - t0))
+        time.sleep(max(0, DT - (time.time() - t0)))
     print(f'Avg fps: {max_timesteps / (time.time() - time0)}')
 
     # Torque on both master bots
@@ -140,7 +140,7 @@ def capture_one_episode(dt, max_timesteps, camera_names, dataset_dir, dataset_na
         '/observations/effort': [],
         '/action': [],
         '/base_action': [],
-        '/base_action_t265': [],
+        # '/base_action_t265': [],
     }
     for cam_name in camera_names:
         data_dict[f'/observations/images/{cam_name}'] = []
@@ -154,18 +154,18 @@ def capture_one_episode(dt, max_timesteps, camera_names, dataset_dir, dataset_na
         data_dict['/observations/effort'].append(ts.observation['effort'])
         data_dict['/action'].append(action)
         data_dict['/base_action'].append(ts.observation['base_vel'])
-        data_dict['/base_action_t265'].append(ts.observation['base_vel_t265'])
+        # data_dict['/base_action_t265'].append(ts.observation['base_vel_t265'])
         for cam_name in camera_names:
             data_dict[f'/observations/images/{cam_name}'].append(ts.observation['images'][cam_name])
 
     # plot /base_action vs /base_action_t265
-    import matplotlib.pyplot as plt
-    plt.plot(np.array(data_dict['/base_action'])[:, 0], label='base_action_linear')
-    plt.plot(np.array(data_dict['/base_action'])[:, 1], label='base_action_angular')
-    plt.plot(np.array(data_dict['/base_action_t265'])[:, 0], '--', label='base_action_t265_linear')
-    plt.plot(np.array(data_dict['/base_action_t265'])[:, 1], '--', label='base_action_t265_angular')
-    plt.legend()
-    plt.savefig('record_episodes_vel_debug.png', dpi=300)
+    # import matplotlib.pyplot as plt
+    # plt.plot(np.array(data_dict['/base_action'])[:, 0], label='base_action_linear')
+    # plt.plot(np.array(data_dict['/base_action'])[:, 1], label='base_action_angular')
+    # plt.plot(np.array(data_dict['/base_action_t265'])[:, 0], '--', label='base_action_t265_linear')
+    # plt.plot(np.array(data_dict['/base_action_t265'])[:, 1], '--', label='base_action_t265_angular')
+    # plt.legend()
+    # plt.savefig('record_episodes_vel_debug.png', dpi=300)
 
     COMPRESS = True
 
@@ -219,7 +219,7 @@ def capture_one_episode(dt, max_timesteps, camera_names, dataset_dir, dataset_na
         _ = obs.create_dataset('effort', (max_timesteps, 14))
         _ = root.create_dataset('action', (max_timesteps, 14))
         _ = root.create_dataset('base_action', (max_timesteps, 2))
-        _ = root.create_dataset('base_action_t265', (max_timesteps, 2))
+        # _ = root.create_dataset('base_action_t265', (max_timesteps, 2))
 
         for name, array in data_dict.items():
             root[name][...] = array