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:

Pick-and-Place Workflow

Install Unity and Create New Project

Follow these steps to install Unity and create a new project:

  1. Download and install Unity Hub.

  2. Launch Unity Hub and sign in with your account.

  3. Install Unity Editor from Unity Hub. This example has been tested with Unity Editor version 2021.3.6f1 LTS.

  4. In Unity Hub, select Create project. Unity Editor opens with a new project.

Install Unity Packages

Install these packages using the package manager in Unity.

  • URDF Importer — To import the robot model from URDF files. Download this package as a ZIP file from the URDF Importer GitHub repository. This example has been tested with URDF Importer version 0.5.2.

  • ROS TCP Connector — To connect Unity with the ROS server. Download this package as a ZIP file from the ROS TCP Connector GitHub repository. This example has been tested with ROS TCP Connector version 0.7.0.

Follow these steps to install the packages:

  1. Extract the ZIP files containing the packages.

  2. In Unity Editor from the menu, select Window > Package Manager to open the Package Manager window.

  3. In the Package Manager window, click the + sign and select Add package from disk.

  4. Browse and open the package.json files for the two packages. The screenshot indicates the locations of the package.json files each of the packages.

Import Robot Model into Unity

The Robot.zip file included with this example contains the URDF file of the Kinova Gen3 robot with a custom gripper attached to it. The file also contains the relevant mesh files for the robot model.

To import the URDF model into Unity:

  1. Extract the contents of the Robot.zip file to the Assets folder in the Unity project directory.

  2. Right-click the URDF file in the Project pane and select Import Robot from Selected URDF file.

  3. In the URDF Import Settings window, click Import URDF.

Zoom into the scene to view the imported Kinova Gen3 robot model.

Set Up Scene in Unity

To simulate a pick-and-place workflow in Unity, create an environment in Unity containing a floor, a table, and a box to be picked.

Add Floor

  1. From the Unity menu, select GameObject > 3D Object > Plane. Unity Editor adds a plane to the scene and lists it in the Hierarchy pane.

  2. In the Hierarchy pane, right-click the plane object and select Rename. Rename it as Floor.

  3. In the Inspector pane, under Transform, set the X, Y, and Z values of Position to 0.

Add Table

  1. From the Unity menu, select GameObject > 3D Object > Cube. Unity Editor adds a cube to the scene and lists it in the Hierarchy pane.

  2. In the Hierarchy pane, right-click the cube object and select Rename. Rename it as Table.

  3. In the Inspector pane, under Transform, set the Y and Z values of Position to 0.25 and set Y value of Scale to 0.5.

Add Box

  1. From the Unity menu, select GameObject > 3D Object > Cube. Unity Editor adds a cube to the scene and lists it in the Hierarchy pane.

  2. In the Hierarchy pane, right-click the cube object and select Rename. Rename it as Box.

  3. In the Inspector pane, under Transform, set the Y value of Position to 0.525 and Z value of Position to 0.25.

  4. Set the X, Y, and Z values of Scale to 0.05.

  5. In the Inspector pane, under Mesh Renderer, change the Material property of the box to a non-default color for ease of visualization.

  6. Click Add Component and search for rigidbody.

  7. Select the RigidBody component to add it to the Inspector pane.

  8. In the Inspector pane, under RigidBody, set the value of Mass to 0.01. If Use Gravity is not selected, select it.

Add Tag to Box

  1. In the Hierarchy pane, select Box.

  2. In the Inspector pane, click the Tag dropdown and select Add Tag.

  3. Click the + sign to create a tag. Name the tag PickObject.

  4. In the Hierarchy pane, select Box.

  5. In the Inspector pane, click the Tag dropdown and select PickObject.

You can repeat the steps in Add Box and Add Tag to Box to add more objects for the robot to pick.

Adjust Camera

  1. In the Hierarchy pane, select Main Camera.

  2. In the Inspector pane, under Transform, adjust the Position and Rotation properties.

  3. Alternatively, you can manually adjust the camera position and orientation using the tool overlay in the Scene pane.

  4. You can preview the point of view of the camera while adjusting it.

