freeze for data collection- ws

Author: danfeiX

cfg/panda.yaml

@@ -14,7 +14,7 @@ panda:
   # neutral_joint_angles:
   #  [0., -0.3135, 0., -2.515, 0., 2.226, 0.87]
   neutral_joint_angles: [0.0, -0.524, 0.0, -2.617, 0.0, 2.094, 0.0]
-
+  # neutral_joint_angles: [-0.16172688795543, 0.446162338187715, -0.211960327838597, -2.49406191371722, -0.0575494087272164, 2.94740106578862, 0.622476284949879]
   limb_joint_names: [
   'panda_link0',
   'panda_link1',

demos/demo.py

@@ -77,9 +77,6 @@ def world_type(self):
     def get_action_list(self):
         raise NotImplementedError
 
-    def run(self):
-        raise NotImplementedError
-
     def save_data(self):
         fpath = "demos/results/{}.npz".format(self.demo_name)
         np.savez(fpath, states=self.states,
@@ -134,7 +131,7 @@ def run(self):
 
     def step_through_demo(self):
         for i, goal_pose in enumerate(self.goal_poses):
-
+            print(self.env.robot_interface.ee_position)
             # Get action corresponding to test_fn and goal pose
             action= self.get_action(goal_pose, self.get_state())
             action_kwargs = self.get_action_kwargs(action)

demos/demo_control_cfg.yaml

@@ -1,7 +1,7 @@
 # Cfg file for  Demo control environment
 world:
   type: 'Bullet' #'Bullet' or 'Real'
-  robot: 'sawyer'
+  robot: 'panda'
   controlType: 'EEImpedance'
 data_dir: 'data'
 

demos/osc_demo.py

@@ -157,7 +157,7 @@ def get_action(self, goal_pose, current_pose):
                 as quaternion.
         """
         if self.test_fn =="move_ee_delta":
-            action = get_delta(goal_pose, current_pose)
+            action = self.get_delta(goal_pose, current_pose)
         elif self.test_fn =="set_ee_pose":
             action = goal_pose
         else:

perls2/robots/real_panda_interface.py

@@ -63,7 +63,7 @@ def reset(self):
 
         self.redisClient.mset(reset_cmd)
         # Wait for reset to be read by contrl interface.
-        time.sleep(self.RESET_TIMEOUT)
+        # time.sleep(self.RESET_TIMEOUT)
         start = time.time()
 
         while (self.redisClient.get(ROBOT_RESET_COMPL_KEY) != b'True' and (time.time() - start < self.RESET_TIMEOUT)):