Обзор

Этот пример сортирует, обнаружил объекты и размещает их в места размещения с помощью манипулятора КИНОВОЙ Gen3. Пример использует инструменты от четырех тулбоксов:

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

Диаграмма Stateflow

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

edit exampleHelperFlowChartPickPlace.sfx

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

Инициализируйте робота и среду

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

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

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

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

Выполните рабочий процесс Pick-Place

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

Возьмите Объект

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

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

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

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

Робот затем кладет объект на соответствующую полку.

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

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

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

Этот рабочий процесс исследован подробно в Плане, и Выполните использование Траекторий Без коллизий Манипулятор КИНОВОЙ Gen3. Контроллер используется, чтобы гарантировать движение без коллизий. Для более простых траекторий, где пути, как известно, без препятствий, траектории могли быть выполнены с помощью инструментов генерации траектории и симулировали использование моделей движения манипулятора. См. План и Выполните Задачу - и использование Объединенных пространственных траекторий Манипулятор КИНОВОЙ Gen3.

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

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

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

Запустите и визуализируйте симуляцию

Эта симуляция использует манипулятор КИНОВОЙ 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. После того, как запущенный, диаграмма Stateflow ответственна за то, что постоянно прошла состояния обнаружения объектов, собирания их и размещения их в правильном районе сосредоточения войск.

coordinator.FlowChart = exampleHelperFlowChartPickPlace('coordinator', coordinator); 

Используйте диалоговое окно, чтобы запустить pick-place выполнение задачи. Нажмите кнопку Да в диалоговом окне, чтобы начать симуляцию.

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

Окончание задачи Pick-Place

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

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

Наблюдайте состояния симуляции

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

Визуализируйте действие Pick-Place

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

Ссылки

[1] Шульман, J., и др. "Движение, планирующее с последовательной выпуклой оптимизацией и выпуклой проверкой столкновения". Международный журнал Исследования Робототехники 33.9 (2014): 1251-1270.