Обзор

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

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

Моделирование роботов и управление ими в беседке

Запустите имитатор на базе ROS для робота KINOVA Gen3 и настройте соединение MATLAB ® с имитатором робота.

В этом примере используется виртуальная машина, доступная для загрузки. См. пример «Начало работы с беседкой и моделируемой системой 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, щелкнув значок, ВМ загружает руку KINOVA Gen3 Robot на стол с одним мусорным баком с каждой стороны. Для моделирования и управления рукой робота в Беседке ВМ содержит пакет ros_kortex ROS, которые предоставляет компания KINOVA.

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

Задачи выбора и размещения

Поток операций комплектования и размещения реализован в MATLAB и состоит из основных шагов инициализации, за которыми следуют два основных раздела:

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

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

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

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

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

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

Команда для активации захвата, 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 используется для назначения временных шагов каждой из интерполированных ППМ, следующих за трапециевидным профилем. Наконец, ППМ с их соответствующими временами интерполируются до желаемой частоты дискретизации (каждые 0,1 секунды). Созданные траектории обеспечивают медленное перемещение робота в начале и в конце движения при приближении или размещении объекта.

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

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

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

После формирования совместной траектории для робота, чтобы следовать, exampleCommandMoveToTaskConfig функция производит выборку траектории с требуемой частотой дискретизации, упаковывает ее в сообщения АФК совместной траектории и посылает запрос на действие контроллеру совместной траектории, реализованному в пакете KINOVA ROS.

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

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

Запуск рабочего процесса выбора и размещения

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

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

Инициализация приложения выбора и размещения

Задайте начальную конфигурацию робота и имя тела конечного эффектора.

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]);

Пошаговое выполнение приложения «Выбор и размещение»

% 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/

Визуализация операции выбора и размещения в беседке

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

Авторское право 2021 The MathWorks, Inc.