Exemple #1
0
def move_arm_joint_goal(move_arm_client,
                        joint_names,
                        joint_positions,
                        allowed_contacts=[],
                        link_padding=[],
                        collision_operations=OrderedCollisionOperations()):
    goal = MoveArmGoal()
    goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
    goal.motion_plan_request.planner_id = ''
    goal.motion_plan_request.group_name = ARM_GROUP_NAME
    goal.motion_plan_request.num_planning_attempts = 3
    goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
    goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
    
    goal.planning_scene_diff.allowed_contacts = allowed_contacts
    goal.planning_scene_diff.link_padding = link_padding
    goal.operations = collision_operations
    
    move_arm_client.send_goal(goal)
    finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
    
    if not finished_within_time:
        move_arm_client.cancel_goal()
        rospy.logerr('timed out trying to achieve joint goal')
        return False
    else:
        state = move_arm_client.get_state()
        
        if state == GoalStatus.SUCCEEDED:
            return True
        else:
            rospy.logerr('failed to achieve joint goal (returned status code %s)' % str(state))
            return False
Exemple #2
0
def move_arm_joint_goal(move_arm_client,
                        joint_names,
                        joint_positions,
                        allowed_contacts=[],
                        link_padding=[],
                        collision_operations=OrderedCollisionOperations()):
    goal = MoveArmGoal()
    goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
    goal.motion_plan_request.planner_id = ''
    goal.motion_plan_request.group_name = ARM_GROUP_NAME
    goal.motion_plan_request.num_planning_attempts = 3
    goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
    goal.motion_plan_request.goal_constraints.joint_constraints = [
        JointConstraint(j, p, 0.1, 0.1, 0.0)
        for (j, p) in zip(joint_names, joint_positions)
    ]

    goal.planning_scene_diff.allowed_contacts = allowed_contacts
    goal.planning_scene_diff.link_padding = link_padding
    goal.operations = collision_operations

    move_arm_client.send_goal(goal)
    finished_within_time = move_arm_client.wait_for_result(
        rospy.Duration(200.0))

    if not finished_within_time:
        move_arm_client.cancel_goal()
        rospy.logerr('timed out trying to achieve joint goal')
        return False
    else:
        state = move_arm_client.get_state()

        if state == GoalStatus.SUCCEEDED:
            return True
        else:
            rospy.logerr(
                'failed to achieve joint goal (returned status code %s)' %
                str(state))
            return False
Exemple #3
0
    def __move_arm(self, arm, position, orientation, frame_id,  waiting_time, 
                   ordered_collision_operations = None,
                   allowed_contacts = None):
        
        goal = MoveArmGoal()
        goal.motion_plan_request.group_name = arm
        goal.motion_plan_request.num_planning_attempts = 2
        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/2.)
        
        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = frame_id
        if arm == "right_arm":
            link_name = "r_wrist_roll_link"
            client = self.move_right_arm_client
        elif arm == "left_arm":
            link_name = "l_wrist_roll_link"
            client = self.move_left_arm_client
        else:
            rospy.logerr("Unknown arm: %s"%arm)
            return False
            
        position_constraint.link_name = link_name
        
        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 = link_name
#        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)
        
        if ordered_collision_operations is not None:
            rospy.loginfo("Adding ordered collisions")
            goal.operations = ordered_collision_operations
        if allowed_contacts is not None:
            rospy.loginfo("Adding allowed_contacts")
            goal.planning_scene_diff.allowed_contacts = allowed_contacts
        goal.disable_collision_monitoring = False
        
        state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
        if state == actionlib.GoalStatus.SUCCEEDED:
            return True
        else:
            return False
Exemple #4
0
    def __move_arm(self,
                   arm,
                   position,
                   orientation,
                   frame_id,
                   waiting_time,
                   ordered_collision_operations=None,
                   allowed_contacts=None):

        goal = MoveArmGoal()
        goal.motion_plan_request.group_name = arm
        goal.motion_plan_request.num_planning_attempts = 2
        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 / 2.)

        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = frame_id
        if arm == "right_arm":
            link_name = "r_wrist_roll_link"
            client = self.move_right_arm_client
        elif arm == "left_arm":
            link_name = "l_wrist_roll_link"
            client = self.move_left_arm_client
        else:
            rospy.logerr("Unknown arm: %s" % arm)
            return False

        position_constraint.link_name = link_name

        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 = link_name
        #        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)

        if ordered_collision_operations is not None:
            rospy.loginfo("Adding ordered collisions")
            goal.operations = ordered_collision_operations
        if allowed_contacts is not None:
            rospy.loginfo("Adding allowed_contacts")
            goal.planning_scene_diff.allowed_contacts = allowed_contacts
        goal.disable_collision_monitoring = False

        state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
        if state == actionlib.GoalStatus.SUCCEEDED:
            return True
        else:
            return False