Reject grasps outside the target bbox

bt_sim_node.py

@@ -14,6 +14,7 @@ import tf2_ros
 import active_grasp.srv
 from robot_utils.ros.conversions import *
 from simulation import Simulation
+from utils import *
 
 
 class BtSimNode:
@@ -46,7 +47,7 @@ class BtSimNode:
         self.reset_requested = True
         rospy.sleep(1.0)  # wait for the latest sim step to finish
         bbox = self.sim.reset()
-        res = active_grasp.srv.ResetResponse(bbox.to_msg())
+        res = active_grasp.srv.ResetResponse(to_bbox_msg(bbox))
         self.step_cnt = 0
         self.reset_requested = False
         return res

controller.py

@@ -18,16 +18,16 @@ class GraspController:
         self.T_G_EE = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
 
     def run(self):
-        self.reset()
+        bbox = self.reset()
-        grasp = self.explore()
+        grasp = self.explore(bbox)
         if grasp:
             self.execute_grasp(grasp)
 
     def reset(self):
         raise NotImplementedError
 
-    def explore(self):
+    def explore(self, bbox):
-        self.policy.activate()
+        self.policy.activate(bbox)
         r = rospy.Rate(self.policy.rate)
         while True:
             cmd = self.policy.update()

launch/active_grasp.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /TF1/Tree1
       Splitter Ratio: 0.5
-    Tree Height: 547
+    Tree Height: 595
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -173,23 +173,23 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
+        panda_link0:
+          panda_link1:
+            panda_link2:
+              panda_link3:
+                panda_link4:
+                  panda_link5:
+                    panda_link6:
+                      panda_link7:
+                        panda_link8:
+                          panda_hand:
+                            cam_optical_frame:
+                              {}
+                            panda_leftfinger:
+                              {}
+                            panda_rightfinger:
+                              {}
         world:
-          panda_link0:
-            panda_link1:
-              panda_link2:
-                panda_link3:
-                  panda_link4:
-                    panda_link5:
-                      panda_link6:
-                        panda_link7:
-                          panda_link8:
-                            panda_hand:
-                              cam_optical_frame:
-                                {}
-                              panda_leftfinger:
-                                {}
-                              panda_rightfinger:
-                                {}
           task:
             {}
       Update Interval: 0
@@ -247,7 +247,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Spheres
-      Topic: /points
+      Topic: /cloud
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -268,6 +268,14 @@ Visualization Manager:
       Topic: /command
       Unreliable: false
       Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /bbox
+      Name: BBox
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /path
@@ -279,7 +287,7 @@ Visualization Manager:
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /grasps
-      Name: PredictedGrasps
+      Name: Grasps
       Namespaces:
         {}
       Queue Size: 100
@@ -312,7 +320,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.5376757383346558
+      Distance: 1.1227775812149048
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -320,25 +328,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.192186176776886
+        X: 0.2026568502187729
-        Y: 0.14037109911441803
+        Y: 0.13089965283870697
-        Z: 0.3879348933696747
+        Z: 0.3290979564189911
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.21479728817939758
+      Pitch: 0.3197976350784302
       Target Frame: <Fixed Frame>
-      Yaw: 5.193514823913574
+      Yaw: 5.398510456085205
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 844
+  Height: 892
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002aefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000397000002ae00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fb0000003efc0100000002fb0000000800540069006d00650100000000000004fb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039f000002de00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -347,6 +355,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1267
+  Width: 1275
-  X: 100
+  X: 61
-  Y: 457
+  Y: 200

policies.py

@@ -2,6 +2,7 @@ import cv_bridge
 import numpy as np
 from pathlib import Path
 import rospy
+from rospy import Publisher
 import sensor_msgs.msg
 from visualization_msgs.msg import Marker, MarkerArray
 
@@ -12,7 +13,7 @@ from robot_utils.ros.rviz import *
 from robot_utils.spatial import Transform
 from vgn.detection import VGN, compute_grasps
 from vgn.perception import UniformTSDFVolume
-import vgn.vis
+from vgn.utils import *
 
 
 def get_policy(name):
@@ -25,8 +26,8 @@ def get_policy(name):
 class BasePolicy:
     def __init__(self):
         self.cv_bridge = cv_bridge.CvBridge()
-        self.tsdf = UniformTSDFVolume(0.3, 40)
         self.vgn = VGN(Path(rospy.get_param("vgn/model")))
+        self.finger_depth = 0.05
 
         self.load_parameters()
         self.lookup_transforms()
@@ -64,9 +65,16 @@ class BasePolicy:
         )
 
     def connect_to_rviz(self):
-        self.path_pub = rospy.Publisher("path", MarkerArray, queue_size=1, latch=True)
+        self.bbox_pub = Publisher("bbox", Marker, queue_size=1, latch=True)
+        self.cloud_pub = Publisher("cloud", PointCloud2, queue_size=1, latch=True)
+        self.path_pub = Publisher("path", MarkerArray, queue_size=1, latch=True)
+        self.grasps_pub = Publisher("grasps", MarkerArray, queue_size=1, latch=True)
 
