Обзор

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

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

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

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

edit exampleHelperFlowChartPickPlace.sfx

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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., и др. «Планирование движения с последовательной оптимизацией выпуклостей и проверкой коллизий выпуклостей». The International Journal of Robotics Research 33.9 (2014): 1251 - 1270.