Main Content

Getting Started with Standalone ROS Node Generation for Universal Robots

Overview

This example shows how to plan, create and validate a deployable motion planning standalone ROS node for Universal Robot UR5e with URSim, Gazebo simulator or physical UR5e. In this example, you will

  • Create a motion planning logic with the Universal Robot UR5e simulated robot using universalrobot object and its functions.

  • Validate a motion planning logic with the simulated robot with Gazebo, URSim or physical UR5e.

  • Generate a deployable standalone ROS node and validate with Gazebo, URSim or physical UR5e.

In this example, you can select the Gazebo physics simulator, URSim offline simulator or physical UR5e hardware. All the necessary files required to setup URSim are included in the support package. Refer to Install ROS Packages and Dependencies for ROS and Set Up URSim Offline Simulator for more details.

Clear all output in the live script by right-clicking anywhere in the live script and then clicking ‘Clear All Output’ or navigate to VIEW > Clear all Output.

Note: Execute the script section by section as mentioned in this example.

Prerequisites

Required Products

  • Robotics System Toolbox™

  • Robotics System Toolbox™ Support Package for Universal Robots UR Series Manipulators

  • ROS Toolbox

  • MATLAB Coder™

Create and validate the motion planning algorithm with URSim, Gazebo simulator or physical UR5e in Connected I/O mode

In this example, we have considered the following tasks for the robot motion planning algorithm.

  1. Move robot to home position (in joint space)

  2. Move robot to desired joint configuration (in joint space)

  3. Move robot in circular trajectory path (in task space)

  4. Move robot in desired end-effector cartesian configuration (in task space)

  5. Move robot back to the home position (in joint space)

Select the simulator using drop-down menu given below.

interface = "UR5e Hardware";

If you select URSim:

  1. Launch URSim with the desktop shortcut of a desired robot model (by default this example uses UR5e).

  2. Ensure that the external control program is configured and loaded in the program section. Refer to the setup document for more details.

If you select Hardware:

  1. Ensure that the external control program is configured and loaded in the program section. Refer to Hardware Setup for UR Series Cobots for more details.

  2. Ensure that the Polyscope is in remote control mode. You can set this by pressing a Teach Pendant icon with 'Local' text on the top right corner of the Teach Pendant.

Connect to ROS host

Provide parameters of the host machine with ROS. Provide robot IP address in RobotAddress, if you have selected the hardware as an interface.

deviceAddress = '192.168.92.132';
username = 'user';
password = 'password';
ROSFolder = '/opt/ros/melodic';
WorkSpaceFolder = '~/ur_ws';
RobotAddress = '192.168.1.10';

Connect to ROS device.

device = rosdevice(deviceAddress,username,password);
device.ROSFolder = ROSFolder;

Generate launch script and transfer it to ROS host computer.

eampleHelperTransferLaunchScriptURCodegen(device,WorkSpaceFolder,interface,RobotAddress);

Launch the required nodes if the ROS core is not running.

if ~isCoreRunning(device)
    w = strsplit(system(device,'who'));
    displayNum = cell2mat(w(2));
    
    system(device,['export SVGA_VGPU10=0; ' ...
                   'export DISPLAY=' displayNum '.0; ' ...
                   './launchURCodegen.sh &']);
end

Load robot RigidBodyTree model and initialize the universalrobot interface

Here we are using Universal Robots UR5e robot model.

ur5e = loadrobot('universalUR5e');

Initialize the communication interface using universalrobot API.

ur = universalrobot(device.DeviceAddress,'RigidBodyTree',ur5e);
Using user-defined rigid body tree model.

Before running the next section, wait for the robot to get ready for accepting the commands if you are using the URSim or the physical hardware. You will be able to see messages like "Robot connected to reverse interface. Ready to receive control commands." in your terminal when the robot is ready for receiving the commands.

Task-1: Move robot to home position (in joint space)

Command robot to perform task-1. For this task, we use sendJointConfigurationAndWait to send joint command to the robot.

q_home = [0 -90 0 -90 0 0]*pi/180;

[result,state] = sendJointConfigurationAndWait(ur,q_home,'EndTime',5)
result = logical
   1

state = 
'succeeded'

Task-2: Move robot to desired joint configuration (in joint space)

Command robot to perform task-2. For this task, we use sendJointConfigurationAndWait to send joint command to the robot.

if isequal(interface,'Gazebo')
    jointWaypoints = [0 -90 90 -180 -90 0]*pi/180;
elseif isequal(interface,'URSim') || isequal(interface,'UR5e Hardware')
    jointWaypoints = [180 -90 90 -180 -90 0]*pi/180;
end

[result,state] = sendJointConfigurationAndWait(ur,jointWaypoints,'EndTime',5)
result = logical
   1