Configure Robot Model in Unity

To configure the robot model:

  1. In the Hierarchy pane, select the robot object gen3_camera.

  2. In the Inspector pane, under Transform, set the Y value of Position to 0.5, which positions the robot on the top of the table.

  3. Under Controller (Script), set Stiffness to 1000, Damping to 100, and Force Limit to 10.

  4. Rename the robot object to gen3, to match the robot name used in the setCurrentRobotJConfig method of the exampleHelperCoordinatorPickPlaceROSUnity class.

The setCurrentRobotJConfig method of the exampleHelperCoordinatorPickPlaceROSUnity class uses this code.

function configRespUnity = setCurrentRobotJConfig(obj,JConfig)
    disp("Sending robots to initial positions...")
    
    %For Unity
    configReqUnity = rosmessage(obj.ROSinfo.UnityConfigClient);
    configReqUnity.ModelName = "gen3";
    configReqUnity.JointPositions = JConfig;
    configRespUnity = call(obj.ROSinfo.UnityConfigClient,configReqUnity,"Timeout",3);
end

To configure the base link of the robot model:

  1. In the Hierarchy pane, under the gen3 robot object, select base_link.

  2. In the Inspector pane, under Articulation Body, select Immovable.

To add collision boxes to the gripper fingers:

  1. In the Hierarchy pane, under the gen3 robot object, select gripper_finger1_finger_tip_link.

  2. In the Inspector pane, click Add Component and search for box collider.

  3. Select the Box Collider component to add it to the Inspector pane.

  4. Adjust the values of the Center and Size properties of the collision box to ensure that it aligns with the gripper finger mesh.

  5. Repeat the previous steps for gripper_finger2_finger_tip_link.

To set the physics solver type:

  1. From the menu, select Edit > Project Settings to launch the Project Settings window.

  2. In the left pane of the Project Settings window, select Physics.

  3. In the Physics settings pane, set Solver Type to Temporal Gauss Seidel.

ROS Custom Services and Publisher

MATLAB needs to communicate with Unity in order to transmit and receive information regarding the robot model. The execution of the pick-and-place workflow requires channels of communication between MATLAB and Unity. To establish communication for these channels, use custom services and a publisher.

This example includes the ROS custom service definition (.srv) files. You can find these files inside the unity_robotics_demo_msgs folder. The Generate Unity Scripts for Custom Services section shows how to generate ROS messages from the custom service definition files in Unity. The Generate Custom ROS Messages in MATLAB section shows how to generate ROS messages from the custom service definition files in MATLAB.

  • SetModelConfiguration.srv — Sends the robot joint configuration from MATLAB to Unity.

  • FollowTrajectory.srv — Sends the robot joint-space trajectory from MATLAB to Unity.

  • ObjectPosition.srv — Receives the position of the box to be picked from the Unity scene in MATLAB.

  • MoveGripperCommand.srv — Sends the command to open or close the gripper fingers from MATLAB.

The PNPScripts.zip file included with this example contains C# (.cs) scripts you can use to establish communication between Unity and MATLAB. The Add ROS Service Scripts in Unity section shows how to add the Unity ROS service scripts to the scene. For more examples of implementing the Unity ROS service, see these Unity Scripts in the Unity Robotics Hub GitHub repository.

  • SetModelConfiguration.cs — Receives the robot joint configuration from MATLAB in Unity. Unity then assigns these joint configuration values to the corresponding joints of the robot model. The script also unselects the Use Gravity option in all the links of the robot during simulation.

  • FollowTrajectory.cs — Moves the robot model along a set of joint configurations provided by the trajectory from MATLAB.

  • ObjectPosition.cs — Sends the position of the box to be picked from the Unity scene to MATLAB.

  • MoveGripper.cs — Opens or closes the gripper fingers attached to the robot arm in Unity.

  • JointStatePublisher.cs — Publishes the current joint configuration from Unity to the ROS network in MATLAB. For more information on publishing messages from Unity, see ROS–Unity Communication.

