Обзор

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

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

Диаграмма Stateflow

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

edit exampleHelperFlowChartPickPlace.sfx

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

Инициализация робота и окружения

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

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

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

Далее позиции должны быть классифицированы. ПримерКомандная ClassifyParts Функция классифицирует детали в два типа, чтобы определить, где их разместить (верхняя или нижняя полка). Снова можно заменить эту функцию любым методом классификации деталей.

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

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

Выберите объект

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

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

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

Поместите объект

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

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

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

Большая часть выполнения задачи состоит из инструкции роботу перемещаться между различными заданными положениями. The exampleHelperPlanExecuteTrajectoryPickPlace функция задает решатель с помощью нелинейного прогнозирующего контроллера модели (см. Nonlinear MPC (Model Predictive Control Toolbox)), который вычисляет допустимую, без столкновения оптимизированную эталонную траекторию с помощью nlmpcmove (Model Predictive Control Toolbox) и checkCollision. Препятствия представлены как сферы, чтобы гарантировать точное приближение ограничения якобиана в определении нелинейной модели прогнозирующего алгоритма управления (см. [1]). Функция helper затем моделирует движение манипулятора под управлением вычисляемым крутящим моментом, когда она отслеживает эталонную траекторию, используя jointSpaceMotionModel объект и обновляет визуализацию. Функция helper вызывается из диаграммы Stateflow через exampleCommandMoveToTaskConfig, который определяет правильные входы.

Этот рабочий процесс подробно рассматривается в разделе «Планирование и выполнение траекторий без столкновений» с использованием KINOVA Gen3 Manipulator. Контроллер используется для обеспечения свободного от столкновения движения. Для более простых траекторий, где пути, как известно, не имеют препятствий, траектории могут быть выполнены с помощью инструментов генерации траекторий и моделированы с помощью моделей движения манипулятора. Смотрите План и Выполнение Траекторий Пространства Задач и Соединений Используя KINOVA Gen3 Manipulator.

Планирование задач на диаграмме Stateflow

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

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

Запуск и визуализация симуляции

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

load('exampleHelperKINOVAGen3GripperColl.mat'); 

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

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

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

Задайте домашнее строение и два положения для размещения объектов разных типов.

coordinator.HomeRobotTaskConfig = trvec2tform([0.4, 0, 0.6])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{1} = trvec2tform([0.23 0.62 0.33])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{2} = trvec2tform([0.23 -0.62 0.33])*axang2tform([0 1 0 pi]);

Запуск и визуализация симуляции

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

coordinator.FlowChart = exampleHelperFlowChartPickPlace('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 для визуализации робота. Визуализация показывает робота в рабочей области, когда он перемещает части вокруг. Робот избегает препятствий в окружении (синий цилиндр) и помещает объекты на верхнюю или нижнюю полку на основе их классификации. Робот продолжает работать до тех пор, пока не будут размещены все детали.

Ссылки

[1] Schulman, J., et al. Планирование движения с последовательной выпуклой оптимизацией и проверкой выпуклого столкновения. Международный журнал исследований робототехники 33.9 (2014): 1251-1270.