revert grasp_point

stretch_demos/nodes/grasp_point

@@ -33,9 +33,8 @@ class GraspObjectNode(hm.HelloNode):
         self.roll_position = None
 
         # TODO: investigate a better place for these to exist
-        self.yaw_offset = 0.0
-        self.mb_offset = 0.01
-        self.arm_offset = -0.03
+        self.yaw_offset = 0.09
+        self.mb_offset = 0.02
 
     def drive_base(self, delta_m):
         tolerance_distance_m = 0.005
@@ -47,23 +46,25 @@ class GraspObjectNode(hm.HelloNode):
 
     def grasp_object(self, target):
         target['elongated'] = False
+        pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer)
+        lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
+        lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1)
+        self.move_to_pose({'joint_lift': lift_to_pregrasp_m})
 
         if self.tool == "tool_stretch_dex_wrist":
             self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0})
-        self.move_to_pose({'joint_wrist_yaw': 0.0})
-        self.move_to_pose({'gripper_aperture': 0.125})
 
-        pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer, tooltip_frame='link_grasp_center_real')
-        lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
-        lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1)
-        self.move_to_pose({'joint_lift': lift_to_pregrasp_m})
+        pregrasp_yaw = self.manip.get_pregrasp_yaw(target, self.tf2_buffer)
+        self.move_to_pose({'joint_wrist_yaw': pregrasp_yaw + self.yaw_offset})
+
+        self.move_to_pose({'gripper_aperture': 0.125})
 
         pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer)
         self.drive_base(pregrasp_base_m + self.mb_offset)
 
         arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01)
         arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5)
-        self.move_to_pose({'wrist_extension': arm_to_pregrasp_m + self.arm_offset})
+        self.move_to_pose({'wrist_extension': arm_to_pregrasp_m})
 
         rospy.sleep(1)
 
@@ -75,7 +76,7 @@ class GraspObjectNode(hm.HelloNode):
         if self.tool == "tool_stretch_dex_wrist":
             lift_to_grasp_m -= 0.03
             arm_to_grasp_m += 0.02
-        self.move_to_pose({'translate_mobile_base': grasp_base_m + self.mb_offset, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m + self.arm_offset})
+        self.move_to_pose({'translate_mobile_base': grasp_base_m + self.mb_offset, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m})
 
         gripper_to_grasp_m = target['width_m'] - 0.18
         self.move_to_pose({'gripper_aperture': gripper_to_grasp_m})
@@ -265,4 +266,3 @@ if __name__ == "__main__":
         node.main()
     except KeyboardInterrupt:
         pass
-