Use KDL for checking IKs

active_grasp/baselines.py

@@ -13,14 +13,14 @@ class TopView(SingleViewPolicy):
     def activate(self, bbox, view_sphere):
         super().activate(bbox, view_sphere)
         self.x_d = self.view_sphere.get_view(0.0, 0.0)
-        self.done = False if self.view_sphere.is_feasible(self.x_d) else True
+        self.done = False if self.is_feasible(self.x_d) else True
 
 
 class TopTrajectory(MultiViewPolicy):
     def activate(self, bbox, view_sphere):
         super().activate(bbox, view_sphere)
         self.x_d = self.view_sphere.get_view(0.0, 0.0)
-        self.done = False if self.view_sphere.is_feasible(self.x_d) else True
+        self.done = False if self.is_feasible(self.x_d) else True
 
     def update(self, img, x):
         self.integrate(img, x)

active_grasp/controller.py

@@ -90,7 +90,7 @@ class GraspController:
         return from_bbox_msg(res.bbox)
 
     def search_grasp(self, bbox):
-        self.view_sphere = ViewHalfSphere(bbox, self.min_z_dist, self.moveit)
+        self.view_sphere = ViewHalfSphere(bbox, self.min_z_dist)
         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)
@@ -160,12 +160,9 @@ class GraspController:
 
 
 class ViewHalfSphere:
-    # TODO move feasibility check to a robot interface module
+    def __init__(self, bbox, min_z_dist):
-    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)
@@ -174,7 +171,3 @@ class ViewHalfSphere:
 
     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

@@ -20,7 +20,7 @@ class NextBestView(MultiViewPolicy):
         self.view_candidates = []
         for theta, phi in itertools.product(thetas, phis):
             view = self.view_sphere.get_view(theta, phi)
-            if self.view_sphere.is_feasible(view):
+            if self.is_feasible(view):
                 self.view_candidates.append(view)
 
     def update(self, img, x):

active_grasp/policy.py

@@ -4,6 +4,7 @@ from pathlib import Path
 import rospy
 
 from .visualization import Visualizer
+from robot_helpers.model import KDLModel
 from robot_helpers.ros import tf
 from robot_helpers.ros.conversions import *
 from vgn.detection import *
@@ -11,21 +12,30 @@ from vgn.perception import UniformTSDFVolume
 
 
 class Policy:
-    def __init__(self, rate=5):
+    def __init__(self):
         self.load_parameters()
+        self.init_robot_model()
         self.init_visualizer()
 
     def load_parameters(self):
         self.base_frame = rospy.get_param("~base_frame_id")
+        self.cam_frame = rospy.get_param("~camera/frame_id")
         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_robot_model(self):
+        self.model = KDLModel.from_parameter_server(self.base_frame, self.cam_frame)
+
     def init_visualizer(self):
         self.vis = Visualizer()
 
+    def is_feasible(self, view, q_init=None):
+        q_init = q_init if q_init else [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]
+        return self.model.ik(q_init, view) is not None
+
     def activate(self, bbox, view_sphere):
         self.vis.clear()
 

active_grasp/simulation.py

@@ -6,7 +6,7 @@ import rospkg
 
 from active_grasp.bbox import AABBox
 from robot_helpers.bullet import *
-from robot_helpers.model import Model
+from robot_helpers.model import KDLModel
 from robot_helpers.spatial import Rotation
 
 
@@ -36,10 +36,12 @@ class Simulation:
 
     def load_robot(self):
         path = Path(rospack.get_path("active_grasp"))
-        panda_urdf = path / "assets/urdfs/franka/panda_arm_hand.urdf"
+        panda_urdf_path = path / "assets/urdfs/franka/panda_arm_hand.urdf"
-        self.arm = BtPandaArm(panda_urdf)
+        self.arm = BtPandaArm(panda_urdf_path)
         self.gripper = BtPandaGripper(self.arm)
-        self.model = Model(panda_urdf, self.arm.base_frame, self.arm.ee_frame)
+        self.model = KDLModel.from_urdf_file(
+            panda_urdf_path, self.arm.base_frame, self.arm.ee_frame
+        )
         self.camera = BtCamera(320, 240, 0.96, 0.01, 1.0, self.arm.uid, 11)
 
     def seed(self, seed):

launch/active_grasp.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 338
+    Tree Height: 755
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -25,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: DepthImage
+    SyncSource: SceneCloud
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -51,6 +51,18 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /camera/color/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: ColorImage
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
     - Class: rviz/Image
       Enabled: false
       Image Topic: /camera/depth/image_rect_raw
@@ -98,7 +110,12 @@ Visualization Manager:
       Marker Topic: visualization_marker_array
       Name: Markers
       Namespaces:
-        {}
+        bbox: true
+        best_grasp: true
+        grasps: true
+        path: true
+        views: true
+        workspace: true
       Queue Size: 100
       Value: true
     - Acceleration_Scaling_Factor: 0.1
@@ -134,6 +151,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          camera_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           panda_hand:
             Alpha: 1
             Show Axes: false
@@ -239,6 +260,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          camera_depth_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           panda_hand:
             Alpha: 1
             Show Axes: false
@@ -397,6 +422,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        camera_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         panda_hand:
           Alpha: 1
           Show Axes: false
@@ -498,12 +527,10 @@ Visualization Manager:
         All Enabled: false
         camera_depth_optical_frame:
           Value: true
-        panda_EE:
+        camera_optical_frame:
-          Value: true
-        panda_K:
           Value: true
         panda_hand:
-          Value: false
+          Value: true
         panda_leftfinger:
           Value: false
         panda_link0:
@@ -526,6 +553,8 @@ Visualization Manager:
           Value: false
         panda_rightfinger:
           Value: false
+        task:
+          Value: true
         world:
           Value: false
       Marker Alpha: 1
@@ -545,30 +574,19 @@ Visualization Manager:
                       panda_link6:
                         panda_link7:
                           panda_link8:
+                            panda_hand:
                               camera_depth_optical_frame:
                                 {}
-                            panda_EE:
+                              camera_optical_frame:
-                              panda_K:
                                 {}
-                            panda_hand:
                               panda_leftfinger:
                                 {}
                               panda_rightfinger:
                                 {}
+            task:
+              {}
       Update Interval: 0
       Value: true
-    - Class: rviz/Image
-      Enabled: true
-      Image Topic: /camera/color/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: ColorImage
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
-      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -597,7 +615,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.8815758228302002
+      Distance: 1.4805835485458374
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -605,17 +623,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.4086047410964966
+        X: 0.2569250166416168
-        Y: 0.23318199813365936
+        Y: 0.09482234716415405
-        Z: 0.28523021936416626
+        Z: 0.4617196023464203
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3297969102859497
+      Pitch: 0.31479594111442566
       Target Frame: <Fixed Frame>
-      Yaw: 5.052877902984619
+      Yaw: 5.353035926818848
     Saved: ~
 Window Geometry:
   ColorImage:
@@ -624,14 +642,14 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 960
+  Height: 969
   Hide Left Dock: false
-  Hide Right Dock: true
+  Hide Right Dock: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -640,6 +658,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
+  Width: 1436
-  X: 0
+  X: 911
-  Y: 27
+  Y: 73