Main Content

Pick-and-Place Workflow in Unity 3D Using ROS

This example uses:

Open Live Script

This example shows how to set up an end-to-end pick-and-place workflow for a robotic manipulator like the Kinova® Gen3, and simulate the robot in the Unity® game engine.

Overview

This example identifies and places objects into a bin using a Kinova Gen3 manipulator. The example uses tools from three toolboxes:

  • Robotics System Toolbox™ — Models and simulates the manipulator.

  • Stateflow® — Schedules the high-level tasks in the example, and steps the simulation from task to task.

  • ROS Toolbox™ — Connects MATLAB® to Unity.

This example builds on key concepts from these related examples:

To set up and launch the Unity® game engine from MATLAB®, see Set Up and Connect to Unity Game Engine.

Pick-and-Place Workflow

Start Pick-and-Place Task

Load Robot Model in MATLAB

This simulation uses a Kinova Gen3 manipulator with a Robotiq gripper attached. Load the robot model from a .mat file as a rigidBodyTree (Robotics System Toolbox) object.

load("exampleHelperKINOVAGen3GripperROSUnity.mat");

Initialize Pick-and-Place Coordinator

Set the initial robot configuration. Initialize the coordinator, which handles the robot control, by specifying 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";
coordinator = exampleHelperCoordinatorPickPlaceROSUnity(robot,initialRobotJConfig,endEffectorFrame);
Sending robots to initial positions...

In Unity, verify that the robot model has moved to the position specified by initialRobotJConfig.

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 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 = exampleHelperFlowChartPickPlaceROSUnity("coordinator",coordinator);

Create a dialog box for starting the pick-and-place task execution. To begin the simulation, click Yes in the dialog box.

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

Observe the robot model in Unity, and check that it performs the pick-and-place actions.

Ending Pick-and-Place Task

The Stateflow chart stops automatically after four failed attempts to detect new objects. To stop the pick-and-place task prematurely, uncomment and execute these lines of code or select the command window and press Ctrl+C.

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

Visualize Pick-and-Place Action in Unity

The Unity scene shows the robot in the working area as it moves the objects to the desired location. The robot continues working until all objects have been placed. When the detection step does not find any more objects four times, the Stateflow chart ends.

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

Kill the ROS TCP endpoint node and shut down the ROS network.

helperKillTCPEndpointNode;
rosshutdown;
Shutting down global node /matlab_global_node_37032 with NodeURI http://hyd-shivarad:64473/ and MasterURI http://localhost:11311.
Shutting down ROS master on http://172.18.250.141:11311.