Python PlanningSceneInterface.remove_attached_object方法代码示例

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def main():
    rospy.init_node('moveit_py_place', anonymous=True)
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    #group = MoveGroupCommander("head")
    right_arm = MoveGroupCommander("right_arm")
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.logwarn("cleaning world")
    GRIPPER_FRAME = 'gripper_bracket_f2'
    #scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()

    p.pose.position.x = 0.67
    p.pose.position.y = -0.
    p.pose.position.z = 0.75
    scene.add_box("part", p, (0.07, 0.01, 0.2))

    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)

    result = False
    n_attempts = 0
       
    # repeat until will succeed
    while result == False:
        result = robot.right_arm.pick("part")      
        n_attempts += 1
        print "Attempts pickup: ", n_attempts
        rospy.sleep(0.2)
    
    #robot.right_arm.pick("part")
    #right_arm.go()
    rospy.sleep(5)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [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()
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
class MoveItDemo:
    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
        self.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 publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)


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

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

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

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

        # Prepare Action Controller for gripper
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

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


        ### OPEN THE GRIPPER ###
        self.open_gripper()

        self.obj_att = None


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

        ############## CLEAR THE SCENE ################


#        planning_scene.world.collision_objects.remove('target')

        # Remove leftover objects from a previous run
        self.scene.remove_world_object('target')
        self.scene.remove_world_object('table')
#        self.scene.remove_world_object(obstacle1_id)

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

        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()


        initial_pose = target_pose
        initial_pose.header.frame_id = 'gazebo_world'


        print "==================== Generating Transformations ==========================="

        #################### PRE GRASPING POSE #########################
#        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
#        M1[0,3] = target_pose.pose.position.x
#        M1[1,3] = target_pose.pose.position.y 
#        M1[2,3] = target_pose.pose.position.z

