From a789fd121bded377e103618cca19bea7d2f6379e Mon Sep 17 00:00:00 2001
From: svl-pair <morphine1021@gmail.com>
Date: Fri, 28 May 2021 18:24:49 -0700
Subject: [PATCH] freeze for data collection- ws

---
 cfg/panda.yaml                        | 2 +-
 demos/demo.py                         | 5 +----
 demos/demo_control_cfg.yaml           | 2 +-
 demos/osc_demo.py                     | 2 +-
 perls2/robots/real_panda_interface.py | 2 +-
 5 files changed, 5 insertions(+), 8 deletions(-)

diff --git a/cfg/panda.yaml b/cfg/panda.yaml
index 3d57ca05..d195fa7f 100644
--- a/cfg/panda.yaml
+++ b/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',
diff --git a/demos/demo.py b/demos/demo.py
index b3f967b7..89597cdc 100644
--- a/demos/demo.py
+++ b/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)
diff --git a/demos/demo_control_cfg.yaml b/demos/demo_control_cfg.yaml
index c2f843fa..f9601c0f 100644
--- a/demos/demo_control_cfg.yaml
+++ b/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'
 
diff --git a/demos/osc_demo.py b/demos/osc_demo.py
index 35c6e4ff..0305ebc3 100644
--- a/demos/osc_demo.py
+++ b/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:
diff --git a/perls2/robots/real_panda_interface.py b/perls2/robots/real_panda_interface.py
index ba31b58a..4aebe072 100644
--- a/perls2/robots/real_panda_interface.py
+++ b/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)):