Move on a half sphere

active_grasp/baselines.py

@@ -6,28 +6,24 @@ from .policy import SingleViewPolicy, MultiViewPolicy, compute_error
 class InitialView(SingleViewPolicy):
     def update(self, img, pose):
         self.x_d = pose
-        cmd = super().update(img, pose)
+        super().update(img, pose)
-        return cmd
 
 
 class TopView(SingleViewPolicy):
-    def activate(self, bbox):
+    def activate(self, bbox, view_sphere):
-        super().activate(bbox)
+        super().activate(bbox, view_sphere)
         self.x_d = self.view_sphere.get_view(0.0, 0.0)
-        self.done = False if self.is_view_feasible(self.x_d) else True
+        self.done = False if self.view_sphere.is_feasible(self.x_d) else True
 
 
 class TopTrajectory(MultiViewPolicy):
-    def activate(self, bbox):
+    def activate(self, bbox, view_sphere):
-        super().activate(bbox)
+        super().activate(bbox, view_sphere)
         self.x_d = self.view_sphere.get_view(0.0, 0.0)
-        self.done = False if self.is_view_feasible(self.x_d) else True
+        self.done = False if self.view_sphere.is_feasible(self.x_d) else True
 
     def update(self, img, x):
         self.integrate(img, x)
-        linear, angular = compute_error(self.x_d, x)
+        linear, _ = compute_error(self.x_d, x)
         if np.linalg.norm(linear) < 0.02:
             self.done = True
-            return np.zeros(6)
-        else:
-            return self.compute_velocity_cmd(linear, angular)

active_grasp/bbox.py

@@ -10,6 +10,7 @@ class AABBox:
         self.min = np.asarray(bbox_min)
         self.max = np.asarray(bbox_max)
         self.center = 0.5 * (self.min + self.max)
+        self.size = self.max - self.min
 
     @property
     def corners(self):

active_grasp/controller.py

@@ -14,6 +14,7 @@ from robot_helpers.ros.conversions import *
 from robot_helpers.ros.panda import PandaGripperClient
 from robot_helpers.ros.moveit import MoveItClient
 from robot_helpers.spatial import Rotation, Transform
+from vgn.utils import look_at, cartesian_to_spherical, spherical_to_cartesian
 
 
 class GraspController:
@@ -27,10 +28,13 @@ class GraspController:
 
     def load_parameters(self):
         self.base_frame = rospy.get_param("~base_frame_id")
-        self.ee_frame = rospy.get_param("~ee_frame_id")
+        self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
         self.cam_frame = rospy.get_param("~camera/frame_id")
         self.depth_topic = rospy.get_param("~camera/depth_topic")
-        self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
+        self.min_z_dist = rospy.get_param("~camera/min_z_dist")
+        self.control_rate = rospy.get_param("~control_rate")
+        self.linear_vel = rospy.get_param("~linear_vel")
+        self.policy_rate = rospy.get_param("~policy_rate")
 
     def init_service_proxies(self):
         self.reset_env = rospy.ServiceProxy("reset", Reset)
@@ -44,10 +48,9 @@ class GraspController:
 
     def init_moveit(self):
         self.moveit = MoveItClient("panda_arm")
-        rospy.sleep(1.0)  # wait for connections to be established
+        rospy.sleep(1.0)  # Wait for connections to be established.
         # msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
         # self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))
-        self.policy.moveit = self.moveit
 
     def switch_to_cartesian_velocity_control(self):
         req = SwitchControllerRequest()
@@ -83,17 +86,20 @@ class GraspController:
 
     def reset(self):
         res = self.reset_env(ResetRequest())
-        rospy.sleep(1.0)  # wait for states to be updated
+        rospy.sleep(1.0)  # Wait for the TF tree to be updated.
         return from_bbox_msg(res.bbox)
 
     def search_grasp(self, bbox):
-        self.policy.activate(bbox)
+        self.view_sphere = ViewHalfSphere(bbox, self.min_z_dist, self.moveit)
-        r = rospy.Rate(self.policy.rate)
+        self.policy.activate(bbox, self.view_sphere)
+        timer = rospy.Timer(rospy.Duration(1.0 / self.control_rate), self.send_vel_cmd)
+        r = rospy.Rate(self.policy_rate)
         while not self.policy.done:
             img, pose = self.get_state()
