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