Python PlanningSceneInterface.attach_box方法代码示例

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

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

        if cmd == "id":
            return (MoveGroupInfoLevel.INFO, g.get_name())

        if cmd == "help":
            return (MoveGroupInfoLevel.INFO, self.get_help())

        if cmd == "vars":
            known = g.get_remembered_joint_values()
            return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")

        if cmd == "joints":
            joints = g.get_joints()
            return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")

        if cmd == "show":
            return self.command_show(g)

        if cmd == "current":
            return self.command_current(g)

        if cmd == "ground":
            pose = PoseStamped()
            pose.pose.position.x = 0
            pose.pose.position.y = 0
            # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
            pose.pose.position.z = -0.0501
            pose.pose.orientation.x = 0
            pose.pose.orientation.y = 0
            pose.pose.orientation.z = 0
            pose.pose.orientation.w = 1
            pose.header.stamp = rospy.get_rostime()
            pose.header.frame_id = self._robot.get_root_link()
            self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
            return (MoveGroupInfoLevel.INFO, "Added ground")

        if cmd == "eef":
            if len(g.get_end_effector_link()) > 0:
                return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
            else:
                return (MoveGroupInfoLevel.INFO, "There is no end effector defined")

        if cmd == "database":
            if self._db_host == None:
                return (MoveGroupInfoLevel.INFO, "Not connected to a database")
            else:
                return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
        if cmd == "plan":
            if self._last_plan != None:
                return (MoveGroupInfoLevel.INFO, str(self._last_plan))
            else:
                return (MoveGroupInfoLevel.INFO, "No previous plan")

        if cmd == "constrain":
            g.clear_path_constraints()
            return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")

        if cmd == "tol" or cmd == "tolerance":
            return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())

        if cmd == "time":
            return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))

        if cmd == "execute":
            if self._last_plan != None:
                if g.execute(self._last_plan):

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

#.........这里部分代码省略.........
                    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]



            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])
                    self.setColor(obj_id[i], 1, 0.623, 0, 1.0)

                ### Make the target purple and table green ###
                self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
                self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0)

                self.scene.remove_attached_object(GRIPPER_FRAME)


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

            else:
                if self.flag == 0:
                    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', 'r_wrist_roll_link', 'r_upper_arm_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])
                    self.flag +=1 

            time.sleep(0.5)




    def model_state_callback(self,msg):

        self.pwh = ModelStates()
        self.pwh = msg

    def bl(self):
        blist = ['target','custom_2','custom_3', 'custom_table'] 
        self.blist = []
        for name in obj_id:
            if name not in blist:
                self.blist.append(obj_id.index(name))
                # Remove any attached objects from a previous session
                self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)])

        self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
        return self.blist

    def drange(self, start, stop, step):
        r = start
        while r < stop:
            yield r
            r += step

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

#.........这里部分代码省略.........
        M3[2,3] = target_pose.pose.position.z

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

        T2 = np.dot(M3,  M4)
        grasping = deepcopy(target_pose)
        grasping.pose.position.x = T2[0,3] 
        grasping.pose.position.y = T2[1,3]
        grasping.pose.position.z = T2[2,3]

        quat2 = transformations.quaternion_from_matrix(T2)
        grasping.pose.orientation.x = quat2[0]
        grasping.pose.orientation.y = quat2[1]
        grasping.pose.orientation.z = quat2[2]
        grasping.pose.orientation.w = quat2[3]
        grasping.header.frame_id = 'gazebo_world'

        self.plan_exec(grasping)


        #Close the gripper
        print "========== Waiting for gazebo to catch up =========="
        self.close_gripper()



        #################### ATTACH OBJECT ######################

        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, target_id, target_pose, target_size, touch_links)

        # counter to let the planning scene know when to remove the object
        obj_att = 1

        #self.scene.remove_world_object(target_id)

        #################### POST-GRASP RETREAT #########################

        M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
        M5[0,3] = initial_pose.pose.position.x
        M5[1,3] = initial_pose.pose.position.y 
        M5[2,3] = initial_pose.pose.position.z

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

        T3 = np.dot(M5,  M6)
        post_grasping = deepcopy(initial_pose)
        post_grasping.pose.position.x = T3[0,3] 
        post_grasping.pose.position.y = T3[1,3]
        post_grasping.pose.position.z = T3[2,3]

        quat3 = transformations.quaternion_from_matrix(T3)
        post_grasping.pose.orientation.x = quat3[0]
        post_grasping.pose.orientation.y = quat3[1]
        post_grasping.pose.orientation.z = quat3[2]
        post_grasping.pose.orientation.w = quat3[3]
        post_grasping.header.frame_id = 'gazebo_world'

        self.plan_exec(post_grasping)

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