Generate Unity Scripts for Custom Services

You must build the ROS custom service definition files into C# scripts for use in Unity programming. The ROS TCP Connector package provides a tool you can use to build messages and services.

To build the custom services:

  1. Copy the unity_robotics_demo_msgs folder, which contains the ROS custom service definition files, to the Unity project folder. For more information on the service definition files in the unity_robotics_demo_msgs folder, see the ROS Custom Services and Publisher section.

  2. From the Unity menu bar, select Robotics > Generate ROS Messages, to launch the tool for building custom services.

  3. In the ROS Message Browser window, browse to the unity_robotics_demo_msgs folder inside the Unity project folder.

  4. Click Build 4 srvs.

Unity generates C# service scripts from the ROS custom service definition files.

Unity also returns errors regarding the missing control_msgs messages.

To build the required control_msgs messages:

  1. Extract the Control_Msgs.zip file inside the Unity project folder. For more information on ROS control messages, see control_msgs Github repository.

  2. From the Unity menu bar, select Robotics > Generate ROS Messages, to launch the tool for building these messages.

  3. In the ROS Message Browser window, browse to the control_msgs folder inside the Unity project folder.

  4. Build only these messages, named in the errors returned by Unity: FollowJointTrajectoryGoal.msg, GripperCommand.msg, and JointTolerance.msg.

  5. Click Build 3 msgs.

Unity generates C# message scripts from the control_msgs message definition files, which resolves the errors.

Add ROS Service Scripts in Unity

To add the Unity ROS service scripts to the scene:

  1. Extract the PNPScripts.zip file to the Assets folder in the Unity project directory. For more information on the service scripts in the PNPScripts.zip file, see the ROS Custom Services and Publisher section.

  2. From the Unity menu bar, select GameObject > Create Empty. Unity adds an empty GameObject to the scene and lists it in the Hierarchy pane.

  3. Rename the empty GameObject to PNPScriptsObject.

  4. In the Project pane, select all of the C# script files in the PNPScripts folder.

  5. Drag the files to PNPScriptsObject in the Hierarchy pane.

The scripts are added as components of PNPScriptsObject. In the Hierarchy pane of PNPScriptsObject, set the Gen 3 property of the Follow Trajectory, Joint State Publisher, Move Gripper, and Set Model Configuration components to the gen3 robot object.

Create Pick-and-Place Workflow in MATLAB

This example uses a Stateflow chart to schedule the tasks of the pick-and-place workflow. Open the chart to examine the contents and follow the state transitions during chart execution.

edit exampleHelperFlowChartPickPlaceROSUnity.sfx

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

  • Identify parts and determine where to place them.

  • Execute the pick-and-place workflow.

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

Opening and Closing Gripper

The exampleCommandActivateGripperROSUnity command function sends a service request to open and close the gripper implemented in Unity.

Moving Manipulator to Specified Pose

Use the exampleCommandMoveToTaskConfigROSUnity command function to move the manipulator to specified poses. After generating a joint trajectory for the robot to follow, the exampleCommandMoveToTaskConfigROSUnity function samples the trajectory at the desired sample rate, then packages it into joint-trajectory ROS messages, and sends a service request to Unity.

Trajectory Planning

Use trapveltraj and transformtraj functions to generate simple task-space trajectories from an initial to a desired task configuration. For more details on planning and executing trajectories, see Plan and Execute Task- and Joint-Space Trajectories Using Kinova Gen3 Manipulator.

Generate Custom ROS Messages in MATLAB

You must generate ROS custom messages in MATLAB from the custom service definition files inside the unity_robotics_demo_msgs folder. For more information on these service definition files, see the ROS Custom Services and Publisher section.

Use the rosgenmsg (ROS Toolbox) function to generate the custom messages, and follow the instructions in the dialog box to add the messages to the MATLAB path.

answer = questdlg("Are the Custom ROS messages generated for MATLAB?", ...
    "Custom ROS messages for MATLAB","Yes","No", "No");
