Flujo de trabajo de pick-and-place en Gazebo usando procesamiento de nubes de puntos y planificación de rutas RRT

Este ejemplo utiliza:

Configure un flujo de trabajo de pick-and-place de extremo a extremo para un manipulador robótico como Kinova® Gen3.

El flujo de trabajo de pick-and-place implementado en este ejemplo se puede adaptar a distintos escenarios, planificadores, plataformas de simulación y opciones de detección de objetos. El ejemplo que se muestra aquí utiliza RRT para planificar y simula el robot en Gazebo utilizando el sistema operativo de robots (ROS). Para otros flujos de trabajo de pick-and-place, consulte:

Visión general

Este ejemplo identifica y recicla objetos en dos contenedores usando un manipulador Kinova Gen3. El ejemplo usa herramientas de cinco toolboxes:

Robotics System Toolbox™ se usa para modelar y simular el manipulador.
ROS Toolbox se usa para conectar MATLAB® a Gazebo.
Image Processing Toolbox™ y Computer Vision Toolbox™ se utilizan para detectar objetos mediante el procesamiento de nubes de puntos y la cámara de profundidad simulada en Gazebo.

Este ejemplo se basa en conceptos clave de los siguientes ejemplos relacionados:

Pick and Place Using RRT for Manipulators
Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox) (ROS Toolbox)
3-D Point Cloud Registration and Stitching (Computer Vision Toolbox) (Computer Vision Toolbox)

Simulación y control de robots en Gazebo

Inicie un simulador basado en ROS para un robot KINOVA Gen3 y configure la conexión MATLAB® con el simulador de robot.

Este ejemplo usa un equipo virtual (VM) que contiene ROS Melodic disponible para descargar aquí.

Inicie el escritorio del equipo virtual Ubuntu®.
En el escritorio de Ubuntu, haga clic en el icono Gazebo Recycling World - Depth Sensing para iniciar el mundo creado en Gazebo creado para este ejemplo. Haga clic en el botón Play para ejecutar el mundo creado en Gazebo.
Especifique la dirección IP y el número de puerto del ROS master en Gazebo para que MATLAB® pueda comunicarse con el simulador de robot. Para este ejemplo, el ROS master en Gazebo usa la dirección IP de 172.21.72.160 que se muestra en el escritorio. Ajuste la variable rosIP según su VM.
Inicie la red ROS 1 usando rosinit.

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

rosshutdown;

rosinit(rosIP,11311); % Initialize ROS connection

Initializing global node /matlab_global_node_36341 with NodeURI http://192.168.17.1:59807/ and MasterURI http://192.168.17.130:11311.

Una vez que el mundo creado en Gazebo comienza a ejecutarse, el VM carga un brazo robótico KINOVA Gen3 sobre una mesa con un contenedor de reciclaje a cada lado. Para simular y controlar el brazo robótico en Gazebo, el VM contiene el paquete ROS ros_kortex, que proporciona KINOVA.

Los paquetes usan ros_control para controlar las articulaciones en las posiciones de articulación deseadas. Para más información sobre cómo usar el VM, consulte Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox)

Tareas de pick-and-place

El flujo de trabajo de pick-and-place se implementa en MATLAB y consta de pasos básicos de inicialización, seguidos de dos secciones principales:

Identificar las piezas y determinar dónde colocarlas
Ejecutar el flujo de trabajo de pick-and-place

Para una implementación que utiliza Stateflow para planificar las tareas, consulte Flujo de trabajo de pick-and-place con Stateflow para MATLAB.

Escaneo del entorno para crear la escena de planificación para el planificador de rutas RRT

Antes de iniciar el trabajo de pick-and-place, el robot realiza un conjunto de tareas para identificar la escena de planificación en la función exampleCommandBuildWorld y detecta los objetos que debe recoger utilizando la función exampleCommandDetectParts.

En primer lugar, el robot se desplaza por las poses de escaneo predefinidas, una a la vez, y captura un conjunto de nubes de puntos de la escena utilizando un sensor de profundidad incorporado. En cada una de las poses de escaneo, se recupera la pose actual de la cámara leyendo la transformación ROS correspondiente mediante rostf (ROS Toolbox) y getTransform (ROS Toolbox). Las poses de escaneo se visualizan a continuación:

Una vez que el robot ha visitado todas las poses de escaneo, las nubes de puntos capturadas se transforman de la cámara al marco del mundo real utilizando pctransform (Computer Vision Toolbox) y se fusionan en una única nube de puntos mediante pcmerge (Computer Vision Toolbox). La nube de puntos final se segmenta en función de la distancia euclidiana utilizando pcsegdist (Computer Vision Toolbox). Después, los segmentos de nubes de puntos resultantes se codifican como mallas de colisión (consulte collisionMesh) para poder identificarse fácilmente como obstáculos durante la planificación de rutas RRT. El proceso desde la nube de puntos hasta las mallas de colisión se muestra a continuación malla a malla.