#.........这里部分代码省略.........
        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())





    def model_state_callback(self,msg):

        self.pwh = ModelStates()
        self.pwh = msg


    def drange(self, start, stop, step):
        r = start
        while r < stop:
            yield r
            r += step

    def close_gripper(self):

        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
        self.ac.send_goal(g_close)
        self.ac.wait_for_result()
        rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object


    def open_gripper(self):

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [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 attach_box [as 别名]
def simple_function():
        rc = RobotCommander()
        mgc = MoveGroupCommander("manipulator")
        # print(rc.get_group_names())
        # print(rc.get_group('manipulator'))
        
        
        # exit()
        
        eef_step = 0.01
        jump_threshold = 2
        ### Create a handle for the Planning Scene Interface
        psi = PlanningSceneInterface()
        
        
        rc.get_current_state()
        rospy.logwarn(rc.get_current_state())
        sss.wait_for_input()
        #rate = rospy.Rate(0.1) # 10hz
        rate = rospy.Rate(1) # 10hz
        rospy.sleep(1)
        
        
        theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
        
        while not rospy.is_shutdown():
            approach_pose = PoseStamped()
            approach_pose.header.frame_id = "table"
            approach_pose.header.stamp = rospy.Time(0)
            approach_pose.pose.position.x = 0.146
            approach_pose.pose.position.y = 0.622
            approach_pose.pose.position.z = 0.01
            quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
            approach_pose.pose.orientation.x = quat[0]
            approach_pose.pose.orientation.y = quat[1]
            approach_pose.pose.orientation.z = quat[2]
            approach_pose.pose.orientation.w = quat[3]
#             mgc.set_start_state_to_current_state()
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed before adding collision objects")
#             else:
#                 rospy.logwarn("Planning succeeded before adding collision objects")
# 
#             rospy.logwarn("waiting for input to add dummy box")
#             sss.wait_for_input()
#             
            box_dummy_pose = PoseStamped()
            box_dummy_pose.header.frame_id =  "table"
            box_dummy_pose.pose.position.x = 0.147
            box_dummy_pose.pose.position.y = 0.636
            box_dummy_pose.pose.position.z = 0
            psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC  
#             rospy.logwarn("I have added the box")
#             rospy.sleep(1)
#             rospy.logwarn("waiting for input to try planning with dummy box")
#             sss.wait_for_input()
#             
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
#             else:
#                 rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#                 
            rospy.logwarn("waiting for input to add end effector box box")
            sss.wait_for_input()
            # end effector collision object
            link_attached_to_ef = "ee_link"
            mb_ef_collisonobj = "metal_bracket"
            mb_ef_pose = PoseStamped()
            mb_ef_pose.header.frame_id =  link_attached_to_ef
            mb_ef_pose.pose.position.x = 0.065/2.0
            mb_ef_pose.pose.position.y = 0.0
            mb_ef_pose.pose.position.z = 0.0
            mb_ef_size = (0.065,0.06,0.06)



            psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
            
            
            raw_input("0 hi")
            
            mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
            

            
            
            rospy.logwarn("I have added the attached box to the end effector")
            
            
            rospy.sleep(1)
            raw_input("1 hi")           
            
            rospy.logwarn(rc.get_current_state())
            # mgc.attach_object(object_name, link_name, touch_links)
            mgc.set_start_state_to_current_state()
            rospy.logwarn(rc.get_current_state())
            raw_input("2 hi")
            rospy.logwarn("waiting for input to try planning with end effector box")
#.........这里部分代码省略.........