Esempio n. 1
0
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 = 0
    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 get_interpolated_ik_motion_plan(self,
                                        start_pose,
                                        goal_pose,
                                        start_angles,
                                        joint_names,
                                        pos_spacing=0.01,
                                        rot_spacing=0.1,
                                        consistent_angle=math.pi / 9,
                                        collision_aware=True,
                                        collision_check_resolution=1,
                                        steps_before_abort=-1,
                                        num_steps=0,
                                        ordered_collision_operations=None,
                                        frame='base_footprint',
                                        start_from_end=0,
                                        max_joint_vels=[1.5] * 7,
                                        max_joint_accs=[8.0] * 7):

        res = self.interpolated_ik_params_srv(num_steps, consistent_angle,
                                              collision_check_resolution,
                                              steps_before_abort, pos_spacing,
                                              rot_spacing, collision_aware,
                                              start_from_end, max_joint_vels,
                                              max_joint_accs)

        req = GetMotionPlanRequest()
        req.motion_plan_request.start_state.joint_state.name = joint_names
        req.motion_plan_request.start_state.joint_state.position = start_angles
        req.motion_plan_request.start_state.multi_dof_joint_state.poses = [
            start_pose.pose
        ]
        req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [
            GRIPPER_LINK_FRAME
        ]
        req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [
            start_pose.header.frame_id
        ]

        pos_constraint = PositionConstraint()
        pos_constraint.position = goal_pose.pose.position
        pos_constraint.header.frame_id = goal_pose.header.frame_id
        req.motion_plan_request.goal_constraints.position_constraints = [
            pos_constraint,
        ]

        orient_constraint = OrientationConstraint()
        orient_constraint.orientation = goal_pose.pose.orientation
        orient_constraint.header.frame_id = goal_pose.header.frame_id
        req.motion_plan_request.goal_constraints.orientation_constraints = [
            orient_constraint,
        ]

        #req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
        #req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS])

        #if ordered_collision_operations is not None:
        #    req.motion_plan_request.ordered_collision_operations = ordered_collision_operations

        res = self.interpolated_ik_srv(req)
        return res
Esempio n. 3
0
def poseConstraintToPositionOrientationConstraints(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 = 0
    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 get_interpolated_ik_motion_plan(
        self,
        start_pose,
        goal_pose,
        start_angles,
        joint_names,
        pos_spacing=0.01,
        rot_spacing=0.1,
        consistent_angle=math.pi / 9,
        collision_aware=True,
        collision_check_resolution=1,
        steps_before_abort=-1,
        num_steps=0,
        ordered_collision_operations=None,
        frame="base_footprint",
        start_from_end=0,
        max_joint_vels=[1.5] * 7,
        max_joint_accs=[8.0] * 7,
    ):

        res = self.interpolated_ik_params_srv(
            num_steps,
            consistent_angle,
            collision_check_resolution,
            steps_before_abort,
            pos_spacing,
            rot_spacing,
            collision_aware,
            start_from_end,
            max_joint_vels,
            max_joint_accs,
        )

        req = GetMotionPlanRequest()
        req.motion_plan_request.start_state.joint_state.name = joint_names
        req.motion_plan_request.start_state.joint_state.position = start_angles
        req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose]
        req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [GRIPPER_LINK_FRAME]
        req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id]

        pos_constraint = PositionConstraint()
        pos_constraint.position = goal_pose.pose.position
        pos_constraint.header.frame_id = goal_pose.header.frame_id
        req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint]

        orient_constraint = OrientationConstraint()
        orient_constraint.orientation = goal_pose.pose.orientation
        orient_constraint.header.frame_id = goal_pose.header.frame_id
        req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint]

        # req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
        # req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS])

        # if ordered_collision_operations is not None:
        #    req.motion_plan_request.ordered_collision_operations = ordered_collision_operations

        res = self.interpolated_ik_srv(req)
        return res
