add offset for gsnet-generated grasp poses

cfg/active_grasp.yaml

@@ -1,9 +1,9 @@
 bt_sim:
   gui: True
   gripper_force: 100
-  scene: random
+  # scene: random
   # scene: manual
-  # scene: $(find active_grasp)/cfg/sim/challenging_scene_4.yaml
+  scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml
 
 hw:
   roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
@@ -47,4 +47,5 @@ ap_grasp:
   crop_radius_step: 0.05
   crop_max_radius: 0.5
   num_knn_neighbours: 5
+  gsnet_grasp_offset: [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.01], [0, 0, 0, 1]] 
 

src/active_grasp/active_perception_policy.py

@@ -76,6 +76,7 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
         self.pcdvis = RealTime3DVisualizer()
         self.updated = False
         self._base_url = flask_base_url
+        self.grasp_net_type = 'gsnet'
 
         # For debugging
         self.pcd_publisher = rospy.Publisher('/debug_pcd', PointCloud2, queue_size=10)

src/active_grasp/controller.py

@@ -38,6 +38,8 @@ class GraspController:
         self.linear_vel = rospy.get_param("~linear_vel")
         self.move_to_target_threshold = rospy.get_param("~move_to_target_threshold")
         self.policy_rate = rospy.get_param("policy/rate")
+        self.gsnet_grasp_offset = np.asarray(rospy.get_param("ap_grasp/gsnet_grasp_offset"))
+        self.gsnet_grasp_offset = Transform.from_matrix(self.gsnet_grasp_offset)
 
     def init_service_proxies(self):
         self.reset_env = rospy.ServiceProxy("reset", Reset)
@@ -177,6 +179,9 @@ class GraspController:
         return np.r_[linear, angular]
 
     def execute_grasp(self, grasp):
+        if self.policy.grasp_net_type == "gsnet":
+            self.T_grasp_ee = self.T_grasp_ee * self.gsnet_grasp_offset
+            
         self.create_collision_scene()
         T_base_grasp = self.postprocess(grasp.pose)
         self.gripper.move(0.08)
@@ -186,7 +191,8 @@ class GraspController:
             self.moveit.scene.clear()
             self.moveit.execute(plan)
             rospy.sleep(0.5)  # Wait for the planning scene to be updated
-            self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
+            grasp_pose = T_base_grasp * self.T_grasp_ee
+            self.moveit.gotoL(grasp_pose)
             rospy.sleep(0.5)
             self.gripper.grasp()
             T_base_retreat = Transform.t_[0, 0, 0.05] * T_base_grasp * self.T_grasp_ee

src/active_grasp/policy.py

@@ -24,6 +24,7 @@ class Policy:
         self.load_parameters()
         self.init_ik_solver()
         self.init_visualizer()
+        self.grasp_net_type = 'vgn'
 
     def load_parameters(self):
         self.base_frame = rospy.get_param("~base_frame_id")