Python MoveGroupCommander.get_end_effector_link方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_test_cartesian', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)

        rospy.sleep(2)

        pose = PoseStamped().pose

        # same orientation for all
        q = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
        pose.orientation = Quaternion(*q)

        i = 1
        while i <= 10:
            waypoints = list()
            j = 1
            while j <= 5:
                # random pose
                rand_pose = arm.get_random_pose(arm.get_end_effector_link())
                pose.position.x = rand_pose.pose.position.x
                pose.position.y = rand_pose.pose.position.y
                pose.position.z = rand_pose.pose.position.z
                waypoints.append(copy.deepcopy(pose))
                j += 1

            (plan, fraction) = arm.compute_cartesian_path(
                                      waypoints,   # waypoints to follow
                                      0.01,        # eef_step
                                      0.0)         # jump_threshold

            print "====== waypoints created ======="
            # print waypoints

            # ======= show cartesian path ====== #
            arm.go()
            rospy.sleep(10)
            i += 1

        print "========= end ========="

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
                
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.02)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        
        # Move to the named "straight_forward" pose stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)
        
        # Move back to the resting position at roughly 1/4 speed
        right_arm.set_named_target('resting')

        # Get back the planned trajectory
        traj = right_arm.plan()
        
        # Scale the trajectory speed by a factor of 0.25
        new_traj = scale_trajectory_speed(traj, 0.25)

        # Execute the new trajectory     
        right_arm.execute(new_traj)
        rospy.sleep(1)

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_move_position', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)
        arm.set_planning_time(10)

        arm.set_named_target(START_POSITION)
        arm.go()
        rospy.sleep(2)

        print "======== create new goal position ========"

        start_pose = PoseStamped()
        start_pose.header.frame_id = arm.get_planning_frame()

        # Test Position
        start_pose.pose.position.x = 0.2  # 20 cm
        start_pose.pose.position.y = -0.11  # -11 cm
        start_pose.pose.position.z = 0.6  # 60 cm
        q = quaternion_from_euler(0.0, 0.0, 0.0)
        start_pose.pose.orientation = Quaternion(*q)

        print start_pose

        print "====== move to position ======="


        # KDL
        # arm.set_joint_value_target(start_pose, True)

        # IK Fast
        arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
                                arm.get_end_effector_link())

        arm.go()
        rospy.sleep(2)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    # arm.set_goal_tolerance(0.01)

    # Action Clients for Place
    rospy.loginfo("Connecting to place AS")
    place_ac = actionlib.SimpleActionClient('/place', PlaceAction)
    place_ac.wait_for_server()
    rospy.loginfo("Successfully connected")

    rospy.sleep(1)

    # take the part to specific pose
    pose = generate_rand_position()

    # IKFast
    arm.set_position_target([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z],
                            arm.get_end_effector_link())
    # KDL
    # arm.set_joint_value_target(pose, True)

    arm.go()
    rospy.sleep(5)

    #  ===== place start ==== #
    place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())

    rospy.loginfo("Place Result is:")
    rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
    rospy.sleep(5)

    # end
    moveit_commander.roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('right_arm')
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_footprint')
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
                                        
        # Start in the "straight_forward" configuration stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = right_arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Set the next waypoint back 0.2 meters and right 0.2 meters
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
         
        # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            right_arm.set_pose_target(start_pose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            right_arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = right_arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.01,        # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')

        right_arm.set_planner_id("KPIECEkConfigDefault");
        left_arm.set_planner_id("KPIECEkConfigDefault");
        rospy.sleep(1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'
        
        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        
        # Give the scene a chance to catch up
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        rospy.sleep(2)
        
        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.56
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.51
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    group = MoveGroupCommander("arm")
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print group.get_end_effector_link()
    print group.get_pose_reference_frame()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"
    tl = tf.TransformListener()
    
    rospy.sleep(1)

    waypoints = []
    
    # start with the current pose
    waypoints.append(group.get_current_pose().pose)
    print waypoints[0]
    currentPose = PoseStamped()
    currentPose.header.frame_id = group.get_pose_reference_frame()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
                                
        # Increase the planning time since contraint planning can take a while
        right_arm.set_planning_time(15)
                        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Start in the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')
         
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0
         
        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)
        
        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)
        
        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)
        
        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        
        # Create an orientation constraint for the right gripper 
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0
        
        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
          
        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)
        
        # Set a target pose for the arm        
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Clear all path constraints
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    p.header.frame_id = REFERENCE_LINK
    p.header.stamp = rospy.Time.now()

    quat = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
    p.pose.orientation = Quaternion(*quat)

    p.pose.position.x = 0.6
    p.pose.position.y = 0.0
    p.pose.position.z = 0.0
    # add table
    scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))

    i = 1
    while i <= 10:

        gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
        # part
        p.pose.position.x = gripper_pose.pose.position.x
        p.pose.position.y = gripper_pose.pose.position.y
        p.pose.position.z = gripper_pose.pose.position.z
        # add part
        scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
        rospy.loginfo("Added object to world")

        # attach object manually
        arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
        rospy.sleep(1)

        #  ===== place start ==== #
        place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        eef = right_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
