def pose_constraint_to_position_orientation_constraints(pose_constraint):
    position_constraint = PositionConstraint()
    orientation_constraint = OrientationConstraint()
    position_constraint.header = pose_constraint.header
    position_constraint.link_name = pose_constraint.link_name
    position_constraint.position = pose_constraint.pose.position

    position_constraint.constraint_region_shape.type = geometric_shapes_msgs.msg.Shape.BOX
    position_constraint.constraint_region_shape.dimensions.append(
        2 * pose_constraint.absolute_position_tolerance.x)
    position_constraint.constraint_region_shape.dimensions.append(
        2 * pose_constraint.absolute_position_tolerance.y)
    position_constraint.constraint_region_shape.dimensions.append(
        2 * pose_constraint.absolute_position_tolerance.z)

    position_constraint.constraint_region_orientation.x = 0.0
    position_constraint.constraint_region_orientation.y = 0.0
    position_constraint.constraint_region_orientation.z = 0.0
    position_constraint.constraint_region_orientation.w = 1.0

    position_constraint.weight = 1.0

    orientation_constraint.header = pose_constraint.header
    orientation_constraint.link_name = pose_constraint.link_name
    orientation_constraint.orientation = pose_constraint.pose.orientation
    orientation_constraint.type = pose_constraint.orientation_constraint_type

    orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
    orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
    orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
    orientation_constraint.weight = 1.0

    return position_constraint, orientation_constraint
def pose_constraint_to_position_orientation_constraints(pose_constraint):
    position_constraint = PositionConstraint()
    orientation_constraint = OrientationConstraint()
    position_constraint.header = pose_constraint.header
    position_constraint.link_name = pose_constraint.link_name
    position_constraint.position = pose_constraint.pose.position

    position_constraint.constraint_region_shape.type = geometric_shapes_msgs.msg.Shape.BOX
    position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
    position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
    position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)

    position_constraint.constraint_region_orientation.x = 0.0
    position_constraint.constraint_region_orientation.y = 0.0
    position_constraint.constraint_region_orientation.z = 0.0
    position_constraint.constraint_region_orientation.w = 1.0

    position_constraint.weight = 1.0

    orientation_constraint.header = pose_constraint.header
    orientation_constraint.link_name = pose_constraint.link_name
    orientation_constraint.orientation = pose_constraint.pose.orientation
    orientation_constraint.type = pose_constraint.orientation_constraint_type

    orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
    orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
    orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
    orientation_constraint.weight = 1.0

    return position_constraint, orientation_constraint
Beispiel #3
0
    def move_right_arm(self, position, orientation, frame_id,  waiting_time):
        goal = MoveArmGoal()
        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 10
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(waiting_time / 10.);
        
        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = frame_id
        position_constraint.link_name = "r_wrist_roll_link"
        
        position_constraint.position.x = position[0]
        position_constraint.position.y = position[1]
        position_constraint.position.z = position[2]
        position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
        tolerance = 2 * 0.02
        position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance]
        
        position_constraint.constraint_region_orientation.x = 0.
        position_constraint.constraint_region_orientation.y = 0.
        position_constraint.constraint_region_orientation.z = 0.
        position_constraint.constraint_region_orientation.w = 1.        
        position_constraint.weight = 1.0
        
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header.frame_id = frame_id
        orientation_constraint.link_name = "r_wrist_roll_link"
#        orientation_constraint.type = dunno!
        orientation_constraint.orientation.x = orientation[0]
        orientation_constraint.orientation.y = orientation[1]
        orientation_constraint.orientation.z = orientation[2]
        orientation_constraint.orientation.w = orientation[3]
        
        orientation_constraint.absolute_roll_tolerance = 0.04
        orientation_constraint.absolute_pitch_tolerance = 0.04
        orientation_constraint.absolute_yaw_tolerance = 0.04
        orientation_constraint.weight = 1.0
        
        goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
        goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
        
        goal.disable_collision_monitoring = True
        
#        rospy.loginfo("Goal: " + str(goal))
        state = self.move_right_arm_client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
        if state == actionlib.GoalStatus.SUCCEEDED:
            return True
        else:
            return False