-            cmd = self.policy.update(img, pose)
+            self.policy.update(img, pose)
-            self.cartesian_vel_pub.publish(to_twist_msg(cmd))
             r.sleep()
+        rospy.sleep(0.1)  # Wait for a zero command to be sent to the robot.
+        timer.shutdown()
         return self.policy.best_grasp
 
     def get_state(self):
@@ -102,6 +108,26 @@ class GraspController:
         pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
         return img, pose
 
+    def send_vel_cmd(self, event):
+        if self.policy.x_d is None or self.policy.done:
+            cmd = np.zeros(6)
+        else:
+            x = tf.lookup(self.base_frame, self.cam_frame)
+            cmd = self.compute_velocity_cmd(self.policy.x_d, x)
+        self.cartesian_vel_pub.publish(to_twist_msg(cmd))
+
+    def compute_velocity_cmd(self, x_d, x):
+        # TODO verify that the frames are handled correctly
+        r, theta, phi = cartesian_to_spherical(x.translation - self.view_sphere.center)
+        e_t = x_d.translation - x.translation
+        e_n = (self.view_sphere.center - x.translation) * (r - self.view_sphere.r) / r
+        linear = 1.0 * e_t + 10.0 * e_n
+        scale = np.linalg.norm(linear)
+        linear *= np.clip(scale, 0.0, self.linear_vel) / scale
+        angular = self.view_sphere.get_view(theta, phi).rotation * x.rotation.inv()
+        angular = 0.5 * angular.as_rotvec()
+        return np.r_[linear, angular]
+
     def execute_grasp(self, grasp):
         if not grasp:
             return "aborted"
@@ -131,3 +157,24 @@ class GraspController:
         }
         info.update(Timer.timers)
         return info
+
+
+class ViewHalfSphere:
+    # TODO move feasibility check to a robot interface module
+    def __init__(self, bbox, min_z_dist, moveit):
+        self.center = bbox.center
+        self.r = 0.5 * bbox.size[2] + min_z_dist
+        self.moveit = moveit
+        self.T_cam_ee = tf.lookup("camera_depth_optical_frame", "panda_link8")
+
+    def get_view(self, theta, phi):
+        eye = self.center + spherical_to_cartesian(self.r, theta, phi)
+        up = np.r_[1.0, 0.0, 0.0]
+        return look_at(eye, self.center, up)
+
+    def sample_view(self):
+        raise NotImplementedError
+
+    def is_feasible(self, view):
+        success, _ = self.moveit.plan(view * self.T_cam_ee)
+        return success

active_grasp/nbv.py

@@ -2,59 +2,40 @@ import itertools
 import numpy as np
 import rospy
 
-from .policy import MultiViewPolicy, compute_error
+from .policy import MultiViewPolicy
-from vgn.utils import look_at
 
 
 class NextBestView(MultiViewPolicy):
-    def __init__(self, rate):
+    def __init__(self):
-        super().__init__(rate)
+        super().__init__()
-        self.max_views = 20
+        self.max_views = 40
-        self.min_ig = 10.0
-        self.cost_factor = 10.0
 
-    def activate(self, bbox):
+    def activate(self, bbox, view_sphere):
-        super().activate(bbox)
+        super().activate(bbox, view_sphere)
         self.generate_view_candidates()
 
-    def update(self, img, x):
-        if len(self.views) > self.max_views:
-            self.done = True
-            return np.zeros(6)
-        else:
-            self.integrate(img, x)
-            views = self.view_candidates
-            gains = self.compute_expected_information_gains(views)
-            costs = self.compute_movement_costs(views)
-            utilities = gains / np.sum(gains) - costs / np.sum(costs)
-            self.vis.views(self.base_frame, self.intrinsic, views, utilities)
-            i = np.argmax(utilities)
-            nbv, ig = views[i], gains[i]
-            cmd = self.compute_velocity_cmd(*compute_error(nbv, x))
-            if self.best_grasp:
-                R, t = self.best_grasp.pose.rotation, self.best_grasp.pose.translation
-                if np.linalg.norm(t - x.translation) < self.min_z_dist:
-                    self.done = True
-                    return np.zeros(6)
-
-                center = t
-                eye = R.apply([0.0, 0.0, -0.2]) + t
-                up = np.r_[1.0, 0.0, 0.0]
-                x_d = look_at(eye, center, up)
-                cmd = self.compute_velocity_cmd(*compute_error(x_d, x))
-            return cmd
-
     def generate_view_candidates(self):
         thetas = np.arange(1, 4) * np.deg2rad(30)
         phis = np.arange(1, 6) * np.deg2rad(60)
         self.view_candidates = []
         for theta, phi in itertools.product(thetas, phis):
             view = self.view_sphere.get_view(theta, phi)
