Обзор

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

В этом примере используется алгоритм RRT для планирования тракта. Для получения дополнительной информации о планировщике RRT см. раздел Комплектование и размещение с использованием RRT для манипуляторов.

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

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

edit exampleHelperFlowChartPickPlaceRRT.sfx

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

Инициализация робота и среды

Во-первых, диаграмма создает среду, состоящую из манипулятора Kinova Gen3, трех частей, подлежащих сортировке, полок, используемых для сортировки, и синего препятствия. Затем робот перемещается в исходное положение.

Определение деталей и места их размещения

На первом этапе фазы идентификации детали должны быть обнаружены. exampleCommandDetectParts функция непосредственно дает позициям объекта. Замените этот класс собственным алгоритмом обнаружения объектов на основе датчиков или объектов.

Затем детали должны быть классифицированы. exampleCommandClassifyParts функция классифицирует детали по двум типам для определения места их размещения (верхняя или нижняя полка). Опять же, эту функцию можно заменить любым методом классификации деталей.

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

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

Выбрать объект

Этап захвата перемещает робота к объекту, захватывает его и перемещается в безопасное положение, как показано на следующей диаграмме:

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

Этот робот захватывает объекты с помощью имитационного пневматического захвата. Когда захват активирован, exampleCommandActivateGripper добавляет сетку столкновения для детали в rigidBodyTree представление робота с помощью addCollision объектная функция. Обнаружение столкновений включает этот объект во время его присоединения. Затем робот перемещается в убранное положение в сторону от других частей.

Размещение объекта

Затем робот помещает объект на соответствующую полку.

Как и в случае рабочего процесса комплектования, подход к размещению и отведенные позиции вычисляются относительно известной желаемой позиции размещения. Захват деактивируется с помощью команды exampleCommandActivateGripper, которая удаляет деталь из робота, используя clearCollision. Каждый раз при размещении детали определенного типа поза размещения для этого типа объекта обновляется таким образом, что рядом с размещенной деталью размещается следующая деталь того же типа.

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

Большая часть выполнения задачи состоит в инструктировании робота перемещаться между различными заданными позами. exampleHelperMoveToTaskConfig функция определяет плановика RRT с помощью manipulatorRRT , который планирует пути от начальной до требуемой конфигурации соединения, избегая столкновений с указанными объектами столкновения в сцене. Результирующий путь сначала укорачивается, а затем интерполируется на требуемом расстоянии проверки. Чтобы создать траекторию, trapveltraj используется для назначения временных шагов каждой из интерполированных ППМ, следующих за трапециевидным профилем. Наконец, ППМ с их соответствующими временами интерполируются до желаемой частоты дискретизации (каждые 0,1 секунды). Созданные траектории обеспечивают медленное перемещение робота в начале и в конце движения при приближении или размещении объекта.

Для получения дополнительной информации о планировщике RRT см. раздел Комплектование и размещение с использованием RRT для манипуляторов.

Для более простых траекторий, где пути, как известно, свободны от препятствий, траектории могут быть выполнены с использованием инструментов формирования траекторий и смоделированы с использованием моделей движения манипулятора. См. пример «Планирование и выполнение траекторий задач и совместного пространства с использованием манипулятора KINOVA Gen3».

Планирование задач в диаграмме потока состояний

В этом примере диаграмма Stateflow используется для управления рабочим процессом в MATLAB ®. Дополнительные сведения о создании запусков потока состояний см. в разделе Создание диаграмм Stateflow для выполнения как объектов MATLAB (Stateflow).

Диаграмма Stateflow управляет выполнением задач в MATLAB с помощью функций команд. Когда команда завершает выполнение, она посылает входное событие для пробуждения диаграммы и перехода к следующему шагу выполнения задачи, см. раздел Выполнение автономной диаграммы (Stateflow).

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

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

load('exampleHelperKINOVAGen3GripperCollRRT.mat'); 

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

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

currentRobotJConfig = homeConfiguration(robot);
coordinator = exampleHelperCoordinatorPickPlaceRRT(robot,currentRobotJConfig, "gripper");

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

coordinator.HomeRobotTaskConfig = trvec2tform([0.4, 0, 0.5])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{1} = trvec2tform([[-0.15 0.52 0.46]])*axang2tform([1 0 0 -pi/2]);
coordinator.PlacingPose{2} = trvec2tform([[-0.15 0.52 0.63]])*axang2tform([1 0 0 -pi/2]);

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

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

coordinator.FlowChart = exampleHelperFlowChartPickPlaceRRT('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'
        % End Pick and Place
        coordinator.FlowChart.endPickPlace;        
        delete(coordinator.FlowChart);
        delete(coordinator);
end

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

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

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

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

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

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

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