Обзор

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

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

Диаграмма Stateflow

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

edit exampleHelperFlowChartPickPlaceRRT.sfx

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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