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)
    def testMotionExecutionBuffer(self):

        global padd_name
        global extra_buffer

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

        joint_names = [
            '%s_%s' % ('r', j) for j in [
                'shoulder_pan_joint', 'shoulder_lift_joint',
                'upper_arm_roll_joint', 'elbow_flex_joint',
                'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
            ]
        ]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "chomp_planner_longrange/plan_path"
        goal.disable_collision_monitoring = True

        r = rospy.Rate(10)

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]
        for z in range(6):
            for i in range(len(joint_names)):
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    i].joint_name = joint_names[i]
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    i].position = 0.0
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    i].tolerance_above = 0.08
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    i].tolerance_below = 0.08

            if (z % 2 == 0):
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    0].position = -1.0
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    3].position = -0.2
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    5].position = -0.2
            else:
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    0].position = .22
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    1].position = .51
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    2].position = .05
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    3].position = -0.41
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    4].position = -.06
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    5].position = -.1
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    6].position = 0.0

            for x in range(3):

                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
                    r.sleep()

                end_state = self.move_arm_action_client.get_state()

                if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
                    break

            final_state = self.move_arm_action_client.get_state()

            self.assertEqual(final_state,
                             actionlib_msgs.msg.GoalStatus.SUCCEEDED)
Beispiel #3
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
Beispiel #4
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
Beispiel #5
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
    def testMotionExecutionBuffer(self):
        
        global padd_name
        global extra_buffer
        
        #too much trouble to read for now
        allow_padd = .05#rospy.get_param(padd_name)
        

        joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "chomp_planner_longrange/plan_path"
        goal.disable_collision_monitoring = True;

        r = rospy.Rate(10)

        goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
        for z in range(6):
            for i in range(len(joint_names)):
                goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
                goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
                goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
                goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08

            if(z%2==0):
                goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0
                goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
                goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
            else:
                goal.motion_plan_request.goal_constraints.joint_constraints[0].position = .22
                goal.motion_plan_request.goal_constraints.joint_constraints[1].position = .51
                goal.motion_plan_request.goal_constraints.joint_constraints[2].position = .05
                goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41
                goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -.06
                goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -.1
                goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0

            for x in range(3):

                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
                    r.sleep()
                
                end_state = self.move_arm_action_client.get_state()
             
                if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
                
            final_state = self.move_arm_action_client.get_state()

            self.assertEqual(final_state,  actionlib_msgs.msg.GoalStatus.SUCCEEDED)
Beispiel #7
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