diff --git a/stretch_demos/nodes/grasp_point b/stretch_demos/nodes/grasp_point index a2e7955..41fd913 100755 --- a/stretch_demos/nodes/grasp_point +++ b/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 -