Обзор

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

Этот пример использует алгоритм RRT для планирования пути. Другой пример, который посвящен более подробной информации о планировщике RRT, см. в разделе Выбор и размещение использования RRT для манипуляторов.

Диаграмма Stateflow

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

edit exampleHelperFlowChartPickPlaceRRT.sfx

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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