-            if self.is_view_feasible(view):
+            if self.view_sphere.is_feasible(view):
                 self.view_candidates.append(view)
 
-    def compute_expected_information_gains(self, views):
+    def update(self, img, x):
-        return [self.ig_fn(v) for v in views]
+        if len(self.views) > self.max_views:
+            self.done = True
+        else:
+            self.integrate(img, x)
+            views = self.view_candidates
+            gains = [self.ig_fn(v) for v in views]
+            costs = [self.cost_fn(v) for v in views]
+            utilities = gains / np.sum(gains) - costs / np.sum(costs)
+            self.vis.views(self.base_frame, self.intrinsic, views, utilities)
+            i = np.argmax(utilities)
+            nbv, _ = views[i], gains[i]
+            self.x_d = nbv
 
     def ig_fn(self, view, downsample=20):
         fx = self.intrinsic.fx / downsample
@@ -88,18 +69,14 @@ class NextBestView(MultiViewPolicy):
                 origin = view.translation
                 direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0]
                 direction = view.rotation.apply(direction / np.linalg.norm(direction))
-
                 # self.vis.rays(self.task_frame, origin, [direction])
                 # rospy.sleep(0.01)
-
                 t, tsdf_prev = t_min, -1.0
                 while t < t_max:
                     p = origin + t * direction
                     t += t_step
-
                     # self.vis.point(self.task_frame, p)
                     # rospy.sleep(0.01)
-
                     index = get_voxel_at(p)
                     if index is not None:
                         i, j, k = index
@@ -117,8 +94,5 @@ class NextBestView(MultiViewPolicy):
 
         return ig
 
-    def compute_movement_costs(self, views):
-        return [self.cost_fn(v) for v in views]
-
     def cost_fn(self, view):
         return 1.0

active_grasp/policy.py

@@ -8,42 +8,31 @@ from robot_helpers.ros import tf
 from robot_helpers.ros.conversions import *
 from vgn.detection import *
 from vgn.perception import UniformTSDFVolume
-from vgn.utils import look_at, spherical_to_cartesian
 
 
 class Policy:
     def __init__(self, rate=5):
-        self.rate = rate
         self.load_parameters()
         self.init_visualizer()
-        self.lookup_transforms()
 
     def load_parameters(self):
         self.base_frame = rospy.get_param("~base_frame_id")
-        self.cam_frame = rospy.get_param("~camera/frame_id")
-        info_topic = rospy.get_param("~camera/info_topic")
-        self.linear_vel = rospy.get_param("~linear_vel")
-        self.min_z_dist = rospy.get_param("~camera/min_z_dist")
-        self.qual_threshold = rospy.get_param("~qual_threshold")
         self.task_frame = "task"
-
+        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")
 
     def init_visualizer(self):
         self.vis = Visualizer()
 
-    def lookup_transforms(self):
+    def activate(self, bbox, view_sphere):
-        self.T_cam_ee = tf.lookup(self.cam_frame, "panda_link8")
-
-    def activate(self, bbox):
         self.vis.clear()
 
         self.bbox = bbox
+        self.view_sphere = view_sphere
         self.vis.bbox(self.base_frame, self.bbox)
 
-        self.view_sphere = ViewSphere(bbox)
-
         self.calibrate_task_frame()
 
         self.tsdf = UniformTSDFVolume(0.3, 40)
@@ -51,6 +40,7 @@ class Policy:
 
         self.views = []
         self.best_grasp = None
+        self.x_d = None
         self.done = False
 
     def calibrate_task_frame(self):
@@ -89,26 +79,12 @@ class Policy:
     def score_fn(self, grasp):
         return grasp.quality
 
