Обзор

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

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

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

Запустите симулятор на основе ROS для робота- Gen3 KINOVA и сконфигурируйте соединение MATLAB ® с симулятором робота.

В этом примере используется виртуальная машина (VM), доступная для загрузки. См. пример «Запуск с Gazebo и моделируемым TurtleBot (ROS Toolbox)» (ROS Toolbox).

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

rosshutdown;

rosinit(rosIP,11311); % Initialize ROS connection

Initializing global node /matlab_global_node_23379 with NodeURI http://172.21.72.67:58523/

После инициализации мира Gazebo путем нажатия кнопки значка VM загружает Gen3 Робота KINOVA на таблицу с одного интервала переработки с каждой стороны. Для симуляции и управления рукой робота в Gazebo ВМ содержит ros_kortex пакет ROS, которые предоставляет KINOVA.

Пакеты используют ros_control для управления соединениями в требуемых положениях соединений. Дополнительные сведения об использовании виртуальной машины см. в разделе Запуске с Gazebo и моделируемым TurtleBot (ROS Toolbox)

Выбор и размещение задач

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

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

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

Перед запуском задания выбора и размещения робот проходит через набор задач, чтобы идентифицировать сцену планирования в 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);

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

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

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

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

Контроллер траектории соединений в ROS

После генерации траектории соединения для следования робота, exampleC ommandMoveToTaskConfig функция производит выборку траектории с желаемой частотой дискретизации, упаковывает ее в сообщения ROS совместной траектории и отправляет запрос действия в контроллер траектории соединения, реализованный в пакете ROS KINOVA.

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

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

Запустите рабочий процесс «Выбор и размещение»

В этой симуляции используется манипулятор Gen3 KINOVA с прикрепленным захватом.

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

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

Установите начальное строение робота и имя корпуса end-effector.

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

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

coordinator = exampleHelperCoordinatorPickPlaceROSGazeboScene(robot,initialRobotJConfig, endEffectorFrame);

Задайте свойства координатора выбора и размещения.

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-and-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_23379 with NodeURI http://172.21.72.67:58523/

Визуализация действия выбора и размещения в Gazebo

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

Копирайт 2021 The MathWorks, Inc.