#        right_arm.set_goal_position_tolerance(0.05)
#        right_arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)

        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5

        # Set a limit on the number of place attempts
        max_place_attempts = 5

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)

        # Prepare Gripper and open it
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
        self.ac.send_goal(g_open)

        rospy.sleep(2)
        
        # PREPARE THE SCENE
       	
       	while self.pwh is None:
            rospy.sleep(0.05)

        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')

        # Remove leftover objects from a previous run
        scene.remove_world_object(target_id)
        scene.remove_world_object(table_id)
        #scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Set the target size [l, w, h]
        target_size = [0.05, 0.05, 0.05]
        table_size = [1.5, 0.8, 0.03]
        #obstacle1_size = [0.1, 0.025, 0.01]

        ## Set the target pose on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose = self.pwh.pose[self.taid]
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        #right_arm.set_planner_id("KPIECEkConfigDefault");
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object(GRIPPER_FRAME, "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        #right_arm.set_named_target("r_start")
        #right_arm.go()
      
        #right_gripper.set_named_target("right_gripper_open")
        #right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.7
        p.pose.position.y = -0.2
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.47636
        start_pose.pose.position.y = -0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)
   
        result = False
        n_attempts = 0
       
        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)      
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)
          
        rospy.spin()
        roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
        pose_grasp = copy.deepcopy(p)

        # create list of grasps
        possible_grasps = create_random_grasps(pose_grasp.pose, 100)

        # create goal for pick
        goal = create_pickup_goal(PLANNING_GROUP, PICK_OBJECT, pose_grasp, possible_grasps)
        rospy.loginfo("Sending goal")
        pickup_ac.send_goal(goal)

        rospy.loginfo("Waiting for result")
        pickup_ac.wait_for_result()
        result = pickup_ac.get_result()

        rospy.loginfo("Pick Result is:")
        rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
        rospy.sleep(5)

         # clean world
        rospy.loginfo("Cleaning world objects")
        scene.remove_world_object(TABLE_OBJECT)
        scene.remove_world_object(PICK_OBJECT)
        scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT)
        rospy.sleep(2)
        i += 1

    # end
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the arm move group
        arm = MoveGroupCommander('arm')
        
        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)
        
        # Set the right arm reference frame
        arm.set_pose_reference_frame('base_link')
                
        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
                                        
        # Set an initial position for the arm
        start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]

        # Set the goal pose of the end effector to the stored pose
        arm.set_joint_value_target(start_position)
        
        # Plan and execute a trajectory to the goal configuration
        arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Move end effector to the right 0.3 meters
        wpose.position.y -= 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        # Move end effector up and back
        wpose.position.x -= 0.2
        wpose.position.z += 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.025,       # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                                
        # Initialize the MoveIt! commander for the right arm
        right_arm = MoveGroupCommander('right_arm')
                
        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander('right_gripper')
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Remove leftover objects from a previous run
        scene.remove_attached_object(end_effector_link, 'tool')
        scene.remove_world_object('table') 
        scene.remove_world_object('box1')    
        scene.remove_world_object('box2')
        scene.remove_world_object('target')

        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table
        table_size = [0.2, 0.7, 0.01]
        
        # Set the length, width and height of the object to attach
        tool_size = [0.3, 0.02, 0.02]
        
        # Create a pose for the tool relative to the end-effector
        p = PoseStamped()
        p.header.frame_id = end_effector_link
        
        scene.attach_mesh
        
        # Place the end of the object within the grasp of the gripper
        p.pose.position.x = tool_size[0] / 2.0 - 0.025
        p.pose.position.y = 0.0
        p.pose.position.z = 0.0
        
        # Align the object with the gripper (straight out)
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        
        # Attach the tool to the end-effector
        scene.attach_box(end_effector_link, 'tool', p, tool_size)

        # Add a floating table top
        table_pose = PoseStamped()
        table_pose.header.frame_id = 'base_footprint'
        table_pose.pose.position.x = 0.35
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box('table', table_pose, table_size)
        
        # Update the current state
        right_arm.set_start_state_to_current_state()

        # Move the arm with the attached object to the 'straight_forward' position
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)  
        
        # Return the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(2)
         
        scene.remove_attached_object(end_effector_link, 'tool')   
        
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_end_effector_link [as 别名]
if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("arm")
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print right_arm.get_end_effector_link()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"

    
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("table")
    #scene.remove_world_object("part")
    scene.remove_world_object("grasp_marker")
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()