Esempio n. 1
0
    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
Esempio n. 2
0
    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
Esempio n. 3
0
def create_motion_plan_request_for_joints(joint_positions):
    """Setup motion plan request.
    The length of parameter joint_positions must match the length of global variable JOINT_NAMES.
    """
    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 = ""

    motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(JOINT_NAMES))]
    for i in range(len(JOINT_NAMES)):
        motion_plan_request.goal_constraints.joint_constraints[i].joint_name = JOINT_NAMES[i]
        motion_plan_request.goal_constraints.joint_constraints[i].position = joint_positions[i]
        motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.01
        motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.01

    return motion_plan_request
Esempio n. 4
0
def create_motion_plan_request_for_joints(joint_positions):
    """Setup motion plan request.
    The length of parameter joint_positions must match the length of global variable JOINT_NAMES.
    """
    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 = ""

    motion_plan_request.goal_constraints.joint_constraints = [
        JointConstraint() for i in range(len(JOINT_NAMES))
    ]
    for i in range(len(JOINT_NAMES)):
        motion_plan_request.goal_constraints.joint_constraints[
            i].joint_name = JOINT_NAMES[i]
        motion_plan_request.goal_constraints.joint_constraints[
            i].position = joint_positions[i]
        motion_plan_request.goal_constraints.joint_constraints[
            i].tolerance_above = 0.01
        motion_plan_request.goal_constraints.joint_constraints[
            i].tolerance_below = 0.01

    return motion_plan_request