switch answer
    case "Yes"
        % Continue with pick and place
    case "No"
        % Run rosgenmsg to build custom ROS messages for MATLAB
        rosgenmsg
end

answer = questdlg("Are the Custom ROS messages added to MATLAB path?", ...
    "ROS messages","Yes","Add path...", "Add path from current directory", "Yes");

switch answer
    case "Yes"
        % Continue with pick and place
    case "Add path..."
        % Add the custom ROS messages to the MATLAB path
        customROSMsgPath = uigetdir("", "Select the install\m folder created by rosgenmsg");
        addpath(customROSMsgPath)
        savepath
        clear classes
        rehash toolboxcache
    case "Add path from current directory"
        customROSMsgPath = uigetdir(fullfile(pwd, "matlab_msg_gen_ros1"), "Select the install\m folder created by rosgenmsg");
        addpath(customROSMsgPath)
        savepath
        clear classes
        rehash toolboxcache
end

Set Up MATLAB for Unity Connection

ROS Master Setup in MATLAB

Start the ROS master from MATLAB. Alternatively, you can connect Unity and MATLAB to a ROS master running elsewhere.

% Close any existing ROS Master
rosshutdown
% Create a ROS Master
rosinit
Launching ROS Core...
........Done in 8.3609 seconds.
Initializing ROS master on http://172.18.250.141:11311.
Initializing global node /matlab_global_node_65508 with NodeURI http://hyd-shivarad:53031/ and MasterURI http://localhost:11311.

Run TCP Endpoint Node in MATLAB

A ROS TCP endpoint node can facilitate the communication between Unity and ROS for sending and receiving messages.

Start the ROS TCP endpoint node in MATLAB.

helperConnectROSUnity;

Unity ROS Connection Settings

From the Unity menu bar, select Robotics > ROS Settings, to open the tool to specify the IP address of the machine running the ROS master in the MATLAB session. For a detailed guide on setting up the ROS Connection, see The Unity Side.

Unity Setup Verification

Click the play button in Unity to simulate the scene. The console displays any critical errors in your setup. Address the errors before proceeding with the example.

Ensure the Unity scene is running and that the connection to MATLAB is functioning. When the Unity sceneis playing, you should see an overlay on the Game pane. The blue and green arrows next to ROS IP indicate that there is a connection between Unity and the ROS master.

Unity Setup Check in MATLAB

Use a dialog to confirm the Unity is running. Click Yes in the dialog to begin the simulation.

answer = questdlg("Click the Play button in Unity. Select Yes once the connection is established.", ...
                  "Run Unity Scene","Yes","No", "No");
switch answer
    case "Yes"
        % Continue with pick and place
    case "No"
        % Stop the pick and place job
        coordinator.FlowChart.endPickPlace;
        delete(coordinator.FlowChart)
        delete(coordinator);
end

To check the connection status in MATLAB is functioning, run the following rostopic commands and verify if MATLAB is receiving the Unity published ROS topic /my_gen3_unity/joint_states.

rostopic list

/my_gen3_unity/joint_states
/rosout                    
/tf  

rostopic echo /my_gen3_unity/joint_states

  Header      
    Stamp      
      Sec  :  0
      Nsec :  0
    Seq     :  1
    FrameId :  1
  Name     :  {1}  shoulder_link   {2}  half_arm_1_link   {3}  half_arm_2_link   {4}  forearm_link   {5}  spherical_wrist_1_link   {6}  spherical_wrist_2_link   {7}  bracelet_link   
  Position :  [3.84349641535664e-06, -5.377179604693083e-06, 5.302986664901255e-06, -1.095666357286973e-05, 5.450971798381943e-07, -2.097234101938739e-07, -3.903266687643736e-08]
  Velocity :  [1.649751438306241e-05, -3.894936167378246e-05, 5.202663783397106e-05, 8.82921963625724e-06, -5.537591719446247e-05, 2.984850922248081e-05, -9.524918674711089e-06]
  Effort   :  []
---

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 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.