Main Content

Flujo de trabajo de pick-and-place en Gazebo usando un ROS

Este ejemplo muestra cómo configurar un flujo de trabajo de pick-and-place de extremo a extremo para un manipulador robótico como KINOVA® Gen3 y simular el robot en el simulador de física Gazebo.

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.

  • Stateflow® se usa para programar las tareas de alto nivel en el ejemplo y pasar de una tarea a otra.

  • ROS Toolbox™ se usa para conectar MATLAB a Gazebo.

  • Computer Vision Toolbox™ y Deep Learning Toolbox™ se usan para la detección de objetos usando una cámara simulada en Gazebo.

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

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í. Si nunca lo ha usado antes, consulte Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox).

  • En la configuración de VM, VM > Settings > Hardware > Display (VM > Configuración> Hardware > Visualización), desactive Accelerate 3D graphics (Acelerar gráficos 3D).

  • Inicie el escritorio del equipo virtual Ubuntu®.

  • En el escritorio de Ubuntu, haga clic en el icono Gazebo Recycling World para iniciar el mundo Gazebo creado para este ejemplo.

  • Especifique la dirección IP y el número de puerto del ROS principal en Gazebo para que MATLAB® pueda comunicarse con el simulador de robot. Para este ejemplo, el ROS principal en Gazebo usa la dirección IP de 192.168.203.131 que se muestra en el escritorio. Ajuste la variable rosIP según su VM.

  • Inicie la red ROS 1 usando rosinit.

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

rosinit(rosIP,11311); % Initialize ROS connection
The value of the ROS_IP environment variable, 192.168.31.1, will be used to set the advertised address for the ROS node.
Initializing global node /matlab_global_node_36570 with NodeURI http://192.168.31.1:51073/

Después de inicializar el mundo Gazebo haciendo clic en el icono, 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)

Gráfico de Stateflow

Este ejemplo usa un gráfico de Stateflow para programar tareas en el ejemplo. Abra el gráfico para examinar el contenido y seguir las transiciones de estado durante la ejecución del gráfico.

edit exampleHelperFlowChartPickPlaceROSGazebo.sfx

El gráfico indica cómo interactúa el manipulador con los objetos o piezas. 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 obtener una descripción de alto nivel de los pasos de pick-and-place, consulte Pick-and-Place Workflow Using Stateflow for MATLAB.

Apertura y cierre de la pinza

El comando para activar la pinza, exampleCommandActivateGripperROSGazebo, envía una solicitud de acción para abrir y cerrar la pinza implementada en Gazebo. Para enviar una solicitud para abrir la pinza, se usa el siguiente código. Nota: El código de ejemplo que se muestra solo ilustra lo que hace el comando. No lo ejecute.

