Python MoveGroupCommander.set_named_target方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [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 set_named_target [as 别名]
def main():
    rospy.init_node('arm_start_up', anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    left_arm = MoveGroupCommander("left_arm")
    right_arm.set_planner_id("RRTConnect:"); 
    left_arm.set_planner_id("RRTConnect:")
    
  
    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)
    right_arm.set_named_target("right_start")
    right_arm.go()
    rospy.sleep(1)
    left_arm.set_named_target("left_start")
    left_arm.go()
    rospy.sleep(1)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [as 别名]
class Robot:
    def __init__(self, config, debug=0):
        print "[INFO] Initialise Robot"
        self.DEBUG = debug
        # configuration
        self.config = config
        # initialise move groups
        self.arm = MoveGroupCommander(ARM_GROUP)
        self.gripper = MoveGroupCommander(GRIPPER_GROUP)
        # initialise pick and place manager
        if self.DEBUG is 1:
            # verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
        else:
            # non verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)

        # tolerance: position (in m), orientation (in rad)
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)

        self.place_manager = None
        self.pick_manager = None

        self.initialise_move_actions()
        self.start_position()

    """Initialise the place and pick manager.
    """
    def initialise_move_actions(self):
        self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG)
        self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG)

    """Move robot arm to "start position".
    """
    def start_position(self):
        self.arm.set_named_target(START_POSITION)
        self.arm.go()
        rospy.sleep(2)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [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 set_named_target [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 set_named_target [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 set_named_target [as 别名]
#!/usr/bin/env python

import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander

if __name__ == '__main__':

	rospy.init_node('heh')
	scene = PlanningSceneInterface()

	robot = MoveGroupCommander("sia5d");
	gripper = MoveGroupCommander("gripper");
	rospy.sleep(1)
	
	while not rospy.is_shutdown():	
	
		gripper.set_named_target('pinch')
		gripper.go()
		rospy.sleep(1)
		gripper.set_named_target('default')
		gripper.go()
		rospy.sleep(1)
		
	rospy.spin()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [as 别名]
    punch_pose.pose.position.z = 0
    temp_eul = None

    #detach/remove current scene objects
    print 'removing objects...'
    robot.detach_object("bowl")
    rospy.sleep(0.5)
    scene.remove_world_object("bowl")
    scene.remove_world_object("punch")
    scene.remove_world_object("flat")

    rospy.sleep(0.5)

    #reset the gripper and arm position to home
    robot.set_start_state_to_current_state()
    robot.set_named_target("home")
    robot.go()
    startpose = PoseStamped()
    startpose.header.frame_id = 'world'
    startpose.header.stamp = rospy.Time.now()
    startpose = robot.get_current_pose()
    startpose.pose.position.x -= 0.3
    robot.set_pose_target(startpose)
    robot.go()
#    gripper.set_start_state_to_current_state()
#    gripper.set_named_target("default")
#    gripper.go()

    #add scene objects
    print 'adding scene objects'
    scene.add_mesh("bowl", bowl_pose, "8inhemi.STL")

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [as 别名]
class ArmTracker:
    def __init__(self):
        rospy.init_node('arm_tracker')
        
        rospy.on_shutdown(self.shutdown)
        
        # Maximum distance of the target before the arm will lower
        self.max_target_dist = 1.2
        
        # Arm length to center of gripper frame
        self.arm_length = 0.4
        
        # Distance between the last target and the new target before we move the arm
        self.last_target_threshold = 0.01
        
        # Distance between target and end-effector before we move the arm
        self.target_ee_threshold = 0.025
        
        # Initialize the move group for the right arm
        self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Set the reference frame for pose targets
        self.reference_frame = REFERENCE_FRAME
        
        # Keep track of the last target pose
        self.last_target_pose = PoseStamped()
        
        # Set the right arm reference frame accordingly
        self.right_arm.set_pose_reference_frame(self.reference_frame)
                        
        # Allow replanning to increase the chances of a solution
        self.right_arm.allow_replanning(False)
                
        # Set a position tolerance in meters
        self.right_arm.set_goal_position_tolerance(0.05)
        
        # Set an orientation tolerance in radians
        self.right_arm.set_goal_orientation_tolerance(0.2)
        
        # What is the end effector link?
        self.ee_link = self.right_arm.get_end_effector_link()
        
        # Create the transform listener
        self.listener = tf.TransformListener()
        
        # Queue up some tf data...
        rospy.sleep(3)
        
        # Set the gripper target to closed position using a joint value target
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
         
        # Plan and execute the gripper motion
        right_gripper.go()
        rospy.sleep(1)
                
        # Subscribe to the target topic
        rospy.wait_for_message('/target_pose', PoseStamped)
        
        # Use queue_size=1 so we don't pile up outdated target messages
        self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
        
        rospy.loginfo("Ready for action!")
        
        while not rospy.is_shutdown():
            try:
                target = self.target
            except:
                rospy.sleep(0.5)
                continue
                        
            # Timestamp the target with the current time
            target.header.stamp = rospy.Time()
            
            # Get the target pose in the right_arm shoulder lift frame
            #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
            target_arm = self.listener.transformPose('right_arm_base_link', target)
            
            # Convert the position values to a Python list
            p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
             
            # Compute the distance between the target and the shoulder link
            dist_target_shoulder = euclidean(p0, [0, 0, 0])
                                         
            # If the target is too far away, then lower the arm
            if dist_target_shoulder > self.max_target_dist:
                rospy.loginfo("Target is too far away")
                self.right_arm.set_named_target('resting')
                self.right_arm.go()
                rospy.sleep(1)
                continue
            
            # Transform the pose to the base reference frame
            target_base = self.listener.transformPose(self.reference_frame, target)
            
            # Compute the distance between the current target and the last target
            p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
            p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [as 别名]
	p2.pose.orientation.z = b2[2]
	p2.pose.orientation.w = b2[3]

	#detach/remove current scene objects
	print 'removing objects...'
	robot.detach_object("bowl")
	rospy.sleep(1)
	scene.remove_world_object("bowl")
	scene.remove_world_object("punch")
	scene.remove_world_object("glovebox")

	rospy.sleep(2)

	#reset the gripper and arm position to home
	robot.set_start_state_to_current_state()
	robot.set_named_target("start_glove")
	robot.go()
	gripper.set_start_state_to_current_state()
	gripper.go()

	#add scene objects	
	print 'adding scene objects'
	scene.add_mesh("bowl", p, "8inhemi.STL")
	scene.add_mesh("punch", p1, "punch.STL")
	scene.add_mesh("glovebox", p2, "GloveBox.stl")
	print 'attaching bowl...'
	robot.attach_object("bowl")
	rospy.sleep(2)
	currentbowlpose = p;

	gripper.set_named_target("pinch")

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

        rospy.init_node('moveit_obstacles_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, queue_size=5)

        # 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
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

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

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

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

        # Allow 5 seconds per planning attempt
        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
        arm.set_named_target('l_arm_init')
        arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.65

        # 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.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_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.3
        box1_pose.pose.position.y = 0
        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.3
        box2_pose.pose.position.y = 0.25
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
                
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface("base_link")
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_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.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(15)
        
        # 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 = 3
                
        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Connect to the UBR-1 find_objects action server
        rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
        find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
        find_objects.wait_for_server()
        rospy.loginfo("...connected")
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)
        
        # Begin the main perception and pick-and-place loop
        while not rospy.is_shutdown():
            # Initialize the grasping goal
            goal = FindGraspableObjectsGoal()
            
            # We don't use the UBR-1 grasp planner as it does not work with our gripper
            goal.plan_grasps = False
            
            # Send the goal request to the find_objects action server which will trigger
            # the perception pipeline
            find_objects.send_goal(goal)
            
            # Wait for a result
            find_objects.wait_for_result(rospy.Duration(5.0))
            
            # The result will contain support surface(s) and objects(s) if any are detected
            find_result = find_objects.get_result()
    
            # Display the number of objects found
            rospy.loginfo("Found %d objects" % len(find_result.objects))
    
            # Remove all previous objects from the planning scene
            for name in scene.getKnownCollisionObjects():
                scene.removeCollisionObject(name, False)
            for name in scene.getKnownAttachedObjects():
                scene.removeAttachedObject(name, False)
            scene.waitForSync()
            
            # Clear the virtual object colors
            scene._colors = dict()
    
            # Use the nearest object on the table as the target
#.........这里部分代码省略.........

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

    # Initialize the move_group API
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node("cobra_move_pick")

    arm = MoveGroupCommander(PLANNING_GROUP)

    # # tolerance
    # arm.set_goal_position_tolerance(0.01)
    # arm.set_goal_orientation_tolerance(0.01)
    # arm.set_goal_joint_tolerance(0.01)
    # arm.set_goal_tolerance(0.01)

    # move arm to start position
    arm.set_named_target(START_POSITION)
    arm.go()
    rospy.sleep(2)

    # Action Clients for Pick
    rospy.loginfo("Connecting to pickup AS")
    pickup_ac = actionlib.SimpleActionClient('/pickup', PickupAction)
    pickup_ac.wait_for_server()
    rospy.loginfo("Successfully connected")

    # create Planning Scene
    scene = PlanningSceneInterface()
    rospy.sleep(1)

    # clean world
    rospy.loginfo("Cleaning world objects")

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_named_target [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 set_named_target [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # 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)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_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.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 10 seconds per planning attempt
        right_arm.set_planning_time(10)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10
        
        # Set a limit on the number of place attempts
        max_place_attempts = 5
                
        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name        
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'target'
        tool_id = 'tool'
                
        # 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)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)
        
        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.65
        
        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Set the target size [l, w, h]
        target_size = [0.02, 0.01, 0.12]
        
        # 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.55
        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()
#.........这里部分代码省略.........