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
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
goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.allowed_planning_time = rospy.Duration(0.5) desired_pose = SimplePoseConstraint() desired_pose.header.frame_id = "torso_lift_link"; desired_pose.link_name = "r_wrist_roll_link"; # desired_pose.pose.position.x = 0.75; desired_pose.pose.position.x = 0.745 desired_pose.pose.position.y = -0.188; desired_pose.pose.position.z = 0; # desired_pose.pose.position.z = -0.3; desired_pose.pose.orientation.x = 1.0 desired_pose.pose.orientation.y = 0.0 desired_pose.pose.orientation.z = 0.0 desired_pose.pose.orientation.w = 1.0 desired_pose.absolute_position_tolerance.x = 0.001 desired_pose.absolute_position_tolerance.y = 0.001 desired_pose.absolute_position_tolerance.z = 0.001 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, goal) client.send_goal(goal)