From 59ce141072f1ac45e2924142534bd2aa0b549fee Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Tue, 14 Sep 2021 10:28:31 +0200
Subject: [PATCH] Fix mustard scenes

---
 active_grasp/policy.py                   | 7 +++----
 cfg/scenes/{test1.yaml => mustard1.yaml} | 2 +-
 cfg/scenes/{test2.yaml => mustard2.yaml} | 6 +++---
 3 files changed, 7 insertions(+), 8 deletions(-)
 rename cfg/scenes/{test1.yaml => mustard1.yaml} (84%)
 rename cfg/scenes/{test2.yaml => mustard2.yaml} (56%)

diff --git a/active_grasp/policy.py b/active_grasp/policy.py
index 16f1222..4e76908 100644
--- a/active_grasp/policy.py
+++ b/active_grasp/policy.py
@@ -109,13 +109,12 @@ class SingleViewPolicy(Policy):
             self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
 
             grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
-            grasps, scores = self.sort_grasps(grasps, q)
+            grasps, _ = self.sort_grasps(grasps, q)
 
             if len(grasps) > 0:
-                smin, smax = np.min(scores), np.max(scores)
                 self.best_grasp = grasps[0]
-                self.vis.grasps(self.base_frame, grasps, scores, smin, smax)
-                self.vis.best_grasp(self.base_frame, grasps[0], scores[0], smin, smax)
+                self.vis.grasps(self.base_frame, grasps)
+                self.vis.best_grasp(self.base_frame, self.best_grasp)
 
             self.done = True
 
diff --git a/cfg/scenes/test1.yaml b/cfg/scenes/mustard1.yaml
similarity index 84%
rename from cfg/scenes/test1.yaml
rename to cfg/scenes/mustard1.yaml
index 5f91107..1a986ce 100644
--- a/cfg/scenes/test1.yaml
+++ b/cfg/scenes/mustard1.yaml
@@ -2,5 +2,5 @@ center: [0.5, 0.1, 0.20]
 q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
 objects:
 - object_id: 006_mustard_bottle
-  xyz: [0.05, 0.1, 0.0]
+  xyz: [0.0, 0.1, 0.0]
   rpy: [0, 0, -50]
diff --git a/cfg/scenes/test2.yaml b/cfg/scenes/mustard2.yaml
similarity index 56%
rename from cfg/scenes/test2.yaml
rename to cfg/scenes/mustard2.yaml
index 6145ec6..9a97dbc 100644
--- a/cfg/scenes/test2.yaml
+++ b/cfg/scenes/mustard2.yaml
@@ -1,6 +1,6 @@
-center: [0.5, 0.0, 0.40]
+center: [0.5, 0.1, 0.40]
 q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
 objects:
 - object_id: 006_mustard_bottle
-  xyz: [0.05, 0.1, 0.0]
-  rpy: [0, 0, -50]
+  xyz: [0.0, 0.1, 0.0]
+  rpy: [0, 0, 130]