Обзор

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

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

Симуляция робота и управление в Gazebo

Запустите ОСНОВАННОЕ НА ROS средство моделирования для робота КИНОВОЙ Gen3 и сконфигурируйте связь MATLAB® со средством моделирования робота.

Этот пример использует виртуальную машину (VM) доступную для скачивания. Смотрите Начало работы с Gazebo и Симулированным TurtleBot (ROS Toolbox) (ROS Toolbox) пример.

rosIP = '192.168.203.132';   % IP address of ROS-enabled machine  

rosshutdown;

rosinit(rosIP,11311); % Initialize ROS connection

Initializing global node /matlab_global_node_52545 with NodeURI http://192.168.203.1:53426/

После инициализации мира Gazebo нажатием кнопки значок VM загружает Манипулятор КИНОВОЙ Gen3 на таблице с одной корзиной на каждой стороне. Чтобы симулировать и управлять манипулятором в Gazebo, VM содержит ros_kortex пакет ROS, которые обеспечиваются КИНОВОЙ.

Пакеты используют ros_control, чтобы управлять соединениями к желаемым объединенным позициям. Для дополнительных деталей об использовании VM обратитесь к Начало работы с Gazebo и Симулированным TurtleBot (ROS Toolbox)

Задачи Pick-Place

Рабочий процесс Pick-Place реализован в MATLAB и состоит из основных шагов инициализации, выполненных двумя основными разделами:

Для реализации, которая использует Stateflow, чтобы запланировать задачи, см., что Рабочий процесс Pick-Place использует Stateflow для MATLAB.

Сканирование среды, чтобы создать сцену планирования для планировщика пути RRT

Прежде, чем запустить pick-place задание, робот проходит набор задач идентифицировать сцену планирования в exampleCommandBuildWorld функционируйте и обнаруживает объекты выбрать использование exampleCommandDetectParts функция.

Во-первых, робот перемещается в предопределенные положения сканирования один за другим и получает набор облаков точек сцены с помощью встроенного датчика глубины. В каждом из положений сканирования текущее положение камеры получено путем чтения соответствующего преобразования ROS с помощью rostf (ROS Toolbox) и getTransform (ROS Toolbox). Положения сканирования визуализируются ниже:

Если робот посетил все положения сканирования, полученные облака точек преобразовываются от камеры до мировой системы координат с помощью pctransform (Computer Vision Toolbox) и объединенный к одному облаку точек с помощью pcmerge (Computer Vision Toolbox). Облако конечной точки сегментируется на основе Евклидова расстояния с помощью pcsegdist (Computer Vision Toolbox). Получившиеся сегменты облака точек затем закодированы, когда столкновение сцепляется (см. collisionMesh) быть легко идентифицированным как препятствия во время планирования пути RRT. Процессу от облака точек до сеток столкновения показывают одну mesh в за один раз ниже.

Открытие и закрытие механизма захвата

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

[gripAct,gripGoal] = rosactionclient('/my_gen3/custom_gripper_controller/gripper_cmd');
gripperCommand = rosmessage('control_msgs/GripperCommand');
gripperCommand.Position = 0.0;  
gripGoal.Command = gripperCommand;
sendGoal(gripAct,gripGoal);

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

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

Запланированные пути визуализируются в MATLAB наряду со сценой планирования.

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

Объединенный контроллер траектории в ROS

После генерации объединенной траектории для робота, чтобы следовать, exampleCommandMoveToTaskConfig функциональные выборки траектория на уровне желаемой частоты дискретизации, пакеты это в объединенную траекторию ROS передает и отправляет запрос действия контроллеру объединенной траектории, реализованному в пакете КИНОВОЙ ROS.

Обнаружение и классификация объектов в сцене

Функции exampleCommandDetectParts и exampleCommandClassifyParts используйте симулированный канал камеры глубины исполнительного элемента конца от робота, чтобы обнаружить годные для повторного использования части. Поскольку полное облако точек сцены доступно от шага Среды сборки, регистрационный алгоритм итеративной самой близкой точки (ICP), реализованный в pcregistericp (Computer Vision Toolbox) идентифицирует, какое из сегментированных облаков точек совпадает с конфигурациями объектов, которые должны быть выбраны.