state = 
'succeeded'

Task-3: Move robot in circular trajectory path (in task space)

Compute the desired circular trajectory for task-3.

center = [0.4918 0.1334 0.6872]; %[x y z]
radius = 0.2;

theta = (0:30:360)';
points = center + radius*[zeros(size(theta)) cosd(theta) sind(theta)];
points = [center;points];

wayPointTimes = 0:1.2:size(theta,1)*1.2;
taskWayPoints = [ones(size(points)) .*[-pi/2  0  -pi/2], points];

Send command to robot to perform task-3. For this task, we use followWaypoints to send waypoints command to the robot.

followWaypoints(ur,taskWayPoints,wayPointTimes,'InterpolationMethod','minjerkpolytraj','NumberOfSamples',130);

Wait for the robot to complete the motion.

[result,~] = getMotionStatus(ur);
while ~result
    [result,~] = getMotionStatus(ur);
end
pause(2);

Task-4: Move robot in desired end-effectory cartesian configuration (in task space)

Command robot to perform task-4. For this task, we use sendCartesianPoseAndWait to send cartesian command to the robot.

desPos = [-pi/2  0  -pi/2 0.4918 0.1334 0.6872];
[result,state] = sendCartesianPoseAndWait(ur,desPos,'EndTime',3)
result = logical
   1

state = 
'succeeded'

Task-5: Move robot back to the home position (in joint space)

Command robot to perform task-5. For this task, we use sendJointConfigurationAndWait to send joint command to the robot.

[result,state] = sendJointConfigurationAndWait(ur,q_home,'EndTime',5)
result = logical
   1

state = 
'succeeded'

Generate a standalone C++ ROS node

Create a Function for Code Generation

To generate a standalone C++ node, we need to create a MATLAB function with the planned motion planning logic. This example uses pre-created MATLAB functions urStandaloneROSNodeGenerationFcnGazebo.m and urStandaloneROSNodeGenerationFcnURSim.m for the Gazebo and URSim simulator (also for UR5e hardware), respectively. Also, take care of the following things while creating a MATLAB function,

  • Ensure any modifications that you made during connected I/O simulations above are also reflected in MATLAB functions that are going to be used for standalone ROS node generation. If you have made any modifications above and you are using Gazebo simulator, then modify urStandaloneROSNodeGenerationFcnGazebo.m, and if you are using URSim or physicsl UR5e, then modify the urStandaloneROSNodeGenerationFcnURSim.m function.

  • If you are using any non-blocking functions (for example, sendJointConfiguration, followWaypoints, and so on), then it is recommened to use getMotionStatus function in your logic, similar to the case where we added in pre-created function to wait until the motion command is completed.

Note: The warnings similar to the one mentioned below are expected during the code generation process.

Warning: The "JointNames" field of the "trajectory_msgs/JointTrajectory" message is a cell string which is not supported in code generation. Do not access the "JointNames" field in your MATLAB code.

Suppressing the above warning for the subsequent code sections.

warning('off','ros:mlroscpp:codegen:UnsupportedMessageField');

Generate and deploy the standalone ROS node

Create a config object

cfg = coder.config('exe');
cfg.Hardware = coder.hardware('Robot Operating System (ROS)');
cfg.Hardware.BuildAction = 'Build and load';
cfg.Hardware.RemoteDeviceAddress = deviceAddress;
cfg.Hardware.RemoteDeviceUsername = username;
cfg.Hardware.RemoteDevicePassword = password;
cfg.Hardware.CatkinWorkspace = WorkSpaceFolder;
cfg.Hardware.DeployTo = 'Remote Device';
cfg.RuntimeChecks = true;

Assign a nodeName based on the selected interface.

if isequal(interface,'Gazebo')
    nodeName = 'urStandaloneROSNodeGenerationFcnGazebo';
elseif isequal(interface,'URSim') || isequal(interface,'UR5e Hardware')
    nodeName = 'urStandaloneROSNodeGenerationFcnURSim';
end

Generate a node and deploy it to targeted ROS machine. This might take longer time (may be around 5-10 minutes).

codegen(nodeName,'-config',"cfg");

Run the deployed standalone ROS node

Send a system command to ROS machine to run a generated standalone ROS node. If you have selcted Hardware, then ensure that the Polyscope is configured in the remote control mode. Also, ensure that you have verified the ROS node on the URSim before using it with the physical UR5e.

system(device,['cd ' WorkSpaceFolder ';' ' source devel/setup.bash;' ...
                ' rosrun ' lower(nodeName) ' ' nodeName]);

Kill ROS master

clear ur;
system(device,'killall -9 rosmaster');

% turn on the suppressed warning
warning('on','ros:mlroscpp:codegen:UnsupportedMessageField');