From 44190330fea308d2386b77302525cc6f0dc97ce0 Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Thu, 8 Jul 2021 09:36:51 +0200
Subject: [PATCH] Random view baseline

---
 active_grasp/__init__.py  |  1 +
 active_grasp/baselines.py | 34 ++++++++++++++++++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/active_grasp/__init__.py b/active_grasp/__init__.py
index e7a1e5b..548204f 100644
--- a/active_grasp/__init__.py
+++ b/active_grasp/__init__.py
@@ -3,4 +3,5 @@ from .baselines import *
 
 register("single-view", SingleViewBaseline)
 register("top", TopBaseline)
+register("random", RandomBaseline)
 register("fixed-trajectory", FixedTrajectoryBaseline)
diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py
index 50937e2..092d232 100644
--- a/active_grasp/baselines.py
+++ b/active_grasp/baselines.py
@@ -45,6 +45,40 @@ class TopBaseline(BasePolicy):
             return self.target
 
 
+class RandomBaseline(BasePolicy):
+    """
+    Move the camera to a random viewpoint on a circle centered above the target.
+    """
+
+    def __init__(self):
+        super().__init__()
+        self.r = 0.06
+        self.h = 0.3
+
+    def activate(self, bbox):
+        super().activate(bbox)
+        circle_center = (bbox.min + bbox.max) / 2.0
+        circle_center[2] += self.h
+        t = np.random.uniform(np.pi, 3.0 * np.pi)
+        eye = circle_center + np.r_[self.r * np.cos(t), self.r * np.sin(t), 0]
+        center = (self.bbox.min + self.bbox.max) / 2.0
+        up = np.r_[1.0, 0.0, 0.0]
+        self.target = self.T_B_task * (self.T_EE_cam * look_at(eye, center, up)).inv()
+
+    def update(self):
+        current = tf.lookup(self.base_frame, self.ee_frame)
+        error = current.translation - self.target.translation
+
+        if np.linalg.norm(error) < 0.01:
+            self.best_grasp = self.predict_best_grasp()
+            self.done = True
+        else:
+            self.integrate_latest_image()
+            self.draw_scene_cloud()
+            self.draw_camera_path()
+            return self.target
+
+
 class FixedTrajectoryBaseline(BasePolicy):
     """
     Follow a pre-defined circular trajectory centered above the target object.