![]() |
Calling Grasp Adjustment
Description:Tutorial Level: BEGINNER
Contents
This tutorial is still under construction
grasp_adjust_client = None rospy.wait_for_service("/pr2_gripper_grasp_adjust") grasp_adjust_client = rospy.ServiceProxy("/pr2_gripper_grasp_adjust", GripperStereoGraspAdjust) ps = copy.copy(self.target) pose_mat = pose_to_mat(ps.pose) pose_mat[0:3, 3] -= 0.08*pose_mat[0:3, 0] ps.pose = mat_to_pose(pose_mat) (new_goal_list, result) = self.gripper_stereo_grasp_adjust(ps, do_global_search = True) #grasp is impossible! if result == GripperStereoGraspAdjustResponse.IMPOSSIBLE: rospy.loginfo("gripper stereo adjustment returned grasp impossible!") return self.target if result == GripperStereoGraspAdjustResponse.FAILURE: rospy.loginfo("gripper stereo adjustment returned failed!") return self.target for i in range(len(new_goal_list)): #TODO account for tool frame -> wrist frame offset new_goal_list[i] = change_pose_stamped_frame(self.tf, new_goal_list[i], 'torso_lift_link') self.adjusted_poses = new_goal_list[:] print "Received", len(self.adjusted_poses), "adjusted grasps"