From 17b93479767a4727a0b0937ab61a081973621816 Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Mon, 11 Oct 2021 14:31:31 +0200
Subject: [PATCH] Select local_maxima instead of grid

---
 cfg/active_grasp.yaml                      | 4 ++--
 cfg/scenes/{kitchen.yaml => kitchen1.yaml} | 0
 src/active_grasp/policy.py                 | 8 ++++----
 3 files changed, 6 insertions(+), 6 deletions(-)
 rename cfg/scenes/{kitchen.yaml => kitchen1.yaml} (100%)

diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml
index aad4ec8..e2d24e1 100644
--- a/cfg/active_grasp.yaml
+++ b/cfg/active_grasp.yaml
@@ -1,7 +1,7 @@
 bt_sim:
   gui: False
   cam_noise: False
-  scene: kitchen.yaml
+  scene: mustard1.yaml
 
 grasp_controller:
   base_frame_id: panda_link0
@@ -24,5 +24,5 @@ policy:
   window_size: 6
 
 nbv_grasp:
-  max_views: 30
+  max_views: 40
   min_gain: 0.0
diff --git a/cfg/scenes/kitchen.yaml b/cfg/scenes/kitchen1.yaml
similarity index 100%
rename from cfg/scenes/kitchen.yaml
rename to cfg/scenes/kitchen1.yaml
diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py
index 3e3856f..f8abb93 100644
--- a/src/active_grasp/policy.py
+++ b/src/active_grasp/policy.py
@@ -26,7 +26,7 @@ class Policy:
         info_topic = rospy.get_param("~camera/info_topic")
         msg = rospy.wait_for_message(info_topic, CameraInfo, rospy.Duration(2.0))
         self.intrinsic = from_camera_info_msg(msg)
-        self.qual_threshold = rospy.get_param("vgn/qual_threshold")
+        self.qual_thresh = rospy.get_param("vgn/qual_threshold")
 
     def init_robot_model(self):
         self.model = KDLModel.from_parameter_server(self.base_frame, self.cam_frame)
@@ -91,7 +91,7 @@ class Policy:
         return filtered_grasps[i], qualities[i], scores[i]
 
     def score_fn(self, grasp, quality, q, q_grasp):
-        return -np.linalg.norm(q - q_grasp)
+        return grasp.pose.translation[2]
 
 
 class SingleViewPolicy(Policy):
@@ -107,7 +107,7 @@ class SingleViewPolicy(Policy):
             out = self.vgn.predict(tsdf_grid)
             self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
 
-            grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
+            grasps, qualities = select_local_maxima(voxel_size, out, self.qual_thresh)
             grasps, _ = self.sort_grasps(grasps, qualities, q)
 
             if len(grasps) > 0:
@@ -145,7 +145,7 @@ class MultiViewPolicy(Policy):
         self.qual_hist[t, ...] = out.qual
 
         with Timer("grasp_selection"):
-            grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
+            grasps, qualities = select_local_maxima(voxel_size, out, self.qual_thresh)
             grasps, qualities, _ = self.sort_grasps(grasps, qualities, q)
 
         if len(grasps) > 0: