Выбор и размещение рабочего процесса в Gazebo с использованием ROS

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

Обзор

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

  • Robotics System Toolbox™ используется для моделирования и симуляции манипулятора.

  • Stateflow ® используется для планирования задач высокого уровня в примере и шага от задачи к задаче.

  • ROS Toolbox™ используется для подключения MATLAB к Gazebo.

  • Computer Vision Toolbox™ и Deep Learning Toolbox™ используются для обнаружения объектов с помощью имитированной камеры в Gazebo.

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

Симуляция и управление роботом в Gazebo

Запустите симулятор на основе ROS для робота- Gen3 KINOVA и сконфигурируйте соединение MATLAB ® с симулятором робота.

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

  • Запустите рабочий стол виртуальной машины Ubuntu ®.

  • На рабочем столе Ubuntu щелкните значок Gazebo Recycling World, чтобы запустить мир Gazebo, созданный для этого примера.

  • Укажите IP-адрес и номер порта ведущего ROS в Gazebo, чтобы MATLAB ® мог общаться с симулятором робота. В данном примере мастер ROS в Gazebo использует IP-адрес 192.168.203.131 отображается на рабочем столе. Настройте rosIP переменная на основе виртуальной машины.

  • Запустите сеть ROS 1 с помощью rosinit.

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 путем нажатия кнопки значка VM загружает Gen3 Робота KINOVA на таблицу с одного интервала переработки с каждой стороны. Для симуляции и управления рукой робота в Gazebo ВМ содержит ros_kortex пакет ROS, которые предоставляет KINOVA.

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

Диаграмма Stateflow

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

edit exampleHelperFlowChartPickPlaceROSGazebo.sfx

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

  • Идентифицируйте детали и определите, где их разместить

  • Выполните рабочий процесс комплектования и размещения

Описание шагов выбора и размещения на высоком уровне см. в разделе Рабочий процесс выбора и размещения с использованием Stateflow для 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);

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

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

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

Планирование пути генерирует простые траектории пространства задач от начальной до желательного строения задачи с помощью trapveltraj и transformtraj. Для получения дополнительной информации о планировании и выполнении траекторий смотрите План и Выполнение Траекторий Task - и Joint-Space с Использованием KINOVA Gen3 Manipulator.

Контроллер траектории соединений в ROS

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

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

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

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

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

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

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

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

В этой симуляции используется манипулятор Gen3 KINOVA с прикрепленным захватом 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. После запуска диаграмма 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. Это помогает отслеживать, что и когда делает робот. Можно кликнуть подсистемы, чтобы увидеть детали состояния в действии.

Визуализация действия выбора и размещения в Gazebo

Мир Gazebo показывает робота в рабочей области, когда он перемещает части в интервалы переработки. Робот продолжает работать до тех пор, пока не будут размещены все детали. Когда шаг обнаружения не находит больше деталей четыре раза, диаграмма 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/

Копирайт 2020 The MathWorks, Inc.

Для просмотра документации необходимо авторизоваться на сайте