def arm_post_grasp_goal_cb(userdata, old_goal): userdata.target_pose_stamped.pose.orientation = Quaternion(0, 0, 0 ,1) goal_for_pregrasping = copy.deepcopy(userdata.target_pose_stamped) goal_for_pregrasping.pose.position.x -= 0.22 arm_goal = get_arm_goal(goal_for_pregrasping.pose, frame_id="/base_link") rospy.loginfo("Sending arm goal:\n" + str(arm_goal)) return arm_goal
def get_pose_for_arm_down(userdata, old_goal): # FIXME: Check if it is correct relax_pose = PoseStamped() relax_pose.pose.position = Point(0.130007, -0.198804, 0.900355) relax_pose.pose.orientation = Quaternion(0.87292, -0.244933, 0.127606, -0.402163) arm_goal = get_arm_goal(relax_pose.pose, frame_id="/base_link") return arm_goal
def arm_post_grasp_goal_cb(userdata, old_goal): userdata.target_pose_stamped.pose.orientation = Quaternion(0.5, -0.5, 0.5, -0.5) rospy.loginfo("Modifying post grasp pose.") goal_for_pregrasping = copy.deepcopy(userdata.target_pose_stamped.pose) goal_for_pregrasping.position.x -= 0.16 arm_goal = get_arm_goal(goal_for_pregrasping, frame_id="/base_link") rospy.loginfo("Sending arm goal:\n" + str(arm_goal)) return arm_goal
def arm_grasp_goal_cb(userdata, old_goal): userdata.target_pose_stamped.pose.orientation = Quaternion(0, 0, 0 ,1) rospy.loginfo("Modifying grasp pose.") goal_for_pregrasping = copy.deepcopy(userdata.target_pose_stamped) goal_for_pregrasping.pose.position.x -= 0.14 arm_goal = get_arm_goal(goal_for_pregrasping.pose, frame_id="/base_link") # Do a interpolated movement! #arm_goal.motion_plan_request.planner_id = "interpolated_ik" #arm_goal.planner_service_name = "generic_interpolated_ik_right_arm_torso/GetInterpolatedIkMotionPlan" rospy.loginfo("Sending arm goal:\n" + str(arm_goal)) return arm_goal