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, moveit):
+    def __init__(self, bbox, min_z_dist):
         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"
-        self.arm = BtPandaArm(panda_urdf)
+        panda_urdf_path = path / "assets/urdfs/franka/panda_arm_hand.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):