Esempio n. 5
0
def test_move_arm(position, orientation, frame):
  rospy.loginfo("Moving the arm to x: %f y: %f z: %f r: %f p: %f y: %f", position[0], position[1], position[2], orientation[0], orientation[1], orientation[2]);

  client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
  client.wait_for_server()

  goal = MoveArmGoal()
  goal.motion_plan_request.group_name = "right_arm"
  goal.motion_plan_request.num_planning_attempts = 3
  goal.motion_plan_request.planner_id = ""
  goal.planner_service_name = "ompl_planning/plan_kinematic_path"
  goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.)
  
  position_constraint = PositionConstraint()
  position_constraint.header.frame_id = frame
  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 = 0.04
  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
  orientation_constraint.link_name = "r_wrist_roll_link"
  
  orientation_constraint.orientation = quaternion_to_msg(tf.transformations.quaternion_from_euler(orientation[0], orientation[1], orientation[2]))

  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("Calling the move arm client")
  state = client.send_goal_and_wait(goal, rospy.Duration(120.))
  if state == actionlib.GoalStatus.SUCCEEDED:
    rospy.loginfo("Succeeded")
  else:
    rospy.loginfo(state)
Esempio n. 6
0
def processFeedback(feedback):
    #s = "Feedback from marker '" + feedback.marker_name
    #if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
    #    rospy.loginfo( s + ": button click" + mp + "." )



    if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
        if (feedback.menu_entry_id == 1):
            p = feedback.pose
            print p

            goalA = arm_navigation_msgs.msg.MoveArmGoal()
            goalA.motion_plan_request.group_name = "right";
            goalA.motion_plan_request.num_planning_attempts = 1;
            goalA.motion_plan_request.planner_id = "";
            goalA.planner_service_name = "ompl_planning/plan_kinematic_path";
            goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.0);

            desired_pose = PositionConstraint()
            desired_pose.header.frame_id = "/world";
            desired_pose.link_name = "right_arm_7_link";
            desired_pose.position = p.position

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


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

            oc = OrientationConstraint()
            oc.header.stamp = rospy.Time.now()
            oc.header.frame_id = "/world";
            oc.link_name = "right_arm_7_link";
            oc.orientation = p.orientation

            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)

            client.send_goal(goalA)
    req = GetMotionPlanRequest()
    req.motion_plan_request.start_state.joint_state.name = joint_names
    req.motion_plan_request.start_state.joint_state.position = start_angles
    req.motion_plan_request.start_state.multi_dof_joint_state.pose = start_pose.pose
    req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_id = 'L7_wrist_yaw_link'
    req.motion_plan_request.start_state.multi_dof_joint_state.frame_id = start_pose.header.frame_id

    pos_constraint = PositionConstraint()
    pos_constraint.position = goal_pose.pose.position
    pos_constraint.header.frame_id = goal_pose.header.frame_id
    req.motion_plan_request.goal_constraints.position_constraints = [
        pos_constraint,
    ]

    orient_constraint = OrientationConstraint()
    orient_constraint.orientation = goal_pose.pose.orientation
    orient_constraint.header.frame_id = goal_pose.header.frame_id
    req.motion_plan_request.goal_constraints.orientation_constraints = [
        orient_constraint,
    ]

    if ordered_collision_operations != None:
        req.motion_plan_request.ordered_collision_operations = ordered_collision_operations

    try:
        serv = rospy.ServiceProxy("l_interpolated_ik_motion_plan",
                                  GetMotionPlan)
        res = serv(req)
    except rospy.ServiceException as e:
        print "error when calling l_interpolated_ik_motion_plan: %s" % e
        return 0