Apertura y cierre de la pinza

El comando para activar la pinza, exampleCommandActivateGripper, envía una solicitud de acción para abrir y cerrar la pinza implementada en Gazebo. Por ejemplo, para enviar una solicitud para abrir la pinza, se usa el siguiente código.

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

Mover el manipulador a una pose específica

La mayoría de la ejecución de la tarea consiste en instruir al robot para que se desplace entre las diferentes poses especificadas. La función exampleHelperMoveToTaskConfig define un planificador RRT con el objeto manipulatorRRT, que planifica rutas desde una configuración de articulación inicial hasta una deseada evadiendo colisiones con objetos de colisión especificados en la escena. La ruta resultante se acorta en primer lugar y, después, se interpola a una distancia de validación deseada. Para generar una trayectoria, la función trapveltraj se utiliza para asignar unidades de tiempo a cada uno de los waypoints interpolados según un perfil trapezoidal. Por último, los waypoints con sus tiempos asociados se interpolan a una tasa de muestreo deseada (cada 0,1 segundos). Las trayectorias generadas garantizan que el robot se traslade lentamente al inicio y al final del movimiento cuando se está acercando a un objeto o colocándolo.

Las rutas planificadas se visualizan en MATLAB junto con la escena de planificación.

Este flujo de trabajo se examina en detalle en el ejemplo Flujo de trabajo pick-and-place con el planificador RRT y Stateflow para MATLAB. Para obtener más información sobre el planificador RRT, consulte Pick and Place Using RRT for Manipulators. Para las trayectorias más simples en las que se sabe que las rutas están libres de obstáculos, las trayectorias pueden ejecutarse mediante herramientas de generación de trayectorias y simularse con modelos de movimiento del manipulador. Consulte Planificar y ejecutar trayectorias de espacio cartesiano o articular utilizando el manipulador Kinova Gen3.

Controlador de trayectoria articular en ROS

Después de generar una trayectoria articular para que la siga el robot, la función exampleCommandMoveToTaskConfig muestrea la trayectoria a la tasa de muestreo deseada, la empaqueta en mensajes ROS de trayectoria articular y envía una solicitud de acción al controlador de trayectoria articular implementado en el paquete ROS de KINOVA.

Detección y clasificación de objetos en la escena

Las funciones exampleCommandDetectParts y exampleCommandClassifyParts usan la transmisión de la cámara de profundidad del efector final simulado del robot para detectar las piezas reciclables. Dado que se dispone de una nube de puntos completa de la escena desde el paso Build Environment (crear entorno), el algoritmo de registro del punto iterativo más cercano (ICP) implementado en pcregistericp (Computer Vision Toolbox) identifica cuáles de las nubes de puntos segmentadas coinciden con las geometrías de los objetos que deben recogerse.

Iniciar el flujo de trabajo de pick-and-place

Esta simulación usa un manipulador KINOVA Gen3 con una pinza acoplada.

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

Inicializar la aplicación de pick-and-place

Establezca la configuración inicial del robot y el nombre del cuerpo del efector final.

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

Inicialice el coordinador proporcionando el modelo de robot, la configuración inicial y el nombre del efector final.

coordinator = exampleHelperCoordinatorPickPlaceROSGazeboScene(robot,initialRobotJConfig, endEffectorFrame);

Especifique las propiedades del coordinador de pick-and-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]);

Ejecutar la aplicación de pick-and-place paso a paso

% Task 1: Build world
exampleCommandBuildWorldROSGazeboScene(coordinator);

Moving to scanning pose 1
Searching for other config...
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
Searching for other config...
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);

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

    % Move to home position
    exampleCommandMoveToTaskConfigROSGazeboScene(coordinator,coordinator.HomeRobotTaskConfig);
end

Now planning...
Waiting until robot reaches the desired configuration

Gripper closed...

Now planning...
Waiting until robot reaches the desired configuration

Gripper open...

Now planning...
Waiting until robot reaches the desired configuration

Now planning...
Waiting until robot reaches the desired configuration

Gripper closed...

Now planning...
Waiting until robot reaches the desired configuration

Gripper open...

Now planning...
Waiting until robot reaches the desired configuration

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

Shutting down global node /matlab_global_node_36341 with NodeURI http://192.168.17.1:59807/ and MasterURI http://192.168.17.130:11311.

Visualizar la acción de pick-and-place en Gazebo

El mundo creado en Gazebo muestra al robot en el área de trabajo mientras mueve piezas a los contenedores de reciclaje. El robot continúa trabajando hasta que se han colocado todas las piezas.