Запустите рабочий процесс Pick-Place

Эта симуляция использует манипулятор КИНОВОЙ Gen3 с присоединенным механизмом захвата.

load('exampleHelperKINOVAGen3GripperGazeboRRTScene.mat'); 
rng(0)

Инициализируйте приложение Pick-Place

Установите начальную настройку робота и имя корпуса исполнительного элемента конца.

initialRobotJConfig =  [3.5797   -0.6562   -1.2507   -0.7008    0.7303   -2.0500   -1.9053];
endEffectorFrame = "gripper";

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

coordinator = exampleHelperCoordinatorPickPlaceROSGazeboScene(robot,initialRobotJConfig, endEffectorFrame);

Задайте pick-place свойства координатора.

coordinator.HomeRobotTaskConfig = getTransform(robot, initialRobotJConfig, endEffectorFrame);
coordinator.PlacingPose{1} = trvec2tform([[0.2 0.55 0.26]])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{2} = trvec2tform([[0.2 -0.55 0.26]])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);

Запустите приложение Pick-Place шаг за шагом

% Task 1: Build world
exampleCommandBuildWorldROSGazeboScene(coordinator);

Moving to scanning pose 1
Now planning...

Waiting until robot reaches the desired configuration

Capturing point cloud 1
Getting camera pose 1
Moving to scanning pose 2
Now planning...

Waiting until robot reaches the desired configuration

Capturing point cloud 2
Getting camera pose 2
Moving to scanning pose 3
Now planning...

Waiting until robot reaches the desired configuration

Capturing point cloud 3
Getting camera pose 3
Moving to scanning pose 4
Now planning...

Waiting until robot reaches the desired configuration

Capturing point cloud 4
Getting camera pose 4
Moving to scanning pose 5
Now planning...

Waiting until robot reaches the desired configuration

Capturing point cloud 5
Getting camera pose 5

% Task 2: Move to home position
exampleCommandMoveToTaskConfigROSGazeboScene(coordinator,coordinator.HomeRobotTaskConfig);

Now planning...

Waiting until robot reaches the desired configuration

% Task 3: Detect objects in the scene to pick
exampleCommandDetectPartsROSGazeboScene(coordinator);

Bottle detected...
Can detected...

% Task 4: Select next part to pick
remainingParts = exampleCommandPickingLogicROSGazeboScene(coordinator);

     1

while remainingParts==true
    % Task 5: [PICKING] Compute grasp pose
    exampleCommandComputeGraspPoseROSGazeboScene(coordinator);
    
    % Task 6: [PICKING] Move to picking pose
    exampleCommandMoveToTaskConfigROSGazeboScene(coordinator, coordinator.GraspPose);
    
    % Task 7: [PICKING] Activate gripper
    exampleCommandActivateGripperROSGazeboScene(coordinator,'on');
    
    % Part has been picked
    
    % Task 8: [PLACING] Move to placing pose
    exampleCommandMoveToTaskConfigROSGazeboScene(coordinator, ...
    coordinator.PlacingPose{coordinator.DetectedParts{coordinator.NextPart}.placingBelt});
    
    % Task 9: [PLACING] Deactivate gripper
    exampleCommandActivateGripperROSGazeboScene(coordinator,'off');
    
    % Part has been placed
    
    % Select next part to pick
    remainingParts = exampleCommandPickingLogicROSGazeboScene(coordinator);    
end

Now planning...

Waiting until robot reaches the desired configuration

Gripper closed...

Now planning...

Waiting until robot reaches the desired configuration

Gripper open...

     2

Now planning...

Waiting until robot reaches the desired configuration

Gripper closed...

Now planning...

Waiting until robot reaches the desired configuration

Gripper open...

% Shut down ros when the pick-and-place application is done
rosshutdown;

Shutting down global node /matlab_global_node_52545 with NodeURI http://192.168.203.1:53426/

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

Мир Gazebo показывает робота в рабочей области, когда это перемещает части в корзины. Робот продолжает работать, пока все части не были помещены.

Copyright 2020 The MathWorks, Inc.