Beispiel #4
0
    pc = PositionConstraint()
    pc.header.stamp = rospy.Time.now()
    pc.header.frame_id = 'torso_lift_link'
    pc.link_name = 'r_wrist_roll_link'
    pc.position.x = 0.75
    pc.position.y = -0.188
    pc.position.z = 0

    pc.constraint_region_shape.type = Shape.BOX
    pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
    pc.constraint_region_orientation.w = 1.0

    goalA.motion_plan_request.goal_constraints.position_constraints.append(pc)

    oc = OrientationConstraint()
    oc.header.stamp = rospy.Time.now()
    oc.header.frame_id = 'torso_lift_link'
    oc.link_name = 'r_wrist_roll_link'
    oc.orientation.x = 0.
    oc.orientation.y = 0.
    oc.orientation.z = 0.
    oc.orientation.w = 1.

    oc.absolute_roll_tolerance = 0.04
    oc.absolute_pitch_tolerance = 0.04
    oc.absolute_yaw_tolerance = 0.04
    oc.weight = 1.

    goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc)
Beispiel #5
0
    def move_arm_constrained(self,
                             start_angles=None,
                             location=None,
                             blocking=1):
        current_pose = self.get_current_wrist_pose_stamped('base_link')
        move_arm_goal = self.create_move_arm_goal()
        # default search-starting arm angles are arm-to-the-side
        if start_angles == None:
            if self.whicharm == 'l':
                start_angles = [
                    2.135, 0.803, 1.732, -1.905, 2.369, -1.680, 1.398
                ]
            else:
                start_angles = [
                    -2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398
                ]
        # default location is arm-to-the-side
        if location == None:
            if self.whicharm == 'l': location = [0.05, 0.576, 0.794]
            else: location = [0.05, -0.576, 0.794]
        # add a position constraint for the goal
        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = current_pose.header.frame_id
        position_constraint.link_name = self.ik_utilities.link_name  #r_ or l_wrist_roll_link
        set_xyz(position_constraint.position, location)
        position_constraint.constraint_region_shape.type = Shape.BOX
        position_constraint.constraint_region_shape.dimensions = [0.02] * 3
        position_constraint.constraint_region_orientation.w = 1.0
        position_constraint.weight = 1.0
        move_arm_goal.motion_plan_request.goal_constraints.position_constraints.append(
            position_constraint)
        # search for a feasible goal pose that gets our wrist to location while keeping all but the (base-link) yaw fixed
        current_pose_mat = pose_to_mat(current_pose.pose)
        current_pose_mat = pose_to_mat(current_pose.pose)
        found_ik_solution = 0
        for angle in range(0, 180, 5):
            for dir in [1, -1]:
                rot_mat = tf.transformations.rotation_matrix(
                    dir * angle / 180. * math.pi, [0, 0, 1])
                rotated_pose_mat = rot_mat * current_pose_mat
                desired_pose = stamp_pose(mat_to_pose(rotated_pose_mat),
                                          'base_link')
                desired_pose.pose.position = position_constraint.position
                # check if there's a collision-free IK solution at the goal location with that orientation
                (solution, error_code) = self.ik_utilities.run_ik(
                    desired_pose,
                    start_angles,
                    self.ik_utilities.link_name,
                    collision_aware=1)
                if error_code == "SUCCESS":
                    found_ik_solution = 1
                    break
            if found_ik_solution: break
            else:
                rospy.loginfo("no IK solution found for goal, aborting")
                return 0
        # add an orientation constraint for the goal
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header.stamp = rospy.Time.now()
        orientation_constraint.header.frame_id = current_pose.header.frame_id
        orientation_constraint.link_name = self.ik_utilities.link_name
        orientation_constraint.orientation = desired_pose.pose.orientation
        orientation_constraint.type = OrientationConstraint.HEADER_FRAME
        orientation_constraint.absolute_roll_tolerance = 0.2
        orientation_constraint.absolute_pitch_tolerance = 0.5
        orientation_constraint.absolute_yaw_tolerance = 2 * math.pi
        orientation_constraint.weight = 1.0
        move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.append(
            orientation_constraint)

        # add an orientation constraint for the entire path to keep the object upright
        path_orientation_constraint = copy.deepcopy(orientation_constraint)
        # temporary, while deepcopy of rospy.Time is broken
        path_orientation_constraint.header.stamp = rospy.Time(
            orientation_constraint.header.stamp.secs)
        path_orientation_constraint.orientation = current_pose.pose.orientation
        move_arm_goal.motion_plan_request.path_constraints.orientation_constraints.append(
            path_orientation_constraint)
        # send the goal off to move arm, blocking if desired and modifying the start state if it's in collision
        result = self.send_move_arm_goal(move_arm_goal, blocking)
        return result