[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 función del comando exampleCommandMoveToTaskConfigROSGazebo se usa para mover el manipulador a poses específicas.

Planificación

La planificación de rutas genera trayectorias de espacio cartesiano simple desde una configuración de tarea inicial a una deseada usando trapveltraj y transformtraj. Para obtener más información sobre la planificación y ejecución de trayectorias, 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, exampleCommandMoveToTaskConfigROSGazebo 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 exampleCommandDetectPartsROSGazebo y exampleCommandClassifyPartsROSGazebo usan la transmisión de la cámara del efector final simulado del robot y aplican un modelo de aprendizaje profundo preentrenado para detectar las piezas reciclables. El modelo toma un marco de cámara como entrada y genera la ubicación 2D del objeto (posición de píxel) y el tipo de reciclaje que requiere (contenedor azul o verde). La ubicación 2D en el marco de la imagen se asigna al marco base del robot.

Entrenamiento del modelo de deep learning: adquisición y etiquetado de imágenes de Gazebo

El modelo de detección se entrenó usando un conjunto de imágenes adquiridas en el entorno de simulación dentro del mundo Gazebo con las dos clases de objetos (botella, lata) colocados en diferentes lugares de la mesa. Las imágenes se obtienen de la cámara simulada a bordo del robot, que se mueve a lo largo del plano horizontal y vertical para obtener imágenes de los objetos desde muchas perspectivas de cámara diferentes.

Las imágenes se etiquetan a continuación con la app Image Labeler (Computer Vision Toolbox), creando el conjunto de datos de entrenamiento para el modelo de detección YOLO v2. trainYOLOv2ObjectDetector (Computer Vision Toolbox) entrena el modelo. Para ver cómo entrenar una red YOLO v2 en MATLAB, consulte Train YOLO v2 Network for Vehicle Detection (Computer Vision Toolbox).

El modelo entrenado se despliega para la inferencia online sobre la imagen única adquirida por la cámara integrada cuando el robot está en la posición inicial. La función detect (Computer Vision Toolbox) devuelve la posición de la imagen de los cuadros delimitadores de los objetos detectados, junto con sus clases, que luego se utiliza para encontrar la posición del centro de la parte superior de los objetos. Usando un enfoque de proyección de cámara simple, suponiendo que se conoce la altura de los objetos, la posición del objeto se proyecta en el mundo y finalmente se usa como posición de referencia para seleccionar el objeto. La clase asociada con los cuadros delimitadores decide en qué contenedor colocar el objeto.

Iniciar la tarea de pick-and-place

Esta simulación usa un manipulador KINOVA Gen3 con una pinza Robotiq acoplada. Cargue el modelo de robot desde un archivo .mat como un objeto rigidBodyTree.

load('exampleHelperKINOVAGen3GripperROSGazebo.mat'); 

Inicializar el coordinador de pick-and-place

Establezca la configuración inicial del robot. Cree el coordinador, que maneja el control del robot, proporcionando el modelo de robot, la configuración inicial y el nombre 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 = exampleHelperCoordinatorPickPlaceROSGazebo(robot,initialRobotJConfig, endEffectorFrame);

Especifique la configuración inicial y dos poses para colocar objetos.

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 y visualizar la simulación

Conecte el coordinador al gráfico de Stateflow. Una vez iniciado, el gráfico de Stateflow es responsable de pasar continuamente por los estados de detección de objetos, recogerlos y colocarlos en el área de preparación correcta.

coordinator.FlowChart = exampleHelperFlowChartPickPlaceROSGazebo('coordinator', coordinator); 

Use un cuadro de diálogo para iniciar la ejecución de la tarea de pick-and-place. Haga clic en Yes en el cuadro de diálogo para comenzar la simulación.

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'
        coordinator.FlowChart.endPickPlace;
        delete(coordinator.FlowChart)
        delete(coordinator);
end

Finalizar la tarea de pick-and-place

El gráfico de Stateflow terminará de ejecutarse automáticamente después de 3 intentos fallidos de detectar nuevos objetos. Para finalizar la tarea de pick-and-place antes de tiempo, elimine el comentario y ejecute las siguientes líneas de código o presione Ctrl+C en la ventana de comandos.

% coordinator.FlowChart.endPickPlace;        
% delete(coordinator.FlowChart);
% delete(coordinator);

Observar los estados de la simulación

Durante la ejecución, los estados activos en cada momento se resaltan en azul en el gráfico de Stateflow. Esto ayuda a realizar un seguimiento de lo que hace el robot y cuándo. Puede hacer clic en los subsistemas para ver los detalles del estado en acción.

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

El mundo 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. Se produce la salida del gráfico de Stateflow cuando el paso de detección no encuentra más piezas cuatro veces.

if strcmp(answer,'Yes')
    while  coordinator.NumDetectionRuns <  4
        % Wait for no parts to be detected.
    end
end

Desconecte la red del ROS después de finalizar el ejemplo.

rosshutdown
Shutting down global node /matlab_global_node_36570 with NodeURI http://192.168.31.1:51073/

Copyright 2020 The MathWorks, Inc.