From 184b380ea963692aea8e4dfd68b98ee853c56c82 Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Mon, 6 Sep 2021 13:36:14 +0200
Subject: [PATCH] Change score function

---
 active_grasp/baselines.py  |  6 +++---
 active_grasp/controller.py |  1 +
 active_grasp/nbv.py        | 11 +++++------
 active_grasp/policy.py     |  3 ++-
 4 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py
index 94a9abb..a3f56c6 100644
--- a/active_grasp/baselines.py
+++ b/active_grasp/baselines.py
@@ -16,7 +16,7 @@ class InitialView(SingleViewPolicy):
 class TopView(SingleViewPolicy):
     def activate(self, bbox):
         super().activate(bbox)
-        eye = np.r_[self.center[:2], self.center[2] + self.min_z_dist]
+        eye = np.r_[self.center[:2], self.center[2] + 0.3]
         up = np.r_[1.0, 0.0, 0.0]
         self.x_d = look_at(eye, self.center, up)
 
@@ -24,7 +24,7 @@ class TopView(SingleViewPolicy):
 class TopTrajectory(MultiViewPolicy):
     def activate(self, bbox):
         super().activate(bbox)
-        eye = np.r_[self.center[:2], self.center[2] + self.min_z_dist]
+        eye = np.r_[self.center[:2], self.center[2] + 0.3]
         up = np.r_[1.0, 0.0, 0.0]
         self.x_d = look_at(eye, self.center, up)
 
@@ -42,7 +42,7 @@ class CircularTrajectory(MultiViewPolicy):
     def __init__(self, rate):
         super().__init__(rate)
         self.r = 0.12
-        self.h = self.min_z_dist
+        self.h = 0.3
         self.duration = 2.0 * np.pi * self.r / self.linear_vel
         self.m = scipy.interpolate.interp1d([0.0, self.duration], [np.pi, 3.0 * np.pi])
 
diff --git a/active_grasp/controller.py b/active_grasp/controller.py
index 2eb6b5a..8ae646e 100644
--- a/active_grasp/controller.py
+++ b/active_grasp/controller.py
@@ -121,6 +121,7 @@ class GraspController:
         rot = T_base_grasp.rotation
         if rot.as_matrix()[:, 0][0] < 0:  # Ensure that the camera is pointing forward
             T_base_grasp.rotation = rot * Rotation.from_euler("z", np.pi)
+        T_base_grasp *= Transform.t([0.0, 0.0, 0.01])
         return T_base_grasp
 
     def collect_info(self, result):
diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py
index 915fa6d..79d3838 100644
--- a/active_grasp/nbv.py
+++ b/active_grasp/nbv.py
@@ -1,7 +1,6 @@
 import itertools
 import numpy as np
-from numpy.lib.twodim_base import eye
-from scipy.ndimage.measurements import center_of_mass
+import rospy
 
 from .policy import MultiViewPolicy
 from vgn.utils import look_at, spherical_to_cartesian
@@ -30,9 +29,9 @@ class NextBestView(MultiViewPolicy):
             i = np.argmax(utilities)
             nbv, ig = views[i], gains[i]
 
-            if ig < self.min_ig:
-                self.done = True
-                return np.zeros(6)
+            # if ig < self.min_ig:
+            #     self.done = True
+            #     return np.zeros(6)
 
             cmd = self.compute_velocity_cmd(*self.compute_error(nbv, x))
 
@@ -51,7 +50,7 @@ class NextBestView(MultiViewPolicy):
             return cmd
 
     def generate_views(self):
-        r, h = 0.14, 0.2
+        r, h = 0.18, 0.2
         thetas = np.arange(1, 4) * np.deg2rad(30)
         phis = np.arange(1, 6) * np.deg2rad(60)
         views = []
diff --git a/active_grasp/policy.py b/active_grasp/policy.py
index aca0e99..4ea9323 100644
--- a/active_grasp/policy.py
+++ b/active_grasp/policy.py
@@ -78,7 +78,8 @@ class Policy:
         return grasps[indices], scores[indices]
 
     def score_fn(self, grasp):
-        return grasp.quality
+        # return grasp.quality
+        return grasp.pose.translation[2]
 
     def update(self, img, pose):
         raise NotImplementedError