Python PlanningSceneInterface.attach_mesh方法代码示例

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

#.........这里部分代码省略.........
        target_pose.pose.position.y = 0.80576308041
        target_pose.pose.position.z = 0.794775212132
        target_pose.pose.orientation.x = 3.01203251908e-05
        target_pose.pose.orientation.y = 0.705562870053
        target_pose.pose.orientation.z = 4.55236739937e-05
        target_pose.pose.orientation.w = 0.708647326547

        start = input("Start right_wam planning ? ")
        right_wam.set_start_state_to_current_state()
        right_wam.set_pose_target(target_pose, right_end_effector_link)
        right_wam.execute(right_wam.plan())
    
        start = input("right_wam into position. ")  
        start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) 
        intermidiate_pose =  deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
        intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6
        plan = self.StraightPath(start_pose, intermidiate_pose, right_wam)
        right_wam.set_start_state_to_current_state()
        right_wam.execute(plan) 

        rospy.sleep(3)
        
        # 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(3)   
        right_wam_finger_2.execute(right_wam_finger_2.plan())
        #rospy.sleep(3)    
        right_wam_finger_3.execute(right_wam_finger_3.plan())  
        rospy.sleep(1)
        scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl')


        #create a circle path
        circles = input("How many circles you want the wam to mix ? ")
  
        start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
        plan = self.CircularPath(start_pose, circles, right_wam)

        #execute the circle path
        right_wam.set_start_state_to_current_state()
        right_wam.execute(plan)

        pause = input("Mix the oatmeal ")

    
        #put the right_wam back to preparation pose
        end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
        intermidiate_pose1 = deepcopy(end_pose)
        intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1

        plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam)
        right_wam.set_start_state_to_current_state()
        right_wam.execute(plan)

        pause = input("wait for the execution of straight path  ")

        end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
        intermidiate_pose2 = deepcopy(end_pose)
        intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25

        plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam)
        right_wam.set_start_state_to_current_state()

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_mesh [as 别名]
 
 ####################### CLOSE RIGHT HAND ###############################
 scene.remove_world_object("right_shoe")
 rospy.sleep(1)
 res = req(2,2) # 2 -> Right _hand 2-> Grasp
 rospy.sleep(1)
 ####################### ATTACH RIGHT SHOE TO THE ROBOT ###############################
 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
 
 scene.attach_mesh('r_eef', 'right_shoe', pose, roslib.packages.get_pkg_dir('hermes_virtual_robot')+'/common/files/stl/right_shoe.stl',links_off)
 rospy.sleep(1)
 
 
 ####################### POSICION ARRIBA ZAPATO ###############################
 pose.pose.position.x = 0.4493
 pose.pose.position.y = -0.3628
 pose.pose.position.z = 1.1937
 pose.pose.orientation.w = 0.68058
 pose.pose.orientation.x = -0.21466
 pose.pose.orientation.y = 0.65539
 pose.pose.orientation.z = 0.24739
 pose.header.frame_id = robot.get_planning_frame()
 pose.header.stamp = rospy.Time.now()
 
  

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