Beispiel #6
0
    pc = PositionConstraint()
    pc.header.stamp = rospy.Time.now()
    pc.header.frame_id = 'torso_lift_link'
    pc.link_name = 'r_wrist_roll_link'
    pc.position.x = 0.75
    pc.position.y = -0.188
    pc.position.z = 0

    pc.constraint_region_shape.type = Shape.BOX
    pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
    pc.constraint_region_orientation.w = 1.0

    goalA.motion_plan_request.goal_constraints.position_constraints.append(pc)

    oc = OrientationConstraint()
    oc.header.stamp = rospy.Time.now()
    oc.header.frame_id = 'torso_lift_link'
    oc.link_name = 'r_wrist_roll_link'
    oc.orientation.x = 0.
    oc.orientation.y = 0.
    oc.orientation.z = 0.
    oc.orientation.w = 1.

    oc.absolute_roll_tolerance = 0.04
    oc.absolute_pitch_tolerance = 0.04
    oc.absolute_yaw_tolerance = 0.04
    oc.weight = 1.

    goalA.motion_plan_request.goal_constraints.orientation_constraints.append(
        oc)
    def testMotionExecutionBuffer(self):

        global padd_name
        global extra_buffer

        #too much trouble to read for now
        allow_padd = .05  #rospy.get_param(padd_name)

        motion_plan_request = MotionPlanRequest()

        motion_plan_request.group_name = "right_arm"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.position_constraints = [
            PositionConstraint() for _ in range(1)
        ]
        motion_plan_request.goal_constraints.position_constraints[
            0].header.stamp = rospy.Time.now()
        motion_plan_request.goal_constraints.position_constraints[
            0].header.frame_id = "base_link"

        motion_plan_request.goal_constraints.position_constraints[
            0].link_name = "right_arm_hand_link"
        motion_plan_request.goal_constraints.position_constraints[
            0].position.x = .65
        motion_plan_request.goal_constraints.position_constraints[
            0].position.y = -0.55
        motion_plan_request.goal_constraints.position_constraints[
            0].position.z = .9

        motion_plan_request.goal_constraints.position_constraints[
            0].constraint_region_shape.type = Shape.BOX
        motion_plan_request.goal_constraints.position_constraints[
            0].constraint_region_shape.dimensions = [
                float(.02) for _ in range(3)
            ]

        motion_plan_request.goal_constraints.position_constraints[
            0].constraint_region_orientation.w = 1.0
        motion_plan_request.goal_constraints.position_constraints[
            0].weight = 1.0

        motion_plan_request.goal_constraints.orientation_constraints = [
            OrientationConstraint() for _ in range(1)
        ]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].header.stamp = rospy.Time.now()
        motion_plan_request.goal_constraints.orientation_constraints[
            0].header.frame_id = "base_link"
        motion_plan_request.goal_constraints.orientation_constraints[
            0].link_name = "right_arm_hand_link"

        vals = [float() for _ in range(4)]
        vals = [-.895249, -.187638, 0.092325, .393443]
        mag = math.sqrt(vals[0] * vals[0] + vals[1] * vals[1] +
                        vals[2] * vals[2] + vals[3] * vals[3])
        vals[0] /= mag
        vals[1] /= mag
        vals[2] /= mag
        vals[3] /= mag

        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.x = vals[0]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.y = vals[1]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.z = vals[2]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.w = vals[3]

        motion_plan_request.goal_constraints.orientation_constraints[
            0].absolute_roll_tolerance = 0.04
        motion_plan_request.goal_constraints.orientation_constraints[
            0].absolute_pitch_tolerance = 0.04
        motion_plan_request.goal_constraints.orientation_constraints[
            0].absolute_yaw_tolerance = 0.04

        motion_plan_request.goal_constraints.orientation_constraints[
            0].weight = 1.0

        goal = MoveArmGoal()
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request = motion_plan_request

        self.move_arm_action_client.send_goal(goal)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break