Обзор

В этом примере объекты идентифицируются и перерабатываются в две ячейки с помощью манипулятора KINOVA Gen3. В примере используются инструменты из пяти панелей инструментов:

Этот пример основан на ключевых концепциях из следующих связанных примеров:

Моделирование роботов и управление ими в беседке

Запустите имитатор на базе ROS для робота KINOVA Gen3 и настройте соединение MATLAB ® с имитатором робота.

В этом примере используется виртуальная машина, доступная для загрузки. Если вы никогда ранее не использовали его, см. раздел Начало работы с беседкой и моделируемым TurtleBot (ROS Toolbox).

rosIP = '192.168.203.131';   % IP address of ROS-enabled machine  

rosinit(rosIP,11311); % Initialize ROS connection

The value of the ROS_IP environment variable, 192.168.31.1, will be used to set the advertised address for the ROS node.
Initializing global node /matlab_global_node_36570 with NodeURI http://192.168.31.1:51073/

После инициализации мира Gazebo, щелкнув значок, ВМ загружает руку KINOVA Gen3 Robot на стол с одним мусорным баком с каждой стороны. Для моделирования и управления рукой робота в Беседке ВМ содержит пакет ros_kortex ROS, которые предоставляет компания KINOVA.

Пакеты используют ros_control для управления соединениями в требуемых положениях соединения. Дополнительные сведения об использовании виртуальной машины см. в разделе Начало работы с беседкой и моделируемым TurtleBot (ROS Toolbox)

Диаграмма статофлоу

В этом примере для планирования задач в примере используется диаграмма Stateflow. Откройте диаграмму для изучения содержимого и отслеживания переходов состояний во время выполнения диаграммы.

edit exampleHelperFlowChartPickPlaceROSGazebo.sfx

Диаграмма определяет способ взаимодействия манипулятора с объектами или деталями. Он состоит из основных этапов инициализации, за которыми следуют два основных раздела:

Подробное описание шагов выбора и размещения см. в разделе Рабочий процесс выбора и размещения с использованием потока состояний для MATLAB.

Открытие и закрытие захвата

Команда для активации захвата, exampleCommandActivateGripperROSGazebo, отправляет запрос на действие, чтобы открыть и закрыть захват, реализованный в Gazebo. Для отправки запроса на открытие захвата используется следующий код. Примечание.Приведенный пример кода иллюстрирует действия команды. Не запускать.

[gripAct,gripGoal] = rosactionclient('/my_gen3/custom_gripper_controller/gripper_cmd');
gripperCommand = rosmessage('control_msgs/GripperCommand');
gripperCommand.Position = 0.0;  
gripGoal.Command = gripperCommand;
sendGoal(gripAct,gripGoal);

Перемещение манипулятора в определенную позу

commandMoveToTaskConfig используется для перемещения манипулятора в указанные позы.

Планирование

Планирование путей создает простые траектории пространства задач от начальной до требуемой конфигурации задач с использованием trapveltraj и transformtraj. Дополнительные сведения о планировании и выполнении траекторий см. в разделе Планирование и выполнение траекторий задач и совместного пространства с помощью манипулятора KINOVA Gen3.

Объединенный контроллер траектории в АФК

После формирования траектории соединения для робота, чтобы следовать, commandMoveToTaskConfig выполняет выборку траектории с требуемой частотой дискретизации, упаковывает ее в сообщения АФК совместной траектории и посылает запрос на действие контроллеру совместной траектории, реализованному в пакете KINOVA ROS.

Обнаружение и классификация объектов в сцене

Функции commandDetectParts и commandClassifyParts использовать моделируемую конечную эффекторную камеру, подаваемую от робота, и применять заранее обученную модель глубокого обучения для обнаружения рециркулируемых частей. Модель принимает кадр камеры в качестве входного и выводит 2D местоположение объекта (положение пикселя) и требуемый тип повторного использования (синий или зеленый бункер). Местоположение 2D на кадре изображения сопоставляется с базовым кадром робота.

Обучение модели глубокого обучения: приобретение и маркировка изображений беседки

Модель обнаружения была обучена с использованием набора изображений, полученных в моделируемой среде в мире Gazebo, с двумя классами объектов (бутылка, банка), размещенных на разных местах стола. Изображения получают из имитированной камеры на борту робота, который перемещается вдоль горизонтальной и вертикальной плоскостей, чтобы получить изображения объектов из множества различных ракурсов камеры.

