Python PlanningSceneInterface.add_mesh方法代码示例

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

	robotstart = robot.get_current_pose()
	print 'start eef pose:'
	print robotstart
	robotstart_quat = [robotstart.pose.orientation.x,robotstart.pose.orientation.y,robotstart.pose.orientation.z,robotstart.pose.orientation.w]
	M_eef = tf.transformations.quaternion_matrix(robotstart_quat)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_mesh [as 别名]
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.pose.position.x = 0.7
    p.pose.position.y = -0.4
    p.pose.position.z = 0.85
    p.pose.orientation.w = 1.57
    scene.add_box("block1", p, (0.044, 0.044, 0.044))
    p.pose.position.y = -0.2
    p.pose.position.z = 0.175
    scene.add_box("block2", p, (0.044, 0.044, 0.044))
    p.pose.position.x = 0.6
    p.pose.position.y = -0.7
    p.pose.position.z = 0.5
    scene.add_box("block3", p, (0.044, 0.044, 0.044))
    p.pose.position.x = 1
    p.pose.position.y = -0.7
    p.pose.position.z = 0.5
    scene.add_box("block4", p, (0.044, 0.044, 0.044))
    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = 0
    scene.add_mesh("table", p, "table_scaled.stl")
    scene.add_mesh("bowl", p, "bowl_scaled.stl")
    scene.add_mesh("box", p, "box_scaled.stl")
    rospy.sleep(1)
    # pick an object
    robot.right_arm.pick("block2")
    rospy.spin()
    roscpp_shutdown()

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_mesh [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('SceneSetup')
        
        # 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)

        # Set the reference frame for pose targets
        reference_frame = 'rack1'
        table_id = 'table'
        bowl_id = 'bowl'
        pitcher_id = 'pitcher'
  
        # 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)

        # Set the height of the table off the ground
        table_ground = 0.5762625 
        
        # Set the length, width and height of the table and boxes
        table_size = [1.0128, 0.481, 0.0238125]

        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0
        table_pose.pose.position.y = 0.847725
        table_pose.pose.position.z = table_ground

        # Set the height of the bowl
        bowl_ground = 0.57816875 
        bowl_pose = PoseStamped()
        bowl_pose.header.frame_id = reference_frame
        bowl_pose.pose.position.x = 0
        bowl_pose.pose.position.y = 0.847725
        bowl_pose.pose.position.z = bowl_ground

        # Set the height of the pitcher
        pitcher_ground = 0.57816875 
        pitcher_pose = PoseStamped()
        pitcher_pose.header.frame_id = reference_frame
        pitcher_pose.pose.position.x = 0.4
        pitcher_pose.pose.position.y = 0.847725
        pitcher_pose.pose.position.z = pitcher_ground
        pitcher_pose.pose.orientation.w = -0.5
        pitcher_pose.pose.orientation.z = 0.707

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0.4, 0, 1.0)
        self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
        self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
        self.sendColors()   


        
        scene.add_box(table_id, table_pose, table_size)
        scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
               #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')

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

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_mesh [as 别名]
 ####################### 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)
 ####################### PREGRASP RIGHT HAND ###############################
 req = rospy.ServiceProxy('grasp_hand', GraspHand)
 res = req(2,1) # 2 -> Right _hand 1-> PreGrasp
 res = req(1,1) # 2 -> Right _hand 1-> PreGrasp
 rospy.sleep(1)
 ####################### POSICION DE GRASP ###############################
 pose.pose.position.x = 0.4508
 pose.pose.position.y = -0.3605
 pose.pose.position.z = 1.1423
 pose.pose.orientation.w = 0.6812
 pose.pose.orientation.x = -0.21744
 pose.pose.orientation.y = 0.65471
 pose.pose.orientation.z = 0.24387
 pose.header.frame_id = robot.get_planning_frame()

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

#.........这里部分代码省略.........
        # 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
        # Attach the tool to the end-effector
        

        # Set the height of the table off the ground
        table_ground = 0.5762625 
        
        # Set the length, width and height of the table and boxes
        table_size = [0.90128, 0.381, 0.0238125]

        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0
        table_pose.pose.position.y = 0.847725
        table_pose.pose.position.z = table_ground
        scene.add_box(table_id, table_pose, table_size)

        # Set the height of the bowl
        bowl_ground = 0.57816875 
        bowl_pose = PoseStamped()
        bowl_pose.header.frame_id = REFERENCE_FRAME
        bowl_pose.pose.position.x = 0.015
        bowl_pose.pose.position.y = 0.847725
        bowl_pose.pose.position.z = bowl_ground
        scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')

        # Set the height of the pitcher
        #pitcher_ground = 0.57816875 
        #pitcher_pose = PoseStamped()
        #pitcher_pose.header.frame_id = REFERENCE_FRAME
        #pitcher_pose.pose.position.x = 0.25
        #pitcher_pose.pose.position.y = 0.847725
        #pitcher_pose.pose.position.z = pitcher_ground
        #pitcher_pose.pose.orientation.w = -0.5
        #pitcher_pose.pose.orientation.z = 0.707
        #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0.4, 0, 1.0)
        self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
        #self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0)
        self.sendColors() 
        rospy.sleep(2) 

        start = input("Start left_wam planning ? ")

        # Set the support surface name to the table object
        #left_wam.set_support_surface_name(table_id)
        #right_wam.set_support_surface_name(table_id)

        # Set the target pose.
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.header.stamp = rospy.Time.now()  

        target_pose.pose.position.x = 0.40363476287
        target_pose.pose.position.y = 0.847725

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_mesh [as 别名]
 p.pose.orientation.z = 0
 p.pose.orientation.w = 1
 
 # add a box there
 scene.add_box("ground", p, (3, 3, 0.02))
 
 # access some meshes
 rospack = rospkg.RosPack()
 resourcepath = rospack.get_path('regrasping_app')+"/../resources/"
 
 # modify the pose
 p.pose.position.x = 0.7
 p.pose.position.y = 0.7
 p.pose.position.z = 0.0
 # add the cup
 scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
 
 # modify the pose
 p.pose.position.x = 0.72
 p.pose.position.z = 0.05
 # add the pen
 scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
 rospy.sleep(1)
  
 # print the existing groups 
 robot = RobotCommander()
 print "Available groups: ",robot.get_group_names()
 
 # setup the arm group and its planner
 arm = MoveGroupCommander("arm")
 arm.set_start_state_to_current_state()

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_mesh [as 别名]
    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")
    scene.add_mesh("punch", punch_pose, "punch.STL")
    rospy.sleep(1)

#    gripper.set_named_target("pinch")
#    gripper.go()
    
    rad = 4
    rad = inchtometer(rad)
    grasp_start = PoseStamped()
    grasp_start.header.frame_id = 'world'
    grasp_start.header.stamp = rospy.Time.now()
    grasp_start.pose.position.x = 0
    grasp_start.pose.position.y = -0.5+rad
    grasp_start.pose.position.z = 0.4
    grasp_start.pose.orientation.x = 0