Overview

This example identifies and recycles objects into two bins using a KINOVA Gen3 manipulator. The example uses tools from five toolboxes:

This example builds on key concepts from the following related examples:

Robot Simulation and Control in Gazebo

Start a ROS-based simulator for a KINOVA Gen3 robot and configure the MATLAB® connection with the robot simulator.

This example uses a virtual machine (VM) available for download at Virtual Machine with ROS 2 Melodic and Gazebo.

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

rosinit(rosIP,11311); % Initialize ROS connection

Initializing global node /matlab_global_node_33097 with NodeURI http://192.168.203.1:60127/

After initializing the Gazebo world by click the icon, the VM loads a KINOVA Gen3 Robot arm on a table with one recycling bin on each side. To simulate and control the robot arm in Gazebo, the VM contains the ros_kortex ROS package, which are provided by KINOVA.

The packages use ros_control to control the joints to desired joint positions. For additional details on using the VM, refer to Get Started with Gazebo and a Simulated TurtleBot (ROS Toolbox)

Stateflow Chart

This example uses a Stateflow chart to schedule tasks in the example. Open the chart to examine the contents and follow state transitions during chart execution.

edit exampleHelperFlowChartPickPlaceROSGazebo.sfx

The chart dictates how the manipulator interacts with the objects, or parts. It consists of basic initialization steps, followed by two main sections:

For a high-level description of the pick-and-place steps, see Pick-and-Place Workflow using Stateflow for MATLAB.

Opening and closing the gripper

The command for activating the gripper, exampleCommandActivateGripperROSGazebo, sends an action request to open and close the gripper implemented in Gazebo. For example, to send a request to open the gripper, the following code is used.

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

Moving the Manipulator to a Specified Pose

The commandMoveToTaskConfig command function is used to move the manipulator to specified poses.

Planning

The path planning generates simple task-space trajectories from an initial to a desired task configuration using trapveltraj and transformtraj. For more details on planning and executing trajectories, see Plan and Execute Task- and Joint-space Trajectories using KINOVA Gen3 Manipulator.

Joint Trajectory Controller in ROS

After generating a joint trajectory for the robot to follow, commandMoveToTaskConfig samples the trajectory at the desired sample rate, packages it into joint-trajectory ROS messages and sends an action request to the joint-trajectory controller implemented in the KINOVA ROS package.

Detecting and classifying objects in the scene

The functions commandDetectParts and commandClassifyParts use the simulated end-effector camera feed from the robot and apply a pretrained deep-learning model to detect the recyclable parts. The model takes a camera frame as input and outputs the 2D location of the object (pixel position) and the type of recycling it requires (blue or green bin). The 2D location on the image frame is mapped to the robot base frame.

Deep-Learning Model Training: Acquiring and Labeling Gazebo Images

The detection model was trained using a set of images acquired in the simulated environment within the Gazebo world with the two classes of objects (bottle, can) placed on different locations of the table. The images are acquired from the simulated camera on-board the robot, which is moved along the horizontal and vertical planes to get images of the objects from many different camera perspectives.

The images are then labeled using the Image Labeler app, creating the training dataset for the YOLO v2 detection model. trainYOLOv2ObjectDetector trains the model. To see how to train a YOLO v2 network in MATLAB, see Train YOLO v2 Network for Vehicle Detection (Computer Vision Toolbox).

The trained model is deployed for online inference on the single image acquired by the on-board camera when the robot is in the home position. The detect function returns the image position of the bounding boxes of the detected objects, along with their classes, that is then used to find the position of the center of the top part of the objects. Using a simple camera projection approach, assuming the height of the objects is known, the object position is projected into the world and finally used as reference position for picking the object. The class associated with the bounding boxed decides which bin to place the object.

Start the Pick-and-Place Task

This simulation uses a KINOVA Gen3 manipulator with a Robotiq gripper attached. Load the robot model from a .mat file as a rigidBodyTree object.

load('exampleHelperKINOVAGen3GripperROSGazebo.mat'); 

Initialize the Pick and Place Coordinator

Set the initial robot configuration. Create the coordinator, which handles the robot control, by giving the robot model, initial configuration, and end-effector name.

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

Initialize the coordinator by giving the robot model, initial configuration, and end-effector name.

coordinator = exampleHelperCoordinatorPickPlaceROSGazebo(robot,initialRobotJConfig, endEffectorFrame);

Specify the home configuration and two poses for placing objects.

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

Run and Visualize the Simulation

Connect the coordinator to the Stateflow Chart. Once started, the Stateflow chart is responsible for continuously going through the states of detecting objects, picking them up and placing them in the correct staging area.

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

Use a dialog to start the pick-and-place task execution. Click Yes in the dialog to begin the simulation.

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

Ending the pick-and-place task

The Stateflow chart will finish executing automatically after 3 failed attempts to detect new objects. To end the pick-and-place task prematurely, uncomment and execute the following lines of code or press Ctrl+C in the command window.

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

Observe the Simulation States

During execution, the active states at each point in time are highlighted in blue in the Stateflow chart. This helps keeping track of what the robot does and when. You can click through the subsystems to see the details of the state in action.

Visualize the Pick-and-Place Action in Gazebo

The Gazebo world shows the robot in the working area as it moves parts to the recycling bins. The robot continues working until all parts have been placed.

Shutdown the ROS network after finishing the example.

rosshutdown

Shutting down global node /matlab_global_node_33097 with NodeURI http://192.168.203.1:60127/

Copyright 2020 The MathWorks, Inc.