demo_vision_poseest_pickplace.py 文件源码

def pick(self, pose, direction=(0, 0, 1), distance=0.1, controller=None):
        """Go to pose + pick_direction * pick_distance, open, go to pose,
        close, go to pose + pick_direction * pick_distance.

        """
        rospy.logdebug("pick pose: %s" % pose)
        pregrasp_pose = self.translate(pose, direction, distance)
        rospy.logdebug("pregrasp_pose: %s" % pregrasp_pose)
        rospy.sleep(1)
        self.move_ik(pregrasp_pose)
        # We want to block end effector opening so that the next
        # movement happens with the gripper fully opened.
        self.gripper.open(block=True)
        self.move_ik(pose)
        if controller is not None:
            print ('controller ON!!')
            controller.enable()
            rospy.sleep(5)
            controller.disable()
        self.gripper.close(block=True)
        self.move_ik(pregrasp_pose)