def createHandPose(pos, orientation, hand = 'r'):
""" Create the final pose contraint for the hand
"""
desired_pose = SimplePoseConstraint();
desired_pose.header.frame_id = "torso_lift_link"
desired_pose.link_name = hand + "_wrist_roll_link"
desired_pose.pose.position.x = pos[0]
desired_pose.pose.position.y = pos[1]
desired_pose.pose.position.z = pos[2]
desired_pose.pose.orientation.x = orientation[0]
desired_pose.pose.orientation.y = orientation[1]
desired_pose.pose.orientation.z = orientation[2]
desired_pose.pose.orientation.w = orientation[3]
desired_pose.absolute_position_tolerance.x = 0.02
desired_pose.absolute_position_tolerance.y = 0.02
desired_pose.absolute_position_tolerance.z = 0.02
desired_pose.absolute_roll_tolerance = 0.04
desired_pose.absolute_pitch_tolerance = 0.04
desired_pose.absolute_yaw_tolerance = 0.04
return desired_pose
def execute(self, userdata):
self._goal.planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "SchunkArm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
motion_plan_request.planner_id = ""
self._goal.motion_plan_request = motion_plan_request
desired_pose = SimplePoseConstraint()
desired_pose.header.frame_id = userdata.parent_frame
desired_pose.link_name = "Gripper"
desired_pose.pose.position.x = userdata.x
desired_pose.pose.position.y = userdata.y
desired_pose.pose.position.z = userdata.z
quat = tf.transformations.quaternion_from_euler(
-math.pi / 2, math.pi / 2, userdata.phi)
desired_pose.pose.orientation = Quaternion(*quat)
desired_pose.absolute_position_tolerance.x = 0.02
desired_pose.absolute_position_tolerance.y = 0.02
desired_pose.absolute_position_tolerance.z = 0.02
desired_pose.absolute_roll_tolerance = 0.04
desired_pose.absolute_pitch_tolerance = 0.04
desired_pose.absolute_yaw_tolerance = 0.04
add_goal_constraint_to_move_arm_goal(desired_pose, self._goal)
srv_out = SimpleActionState.execute(self, userdata)
return srv_out