-    def is_view_feasible(self, view):
-        # Check whether MoveIt can find a trajectory to the given view
-        success, _ = self.moveit.plan(view * self.T_cam_ee)
-        return success
-
-    def compute_velocity_cmd(self, linear, angular):
-        kp = 4.0
-        linear = kp * linear
-        scale = np.linalg.norm(linear)
-        linear *= np.clip(scale, 0.0, self.linear_vel) / scale
-        return np.r_[linear, angular]
-
 
 class SingleViewPolicy(Policy):
     def update(self, img, x):
-        linear, angular = compute_error(self.x_d, x)
+        linear, _ = compute_error(self.x_d, x)
-
         if np.linalg.norm(linear) < 0.02:
             self.views.append(x)
-
             self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task)
             tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
             self.vis.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud())
@@ -119,14 +95,10 @@ class SingleViewPolicy(Policy):
 
             grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
             grasps, scores = self.sort_grasps(grasps)
-
             self.vis.grasps(self.base_frame, grasps, scores)
 
             self.best_grasp = grasps[0] if len(grasps) > 0 else None
             self.done = True
-            return np.zeros(6)
-        else:
-            return self.compute_velocity_cmd(linear, angular)
 
 
 class MultiViewPolicy(Policy):
@@ -152,21 +124,6 @@ class MultiViewPolicy(Policy):
         self.vis.grasps(self.base_frame, grasps, scores)
 
 
-class ViewSphere:
-    # Define sphere for view generation on top of the bbox.
-    # TODO an ellipse around the bbox's center would be even nicer ;)
-
-    def __init__(self, bbox):
-        self.center = np.r_[bbox.center[:2], bbox.max[2]]
-        self.r = rospy.get_param("~camera/min_z_dist")
-        self.target = bbox.center
-
-    def get_view(self, theta, phi):
-        eye = self.center + spherical_to_cartesian(self.r, theta, phi)
-        up = np.r_[1.0, 0.0, 0.0]
-        return look_at(eye, self.target, up)
-
-
 def compute_error(x_d, x):
     linear = x_d.translation - x.translation
     angular = (x_d.rotation * x.rotation.inv()).as_rotvec()

cfg/active_grasp.yaml

@@ -1,20 +1,21 @@
 bt_sim:
-  gui: False
+  gui: True
   cam_noise: True
-  scene: test.yaml
+  scene: test1.yaml
 
 grasp_controller:
   base_frame_id: panda_link0
-  ee_frame_id: panda_hand
   ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to the moveit ee
+  control_rate: 30
   linear_vel: 0.05
-  qual_threshold: 0.9
+  policy_rate: 5
   camera:
-    frame_id: camera_optical_frame
+    frame_id: camera_depth_optical_frame
     info_topic: /camera/depth/camera_info
     depth_topic: /camera/depth/image_raw
-    min_z_dist: 0.26
+    min_z_dist: 0.3
     
 vgn:
   model: $(find vgn)/assets/models/vgn_conv.pth
   finger_depth: 0.05
+  qual_threshold: 0.9

scripts/run.py

@@ -18,11 +18,12 @@ def main():
     parser = create_parser()
     args = parser.parse_args()
 
-    policy = make(args.policy, args.rate)
+    policy = make(args.policy)
     controller = GraspController(policy)
     logger = Logger(args)
 
     seed_simulation(args.seed)
+    rospy.sleep(1.0)  # Prevents a rare race condiion
 
     for _ in tqdm(range(args.runs)):
         info = controller.run()
@@ -34,7 +35,6 @@ def create_parser():
     parser.add_argument("policy", type=str, choices=registry.keys())
     parser.add_argument("--runs", type=int, default=100)
     parser.add_argument("--logdir", type=Path, default="logs")
-    parser.add_argument("--rate", type=int, default=5)
     parser.add_argument("--seed", type=int, default=1)
     return parser
 
@@ -42,10 +42,9 @@ def create_parser():
 class Logger:
     def __init__(self, args):
         stamp = datetime.now().strftime("%y%m%d-%H%M%S")
-        name = "{}_policy={},rate={},seed={}.csv".format(
+        name = "{}_policy={},seed={}.csv".format(
             stamp,
             args.policy,
-            args.rate,
             args.seed,
         )
         self.path = args.logdir / name