-    def activate(self):
+    def activate(self, bbox):
+        self.clear_grasps()
+        self.bbox = bbox
+        self.draw_bbox()
+        self.tsdf = UniformTSDFVolume(0.3, 40)
         self.viewpoints = []
         self.done = False
         self.best_grasp = None  # grasp pose defined w.r.t. the robot's base frame
@@ -86,13 +94,38 @@ class BasePolicy:
         tsdf_grid = self.tsdf.get_grid()
         out = self.vgn.predict(tsdf_grid)
         score_fn = lambda g: g.pose.translation[2]
-        grasps = compute_grasps(self.tsdf.voxel_size, out, score_fn)
+        grasps = compute_grasps(self.tsdf.voxel_size, out, score_fn, max_filter_size=3)
-        vgn.vis.draw_grasps(grasps, 0.05)
+        grasps = self.filter_grasps_on_target_object(grasps)
+        self.draw_grasps(grasps)
         return self.T_B_task * grasps[0].pose if len(grasps) > 0 else None
 
+    def filter_grasps_on_target_object(self, grasps):
+        return [
+            g
+            for g in grasps
+            if self.bbox.is_inside(
+                g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation
+            )
+        ]
+
+    def clear_grasps(self):
+        self.grasps_pub.publish(DELETE_MARKER_ARRAY_MSG)
+
+    def draw_bbox(self):
+        pose = Transform.translation((self.bbox.min + self.bbox.max) / 2.0)
+        scale = self.bbox.max - self.bbox.min
+        color = np.r_[0.8, 0.2, 0.2, 0.6]
+        msg = create_marker(Marker.CUBE, self.task_frame, pose, scale, color)
+        self.bbox_pub.publish(msg)
+
     def draw_scene_cloud(self):
         cloud = self.tsdf.get_scene_cloud()
-        vgn.vis.draw_points(np.asarray(cloud.points))
+        msg = to_cloud_msg(self.task_frame, np.asarray(cloud.points))
+        self.cloud_pub.publish(msg)
+
+    def draw_grasps(self, grasps):
+        msg = create_grasp_marker_array(self.task_frame, grasps, self.finger_depth)
+        self.grasps_pub.publish(msg)
 
     def draw_camera_path(self):
         identity = Transform.identity()
@@ -129,6 +162,5 @@ class SingleViewBaseline(BasePolicy):
     def update(self):
         self.integrate_latest_image()
         self.draw_scene_cloud()
-        self.draw_camera_path()
         self.best_grasp = self.predict_best_grasp()
         self.done = True

sim_grasp.py

@@ -1,21 +1,23 @@
 import argparse
 import rospy
-import std_srvs.srv as std_srvs
 
 
+from active_grasp.srv import Reset, ResetRequest
 from controller import GraspController
 from policies import get_policy
+from utils import *
 
 
 class SimGraspController(GraspController):
     def __init__(self, policy):
         super().__init__(policy)
-        self.reset_sim = rospy.ServiceProxy("/reset", std_srvs.Trigger)
+        self.reset_sim = rospy.ServiceProxy("reset", Reset)
 
     def reset(self):
-        req = std_srvs.TriggerRequest()
+        req = ResetRequest()
-        self.reset_sim(req)
+        res = self.reset_sim(req)
         rospy.sleep(1.0)  # wait for states to be updated
+        return from_bbox_msg(res.bbox)
 
 
 def create_parser():

simulation.py

@@ -5,13 +5,13 @@ import rospkg
 from robot_utils.bullet import *
 from robot_utils.controllers import CartesianPoseController
 from robot_utils.spatial import Rotation, Transform
-from utils import AABBox
+from utils import *
 
 
 class Simulation(BtSim):
     def __init__(self, gui=True):
         super().__init__(gui=gui, sleep=False)
-        p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+        # p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
         p.resetDebugVisualizerCamera(1.4, 50, -35, [0.0, 0.0, 0.6])
 
         self.object_uids = []
@@ -111,6 +111,8 @@ class Simulation(BtSim):
 
     def get_target_bbox(self, uid):
         aabb_min, aabb_max = p.getAABB(uid)
+        aabb_min = np.array(aabb_min) - self.origin
+        aabb_max = np.array(aabb_max) - self.origin
         return AABBox(aabb_min, aabb_max)
 
 

utils.py

@@ -19,17 +19,18 @@ class AABBox:
         self.min = bbox_min
         self.max = bbox_max
 
-    @classmethod
-    def from_msg(cls, msg):
-        aabb_min = from_point_msg(msg.min)
-        aabb_max = from_point_msg(msg.max)
-        return cls(aabb_min, aabb_max)
-
-    def to_msg(self):
-        msg = active_grasp.msg.AABBox()
-        msg.min = to_point_msg(self.min)
-        msg.max = to_point_msg(self.max)
-        return msg
-
     def is_inside(self, p):
         return np.all(p > self.min) and np.all(p < self.max)
+
+
+def from_bbox_msg(msg):
+    aabb_min = from_point_msg(msg.min)
+    aabb_max = from_point_msg(msg.max)
+    return AABBox(aabb_min, aabb_max)
+
+
+def to_bbox_msg(bbox):
+    msg = active_grasp.msg.AABBox()
+    msg.min = to_point_msg(bbox.min)
+    msg.max = to_point_msg(bbox.max)
+    return msg