From 7e6eb53fa764536650aab7fdf4ed6ea918cc605e Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Sat, 11 Sep 2021 10:55:48 +0200
Subject: [PATCH] Fix order of setting the arm configuration

---
 active_grasp/simulation.py |  3 ++-
 cfg/scenes/test3.yaml      |  7 -------
 cfg/scenes/test4.yaml      | 13 ++++---------
 cfg/scenes/test5.yaml      | 12 ++++++++++++
 4 files changed, 18 insertions(+), 17 deletions(-)
 delete mode 100644 cfg/scenes/test3.yaml
 create mode 100644 cfg/scenes/test5.yaml

diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py
index a5f3f15..ac56332 100644
--- a/active_grasp/simulation.py
+++ b/active_grasp/simulation.py
@@ -46,9 +46,10 @@ class Simulation:
         self.rng = np.random.default_rng(seed)
 
     def reset(self):
+        self.set_arm_configuration([0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79])
+        self.scene.reset(rng=self.rng)
         q = self.scene.sample_initial_configuration(self.rng)
         self.set_arm_configuration(q)
-        self.scene.reset(rng=self.rng)
         uid = self.select_target()
         bbox = self.get_target_bbox(uid)
         return bbox
diff --git a/cfg/scenes/test3.yaml b/cfg/scenes/test3.yaml
deleted file mode 100644
index 8d08b51..0000000
--- a/cfg/scenes/test3.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-center: [0.45, 0.0, 0.20]
-q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
-objects:
-- object_id: 019_pitcher_base
-  xyz: [0, 0.1, 0]
-  rpy: [0, 0, 60]
-  scale: 1.0
diff --git a/cfg/scenes/test4.yaml b/cfg/scenes/test4.yaml
index c456679..225ed13 100644
--- a/cfg/scenes/test4.yaml
+++ b/cfg/scenes/test4.yaml
@@ -1,12 +1,7 @@
-center: [0.5, 0.05, 0.2]
+center: [0.45, 0.0, 0.20]
 q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
 objects:
-- object_id: 004_sugar_box
-  xyz: [-0.04, -0.03, 0.0]
+- object_id: 019_pitcher_base
+  xyz: [0, 0.1, 0]
   rpy: [0, 0, 30]
-- object_id: 008_pudding_box
-  xyz: [-0.02, -0.06, 0.19]
-  rpy: [0, 0, 45]
-- object_id: 010_potted_meat_can
-  xyz: [0.04, 0.03, 0.0]
-  rpy: [0, 0, 60]
+  scale: 1.0
diff --git a/cfg/scenes/test5.yaml b/cfg/scenes/test5.yaml
new file mode 100644
index 0000000..c456679
--- /dev/null
+++ b/cfg/scenes/test5.yaml
@@ -0,0 +1,12 @@
+center: [0.5, 0.05, 0.2]
+q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
+objects:
+- object_id: 004_sugar_box
+  xyz: [-0.04, -0.03, 0.0]
+  rpy: [0, 0, 30]
+- object_id: 008_pudding_box
+  xyz: [-0.02, -0.06, 0.19]
+  rpy: [0, 0, 45]
+- object_id: 010_potted_meat_can
+  xyz: [0.04, 0.03, 0.0]
+  rpy: [0, 0, 60]