#        M2 = transformations.euler_matrix(0, 1.57, 0)
#        M2[0,3] = 0.0  # offset about x
#        M2[1,3] = 0.0   # about y
#        M2[2,3] = 0.25  # about z
#.........这里部分代码省略.........

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

        rospy.init_node('MotionSequence')
        
   
        # 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 left wam finger poses
        self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped)
        self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped)
        self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped)

     

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

        # Define move group comander for each moveit group
        left_wam = moveit_commander.MoveGroupCommander('left_wam')
        right_wam = moveit_commander.MoveGroupCommander('right_wam')
        left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1')
        left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2')
        left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3')
        right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
        right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
        right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')

        left_wam.set_planner_id("PRMstarkConfigDefault")
        right_wam.set_planner_id("PRMstarkConfigDefault")
        #left_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
        #left_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
        #left_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
        #right_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
        #right_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
        #right_wam_finger_3.set_planner_id("RRTstarkConfigDefault")


        # Get the name of the end-effector link
        left_end_effector_link = left_wam.get_end_effector_link()
        right_end_effector_link = right_wam.get_end_effector_link()

        # Display the name of the end_effector link
        rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link))
        rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link))

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

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

        # Allow 5 seconds per planning attempt
        right_wam.set_planning_time(15)
        left_wam.set_planning_time(25)

        # Allow replanning to increase the odds of a solution
        right_wam.allow_replanning(True)
        left_wam.allow_replanning(True)
        
        
        # Set the reference frame for wam arms
        left_wam.set_pose_reference_frame(REFERENCE_FRAME)
        right_wam.set_pose_reference_frame(REFERENCE_FRAME)

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

        # Give each of the scene objects a unique name        
        table_id = 'table'
        bowl_id = 'bowl'
        pitcher_id = 'pitcher'
        spoon_id = 'spoon'
 
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(bowl_id)
        scene.remove_world_object(pitcher_id)
        scene.remove_world_object(spoon_id)

        # Remove leftover objects from a previous run
        scene.remove_attached_object(right_end_effector_link, 'spoon')

        #right_wam.set_named_target('right_wam_start')
        #right_wam.go()
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
     
    
    group_left.set_pose_target(pose, "l_eef")
     
 
    result = group_left.go(None,1)
    
    if result == 0:
        print "ERROR init pos left"
        sys.exit()
    
    

    
    ####################### ADD RIGHT SHOE TO THE SCENE ###############################
    scene.remove_attached_object('r_eef', 'right_shoe')
    
    rospy.sleep(1)
    
    pose.pose.position.x = 0.475
    pose.pose.position.y = -0.1500
    pose.pose.position.z = 1.035
    pose.pose.orientation.w = 0.99144
    pose.pose.orientation.x = 0
    pose.pose.orientation.y = 0
    pose.pose.orientation.z = 0.13053
    pose.header.frame_id = robot.get_planning_frame()
    pose.header.stamp = rospy.Time.now()
    path = roslib.packages.get_pkg_dir('hermes_virtual_robot')+'/common/files/stl/right_shoe.stl'
    scene.add_mesh("right_shoe", pose, path)
    rospy.sleep(1)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [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 PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('AttachMesh')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                                
        # Initialize the MoveIt! commander for the right wam and fingers
        right_wam = moveit_commander.MoveGroupCommander('right_wam')

        right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
        right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
        right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')
                
      
        
        # Get the name of the end-effector link
        end_effector_link = right_wam.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_wam.set_goal_position_tolerance(0.01)
        right_wam.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_wam.allow_replanning(True)
        
        # Allow 5 seconds per planning attempt
        right_wam.set_planning_time(5)
        
        # Remove leftover objects from a previous run
        scene.remove_attached_object(end_effector_link, 'spoon')
        
        right_wam.set_named_target('right_wam_start')
        right_wam.go()
        rospy.sleep(2)
        # Closing the hand first
        # Closing the hand
        right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
        right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
        right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
 
        right_wam_finger_1.execute(right_wam_finger_1.plan())
        rospy.sleep(5)   
        right_wam_finger_2.execute(right_wam_finger_2.plan())
        rospy.sleep(5)    
        right_wam_finger_3.execute(right_wam_finger_3.plan())  
        rospy.sleep(5)
        
        # 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
        
        # Place the end of the object within the grasp of the gripper
        p.pose.position.x = 0.0
        p.pose.position.y = 0.0
        p.pose.position.z = -0.02
        
        # Align the object with the gripper (straight out)
        p.pose.orientation.x = -0.5
        p.pose.orientation.y = 0.5
        p.pose.orientation.z = -0.5
        p.pose.orientation.w = 0.5
        

        
        scene.attach_mesh(end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl')
        

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

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

        global obj_att

        # 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
        self.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 publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)


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

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

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

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

        # Prepare Action Controller for gripper
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

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


        ### OPEN THE GRIPPER ###
        self.open_gripper()


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

        ############## CLEAR THE SCENE ################


#        planning_scene.world.collision_objects.remove('target')

        # Remove leftover objects from a previous run
        self.scene.remove_world_object('target')
        self.scene.remove_world_object('table')
#        self.scene.remove_world_object(obstacle1_id)

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

        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()

        initial_pose = PoseStamped()
        initial_pose.header.frame_id = 'gazebo_world'
        initial_pose.pose = target_pose.pose
        

        print "==================== Generating Transformations ==========================="

        #################### PRE GRASPING POSE #########################
        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
        M1[0,3] = target_pose.pose.position.x
        M1[1,3] = target_pose.pose.position.y 
        M1[2,3] = target_pose.pose.position.z

        M2 = transformations.euler_matrix(0, 1.57, 0)
        M2[0,3] = 0.0  # offset about x
        M2[1,3] = 0.0   # about y
        M2[2,3] = 0.25  # about z

        T = np.dot(M1,  M2)
#.........这里部分代码省略.........

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

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander("right_arm")
        right_gripper = MoveGroupCommander("right_gripper")
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object("right_gripper_link", "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        right_arm.set_named_target("start1")
        right_arm.go()
      
        right_gripper.set_named_target("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.15

        p.pose.position.y = -0.12
        p.pose.position.z = 0.7
        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.0130178
        start_pose.pose.position.y = -0.125155
        start_pose.pose.position.z = 0.597653
        start_pose.pose.orientation.x = 0.0
        start_pose.pose.orientation.y = 0.388109
        start_pose.pose.orientation.z = 0.0
        start_pose.pose.orientation.w = 0.921613
          
        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.2)
          
        rospy.spin()
        roscpp_shutdown()

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
class BTMotion:

    def __init__(self, name):
        # create messages that are used to publish feedback/result
        self._feedback = amazon_challenge_bt_actions.msg.BTFeedback()
        self._result   = amazon_challenge_bt_actions.msg.BTResult()
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, amazon_challenge_bt_actions.msg.BTAction, execute_cb=self.execute_cb, auto_start = False)
        self.pub_posed = rospy.Publisher('arm_posed', String, queue_size=10)
        self.pub_rate = rospy.Rate(30)

        self._planning_scene = PlanningSceneInterface()


        # get ROS parameters
        rospy.loginfo('Getting parameters...')
        while not rospy.is_shutdown():
            try:
                self._base_move_params = rospy.get_param('/base_move')
                self._timeout = rospy.get_param(name + '/timeout')
                self._sim = rospy.get_param(name + '/sim')
                self._base_pos_dict = rospy.get_param('/base_pos_dict')
                self._left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                self._right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,2))
                continue

        self._exit = False

        while not rospy.is_shutdown():
            try:
                self._robot = moveit_commander.RobotCommander()
                self._left_arm = self._robot.get_group('left_arm')
                self._right_arm = self._robot.get_group('right_arm')
                self._arms = self._robot.get_group('arms')
                self._torso = self._robot.get_group('torso')
                self._head = self._robot.get_group('head')
                self._arms_dict = {'left_arm': self._left_arm, 'right_arm': self._right_arm}
                break
            except:
                rospy.sleep(random.uniform(0,2))
                continue


        self._tf_listener = tf.TransformListener()

        self._next_task_sub = rospy.Subscriber("/amazon_next_task", String, self.get_task)
        self._shelf_pose_sub = rospy.Subscriber("/pubShelfSep", PoseStamped, self.get_shelf_pose)
        self._got_shelf_pose = False

        self._l_gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)
        while not rospy.is_shutdown():
            try:
                self._tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04])
                self._contest = rospy.get_param('/contest', True)
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        if self._contest:
            self._length_tool = 0.18 + self._tool_size[0]
        else:
            self._length_tool = 0.216 + self._tool_size[0]

        self._as.start()
        rospy.loginfo('['+rospy.get_name()+']: ready!')


    def get_shelf_pose(self, msg):
        self._shelf_pose = msg
        self._got_shelf_pose = True

    def get_bm_srv(self):
        while not rospy.is_shutdown():
            try:
                rospy.wait_for_service('/base_move_server/move', 5.0)
                rospy.wait_for_service('/base_move_server/preempt', 5.0)
                break
            except:
                rospy.loginfo('[' + rospy.get_name() + ']: waiting for base move server')
                continue

        self._bm_move_srv = rospy.ServiceProxy('/base_move_server/move', BaseMove)
        self._bm_preempt_srv = rospy.ServiceProxy('/base_move_server/preempt', Empty)

    def timer_callback(self, event):
        self._timer_started = True
        rospy.logerr('[' + rospy.get_name() + ']: TIMED OUT!')
        self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object')
        self._planning_scene.remove_world_object('grasped_object')


        # pull the base back 60 cm

        self._left_arm.stop()
        self._right_arm.stop()