Затем изображения помечаются с помощью приложения Image Labeler (Computer Vision Toolbox), создавая обучающий набор данных для модели обнаружения YOLO v2. trainYOLOv2ObjectDetector (Computer Vision Toolbox) обучает модель. Сведения о том, как обучить сеть YOLO v2 в MATLAB, см. в разделе Сеть Train YOLO v2 для обнаружения транспортных средств (панель инструментов компьютерного зрения).

Обучаемая модель развертывается для оперативного вывода на одиночное изображение, получаемое бортовой камерой, когда робот находится в исходном положении. detect Функция (Computer Vision Toolbox) возвращает положение изображения ограничивающих рамок обнаруженных объектов вместе с их классами, которое затем используется для поиска положения центра верхней части объектов. Используя простой подход проекции камеры, предполагая, что высота объектов известна, положение объекта проецируется в мир и, наконец, используется в качестве опорного положения для выбора объекта. Класс, связанный с ограничивающей коробкой, определяет ячейку для размещения объекта.

Запуск задания выбора и размещения

В этом моделировании используется манипулятор KINOVA Gen3 с прикрепленным захватом Robotiq. Загрузка модели робота из .mat файл как rigidBodyTree объект.

load('exampleHelperKINOVAGen3GripperROSGazebo.mat'); 

Инициализация координатора комплектования и размещения

Задайте начальную конфигурацию робота. Создайте координатора, который обрабатывает управление роботом, указав модель робота, начальную конфигурацию и имя конечного эффектора.

initialRobotJConfig =  [3.5797   -0.6562   -1.2507   -0.7008    0.7303   -2.0500   -1.9053];
endEffectorFrame = "gripper";

Инициализируйте координатора, указав модель робота, начальную конфигурацию и имя конечного эффектора.

coordinator = exampleHelperCoordinatorPickPlaceROSGazebo(robot,initialRobotJConfig, endEffectorFrame);

Укажите домашнюю конфигурацию и две позы для размещения объектов.

coordinator.HomeRobotTaskConfig = getTransform(robot, initialRobotJConfig, endEffectorFrame); 
coordinator.PlacingPose{1} = trvec2tform([[0.2 0.55 0.26]])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{2} = trvec2tform([[0.2 -0.55 0.26]])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);

Запуск и визуализация моделирования

Подключите координатора к диаграмме статофлоу. После запуска диаграмма Stateflow отвечает за непрерывное прохождение состояний обнаружения объектов, их сбор и размещение в правильной промежуточной области.

coordinator.FlowChart = exampleHelperFlowChartPickPlaceROSGazebo('coordinator', coordinator); 

Используйте диалоговое окно для запуска выполнения задачи выбора и размещения. Нажмите кнопку «Да» в диалоговом окне, чтобы начать моделирование.

answer = questdlg('Do you want to start the pick-and-place job now?', ...
         'Start job','Yes','No', 'No');

switch answer
    case 'Yes'
        % Trigger event to start Pick and Place in the Stateflow Chart
        coordinator.FlowChart.startPickPlace;       
    case 'No'
        coordinator.FlowChart.endPickPlace;
        delete(coordinator.FlowChart)
        delete(coordinator);
end

Завершение задачи выбора и размещения

Диаграмма Stateflow завершит выполнение автоматически после 3 неудачных попыток обнаружения новых объектов. Чтобы преждевременно завершить задачу выбора и размещения, раскомментируйте и выполните следующие строки кода или нажмите Ctrl + C в окне команд .

% coordinator.FlowChart.endPickPlace;        
% delete(coordinator.FlowChart);
% delete(coordinator);

Наблюдение за состояниями моделирования

Во время выполнения активные состояния в каждый момент времени подсвечиваются синим цветом на диаграмме Stateflow. Это помогает отслеживать, что делает робот и когда. Можно щелкнуть по подсистемам, чтобы просмотреть подробные сведения о состоянии в действии.

Визуализация операции выбора и размещения в беседке

Мир Беседки показывает робота в рабочей области, когда он перемещает части в бункеры рециркуляции. Робот продолжает работу до тех пор, пока не будут размещены все детали. Если шаг обнаружения не находит больше частей четыре раза, диаграмма Stateflow завершает работу.

if strcmp(answer,'Yes')
    while  coordinator.NumDetectionRuns <  4
        % Wait for no parts to be detected.
    end
end

Завершите работу сети ROS после завершения примера.

rosshutdown

Shutting down global node /matlab_global_node_36570 with NodeURI http://192.168.31.1:51073/

Copyright 2020 The MathWorks, Inc.