Esempio n. 8
0
    def move_arm_to(self, goal_in):
        (reachable, ik_goal, duration) = self.full_ik_check(goal_in)
        rospy.sleep(duration)

        rospy.loginfo("Composing move_" + self.arm + "_arm goal")
        goal_out = MoveArmGoal()
        goal_out.motion_plan_request.group_name = self.arm + "_arm"
        goal_out.motion_plan_request.num_planning_attempts = 10
        goal_out.motion_plan_request.planner_id = ""
        goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(
            1.0)

        pos = PositionConstraint()
        pos.header.frame_id = goal_in.header.frame_id
        pos.link_name = self.arm[0] + "_wrist_roll_link"
        pos.position = goal_in.pose.position

        pos.constraint_region_shape.type = 0
        pos.constraint_region_shape.dimensions = [0.01]

        pos.constraint_region_orientation = Quaternion(0, 0, 0, 1)
        pos.weight = 1

        goal_out.motion_plan_request.goal_constraints.position_constraints.append(
            pos)

        ort = OrientationConstraint()
        ort.header.frame_id = goal_in.header.frame_id
        ort.link_name = self.arm[0] + "_wrist_roll_link"
        ort.orientation = goal_in.pose.orientation

        ort.absolute_roll_tolerance = 0.02
        ort.absolute_pitch_tolerance = 0.02
        ort.absolute_yaw_tolerance = 0.02
        ort.weight = 0.5
        goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(
            ort)
        goal_out.accept_partial_plans = True
        goal_out.accept_invalid_goals = True
        goal_out.disable_collision_monitoring = True
        rospy.loginfo("Sending composed move_" + self.arm + "_arm goal")

        finished_within_time = False
        self.move_arm_client.send_goal(goal_out)
        finished_within_time = self.move_arm_client.wait_for_result(
            rospy.Duration(60))
        if not (finished_within_time):
            self.move_arm_client.cancel_goal()
            self.log_out.publish(data="Timed out achieving " + self.arm +
                                 " arm goal pose")
            rospy.loginfo("Timed out achieving right arm goal pose")
            return False
        else:
            state = self.move_arm_client.get_state()
            result = self.move_arm_client.get_result()
            if (state == 3):  #3 == SUCCEEDED
                rospy.loginfo("Action Finished: %s" % state)
                self.log_out.publish(
                    data="Move " + self.arm.capitalize() +
                    " Arm with Motion Planning: %s" %
                    self.move_arm_error_dict[result.error_code.val])
                return True
            else:
                if result.error_code.val == 1:
                    rospy.loginfo("Move_" + self.arm + "_arm_action failed: \
                                    Unable to plan a path to goal")
                    self.log_out.publish(data="Move " + self.arm.capitalize() +
                                         " Arm with Motion Planning Failed: \
                                         Unable to plan a path to the goal")
                elif result.error_code.val == -31:
                    (torso_succeeded,
                     pos) = self.check_torso(self.form_ik_request(goal_in))
                    if torso_succeeded:
                        rospy.loginfo("IK Failed in Current Position. \
                                        Adjusting Height and Retrying")
                        self.log_out.publish(data="IK Failed in Current \
                                                      Position. Adjusting \
                                                      Height and Retrying")
                        self.move_arm_to(pos)
                    else:
                        rospy.loginfo("Move_" + self.arm +
                                      "_arm action failed: %s" % state)
                        rospy.loginfo("Failure Result: %s" % result)
                        self.log_out.publish(
                            data="Move " + self.arm.capitalize() +
                            " Arm with Motion Planning \
                                                    and Torso Check Failed: %s"
                            % self.move_arm_error_dict[result.error_code.val])
                else:
                    rospy.loginfo("Move_" + self.arm +
                                  "_arm action failed: %s" % state)
                    rospy.loginfo("Failure Result: %s" % result)
                    self.log_out.publish(
                        data="Move " + self.arm.capitalize() +
                        " Arm with Motion Planning \
                                                Failed: %s" %
                        self.move_arm_error_dict[result.error_code.val])
            return False
        print "error when calling r_interpolated_ik_motion_plan_set_params: %s"%e  
        return 0        

    req = GetMotionPlanRequest()
    req.motion_plan_request.start_state.joint_state.name = joint_names
    req.motion_plan_request.start_state.joint_state.position = start_angles
    req.motion_plan_request.start_state.multi_dof_joint_state.poses.append(start_pose.pose)
    req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append('r_wrist_roll_link')
    req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(start_pose.header.frame_id)
    pos_constraint = PositionConstraint()
    pos_constraint.position = goal_pose.pose.position
    pos_constraint.header.frame_id = goal_pose.header.frame_id
    req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint,]

    orient_constraint = OrientationConstraint()
    orient_constraint.orientation = goal_pose.pose.orientation
    orient_constraint.header.frame_id = goal_pose.header.frame_id
    req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint,]

    #if ordered_collision_operations != None:
    #    req.motion_plan_request.ordered_collision_operations = ordered_collision_operations

    try:    
        serv = rospy.ServiceProxy("r_interpolated_ik_motion_plan", GetMotionPlan)
        res = serv(req)
    except rospy.ServiceException, e:
        print "error when calling r_interpolated_ik_motion_plan: %s"%e  
        return 0

    error_code_dict = {ArmNavigationErrorCodes.SUCCESS:0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED:1, \
                       ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED:2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED:3, \
Esempio n. 10
0
    def move_arm_to(self, goal_in):
        (reachable, ik_goal, duration) = self.full_ik_check(goal_in)
        rospy.sleep(duration)
        
        rospy.loginfo("Composing move_"+self.arm+"_arm goal")
        goal_out = MoveArmGoal()
        goal_out.motion_plan_request.group_name = self.arm+"_arm"
        goal_out.motion_plan_request.num_planning_attempts = 10 
        goal_out.motion_plan_request.planner_id = ""
        goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(1.0)
        
        pos = PositionConstraint()
        pos.header.frame_id = goal_in.header.frame_id 
        pos.link_name = self.arm[0]+"_wrist_roll_link"
        pos.position = goal_in.pose.position

        pos.constraint_region_shape.type = 0 
        pos.constraint_region_shape.dimensions=[0.01]

        pos.constraint_region_orientation = Quaternion(0,0,0,1)
        pos.weight = 1

        goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
    
        ort = OrientationConstraint()    
        ort.header.frame_id=goal_in.header.frame_id
        ort.link_name = self.arm[0]+"_wrist_roll_link"
        ort.orientation = goal_in.pose.orientation
        
        ort.absolute_roll_tolerance = 0.02
        ort.absolute_pitch_tolerance = 0.02
        ort.absolute_yaw_tolerance = 0.02
        ort.weight = 0.5
        goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
        goal_out.accept_partial_plans = True
        goal_out.accept_invalid_goals = True
        goal_out.disable_collision_monitoring = True
        rospy.loginfo("Sending composed move_"+self.arm+"_arm goal")

        finished_within_time = False
        self.move_arm_client.send_goal(goal_out)
        finished_within_time = self.move_arm_client.wait_for_result(
                                                            rospy.Duration(60))
        if not (finished_within_time):
            self.move_arm_client.cancel_goal()
            self.log_out.publish(data="Timed out achieving "+self.arm+
                                         " arm goal pose")
            rospy.loginfo("Timed out achieving right arm goal pose")
            return False
        else:
            state = self.move_arm_client.get_state()
            result = self.move_arm_client.get_result()
            if (state == 3): #3 == SUCCEEDED
                rospy.loginfo("Action Finished: %s" %state)
                self.log_out.publish(data="Move "+self.arm.capitalize()+
                                          " Arm with Motion Planning: %s"
                                          %self.move_arm_error_dict[
                                                result.error_code.val])
                return True
            else:
                if result.error_code.val == 1:
                    rospy.loginfo("Move_"+self.arm+"_arm_action failed: \
                                    Unable to plan a path to goal")
                    self.log_out.publish(data="Move "+self.arm.capitalize()+
                                        " Arm with Motion Planning Failed: \
                                         Unable to plan a path to the goal")
                elif result.error_code.val == -31:
                    (torso_succeeded, pos) = self.check_torso(
                                                self.form_ik_request(goal_in))
                    if torso_succeeded:
                        rospy.loginfo("IK Failed in Current Position. \
                                        Adjusting Height and Retrying")
                        self.log_out.publish(data="IK Failed in Current \
                                                      Position. Adjusting \
                                                      Height and Retrying")
                        self.move_arm_to(pos)
                    else:
                        rospy.loginfo("Move_"+self.arm+"_arm action failed: %s"
                                        %state)
                        rospy.loginfo("Failure Result: %s" %result)
                        self.log_out.publish(data="Move "+self.arm.capitalize()+
                                                    " Arm with Motion Planning \
                                                    and Torso Check Failed: %s"
                                                    %self.move_arm_error_dict[
                                                        result.error_code.val])
                else:
                    rospy.loginfo("Move_"+self.arm+"_arm action failed: %s" %state)
                    rospy.loginfo("Failure Result: %s" %result)
                    self.log_out.publish(data="Move "+self.arm.capitalize()+
                                                " Arm with Motion Planning \
                                                Failed: %s" 
                                                %self.move_arm_error_dict[
                                                        result.error_code.val])
            return False