#.........这里部分代码省略.........

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

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    GRIPPER_FRAME = 'right_gripper_link'
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    right_gripper = MoveGroupCommander("right_gripper")
    right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    #rospy.logwarn("cleaning world")
    #right_arm.set_named_target("r_start")
    #right_arm.go()
   
    #right_gripper.set_named_target("open")
    #right_gripper.go()
   
    rospy.sleep(3)

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()

    # add a table
    p.pose.position.x = 1.0

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [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 PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [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 PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]

#.........这里部分代码省略.........



    def scene_generator(self):

        obj_pose =[]
        obj_id = []
        obj_size = []
        bl = ['ground_plane','pr2'] 
        global obj_pose, obj_id , obj_size

        ops = PoseStamped()
        ops.header.frame_id = REFERENCE_FRAME


        for model_name in self.pwh.name:
            if model_name not in bl:
                obj_id.append(model_name)
                ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
                obj_pose.append(deepcopy(ops))
                obj_size.append([0.05, 0.05, 0.15])


        obj_id[obj_id.index('custom_1')] = 'target'
        obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
        obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
        obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]

        ### REMOVE OBJECT FROM PREVIOUS RUNS ###
        for i in range(0, len(obj_id)):
            self.scene.remove_world_object(obj_id[i])

        # Remove any attached objects from a previous session
        self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')])

        next_call = time.time()

        while True:
            next_call = next_call+1

            if self.aro is None:
                for i in range(0, len(obj_id)):
                    ### CREATE THE SCENE ###
                    self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])

                ### Make the target purple and obstacles orange ###
                self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
                self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0)
                self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0)
                self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0)


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

                ###### ATTACH ITEM TO GRIPPER ######
                touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
                #print touch_links
                self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)

                ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
                self.scene.remove_world_object(obj_id[self.aro])

            time.sleep(next_call - time.time())

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
        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())

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

        # remove part
        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)