Getting Started with Connecting and Controlling a UR5e Cobot from Universal Robots using ROS 2

This example uses:

This example shows how to use the urROS2Node object to connect with a Universal Robots cobot and move the robot using joint space control, task space control, waypoint tracking in task space, and waypoint tracking in joint space. This example uses Robotics Systems Toolbox™ and ROS Toolbox.

Overview

This example uses functionalities offered by the urROS2Node object from Robotics Systems Toolbox™ support package for Universal Robots UR Series Manipulators to connect with simulated or physical cobot using the ROS 2 network. Once the connection is established with the cobot, you can use various object functions of urROS2Node to manipulate the cobot. The urROS2Node object uses a motion planning algorithm offered by Robotics Systems Toolbox™ to achieve joint space control, task space control, waypoint tracking in task space, and waypoint tracking in joint space.

In this example, you use either URSim offline simulator or UR series robot hardware. All the necessary files required to setup URSim are included in the support package. Refer to Install ROS 2 and Dependencies and Set Up URSim Offline Simulator for ROS 2 for more details. Refer to Hardware Setup for UR Series Cobots for ROS 2 for more details to establish communication between MATLAB and UR series hardware.

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.

Required Products

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

Prerequisites

Follow the Hardware setup screens and complete the steps provided for ROS 2.

The ROS 2 support for Universal robot is currently supported on Ubuntu® 22.04 LTS with ROS 2 version Humble.

Load Rigid Body Tree model

Load rigid body tree model for Universal Robot UR5e robot manipulator. You can modify this according to the cobot model you want to control using MATLAB.

clear;
ur5e = loadrobot('universalUR5e');

Generate Automatic Launch Script and Transfer it to ROS 2 Host Computer

Provide parameters of the host machine with ROS 2.

Note: If you have not yet installed the required ROS 2 drivers in your ROS 2 machine then follow Install ROS 2 and Dependencies to setup a colcon workspace.

username = 'user';
password = 'password';
ROS2Folder = '/opt/ros/humble';
ROS2Workspace = '/opt/ros/humble'; 

ROS2DeviceAddress = '192.168.75.131'

ROS2DeviceAddress = 
'192.168.75.131'

robotAddress = '192.168.56.101';

Connect to ROS 2 device.

device = ros2device(ROS2DeviceAddress,username,password);
device.ROS2Folder = ROS2Folder;
device.ROS2Workspace = ROS2Workspace;

Generate launch script and transfer it to ROS 2 computer.

generateAndTransferLaunchScriptROS2GettingStarted(device,ROS2Workspace,robotAddress);

Launch Required ROS 2 Driver Manually

If you observe issues while running the generateAndTransferLaunchScriptROS2GettingStarted function, you can manually launch ROS 2 driver.

Follow these steps to launch the required ROS 2 drivers in the ROS 2 enabled machine.

Step 1

Open the terminal in your ROS 2 machine and cd (Change directory) to the colcon workspace you built for the UR ROS 2 drivers (for example, "cd ~/ur_ws")

Step 2

Source the setup file using the command,

source install/setup.bash

For URSim:

This step assumes that you have opened the URSim and also configured the external control program node. For more details, see Set Up URSim Offline Simulator for ROS 2.

If you are using URSim from the docker, then robot_ip will be 192.168.56.101. Use the below command to launch the required ROS 2 drivers.

ros2 launch ur_robot_driver ur5e.launch robot_ip:=<robotAddress>

For UR hardware:

This step assumes that you have configured the external control program node on the Teach pendant and also that robot is in remote control mode. For more details, see Hardware Setup for UR Series Cobots for ROS 2.

If you are using UR Series hardware, then use the below command to launch the required ROS 2 drivers. Use the defined robotAddress as a value to robot_ip in the below command. The robotAddress is the static IP address (192.168.1.10, as shown in Configure Network Parameters of OEM Control Box, and also defined in the Hardware Setup screen).

Note: Directly use the robot address as a numeric value and do not use a char array to assign value to the robot_ip.

ros2 launch ur_robot_driver ur5e.launch robot_ip:=<robotAddress>

Initialize urROS2Node Interface

ur = urROS2Node('RigidBodyTree',ur5e)

Using user-defined rigid body tree model.

ur = 
  urROS2Node with properties:

                  RigidBodyTree: [1×1 rigidBodyTree]
                JointStateTopic: '/joint_states'
    FollowJointTrajectoryAction: '/scaled_joint_trajectory_controller/follow_joint_trajectory'
                       DomainID: 0
                 NumberOfJoints: 6
                EndEffectorName: 'tool0'

The urROS2Node object has several useful properties. The RigidBodyTree property contains the rigid body tree model, which is being used internally for trajectory generation and motion planning. You can use this rigid body tree model to visualize the robot manipulator in MATLAB.

The JointStateTopic and FollowJoinTrajectoryAction properties consist of ROS 2 message and ROS 2 action respectively, which are used to communicate with the cobot via the ROS 2 network.

The EndEffectorName is the name of the frame of rigid body tree model which is considered as the end effector of the cobot.

Read Various Robot State Data

Read current joint angle configuration with 10-seconds timeout. The getJointConfiguration function returns the current joint angle of the cobot in the range of -pi to pi. The order of the joints is similar to the rigid body tree model.

jointAngles = getJointConfiguration(ur,10)

jointAngles = 1×6

   -0.7829   -1.6473   -1.0776   -1.1977    0.7802   -0.0152

Show the robot in current joint configuration

show(ur.RigidBodyTree,jointAngles);

Read current end-effector pose

The getCartesianPose function returns current cartesian pose of the end-effector in the form of [thetaz thetay thetax X Y Z] with units [rad rad rad m m m].

pose = getCartesianPose(ur)

pose = 1×6

   -2.9662   -0.5364   -2.1847    0.2183   -0.5051    0.7665

Read the current velocity of each joint

The getJointVelocity function returns current joint angle velocity of the manipulator. The unit is rad/s.

jointVelocity = getJointVelocity(ur)

jointVelocity = 1×6

     0     0     0     0     0     0

Read the current end-effector velocity

The getEndEffectorVelocity method returns the velocity of the end-effector in the form of [theta_dotz theta_doty theta_dotx Vx Vy Vz] with units [rad/s rad/s rad/s m/s m/s m/s].

eeVelocity = getEndEffectorVelocity(ur)

eeVelocity = 1×6

     0     0     0     0     0     0

Control the UR5e Cobot

Joint Space Control

Command the robot to reach to desired joint configuration and wait for the robot to complete the motion

The expected input to sendJointConfigurationAndWait function is desired joint configuration in the range of -pi to pi for all six joints. It also accepts an optional argument 'EndTime' in seconds, The cobot will try to complete the motion from current joint configuration to desired joint configuration in 'EndTime'. Default value of 'EndTime' is 5 seconds.

This method has a blocking function call and it will wait for the robot to complete the motion. The result will be 'true' if the robot motion has been completed successfully and 'false' otherwise. The state output contains more information about the status of the robot motion and it is consistent with the state output of sendGoalAndWait API.

jointWaypoints = [0 -90 0 -90 0 0]*pi/180;
[result,state] = sendJointConfigurationAndWait(ur,jointWaypoints,'EndTime',5)

Command the robot to reach to desired joint configuration

The expected input to sendJointConfiguration method is a desired joint configuration in the range of -pi to pi for all six joints. It also accepts an optional argument 'EndTime' in seconds, The robot will try to complete the motion from current joint configuration to the desired joint configuration in 'EndTime'. Default value of 'EndTime' is 5 seconds.

jointWaypoints = [20 -90 -70 -20 70 0]*pi/180;

Visualize the robot in desired joint angle configuration

show(ur.RigidBodyTree,jointWaypoints);

Command the cobot to move to desired joint angle configuration

sendJointConfiguration(ur,jointWaypoints,'EndTime',5);

Wait for the robot to complete the motion

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

Task Space Control

Command the cobot to reach to desired cartesian pose and wait for the robot to complete the motion

The expected input to sendCartesianPoseAndWait function is desired end-effector pose in the form of [thetaz thetay thetax X Y Z]. It also accepts an optional argument 'EndTime' in seconds, The cobot will try to complete the motion from current pose to the desired pose in 'EndTime'. Default value of 'EndTime' is 5 seconds.

This method has a blocking function call and it will wait for the cobot to complete the motion. The result will be 'true' if the cobot motion has been completed successfully and 'false' otherwise. The state output contains more information about the status of the cobot motion and it is consistent with the state output of sendGoalAndWait API.

desPos = [-pi/2  0  -pi/2 0.5    0    0.8];
[result,state] = sendCartesianPoseAndWait(ur,desPos,'EndTime',2)

Command the cobot to reach to desired cartesian pose

The expected input to sendCartesianPose function is desired end-effector pose in the form of [thetaz thetay thetax X Y Z]. It also accepts an optional argument 'EndTime' in seconds, The robot will try to complete the motion from current pose to desired pose in 'EndTime'. Default value of 'EndTime' is 5 seconds.

desPos = [-pi/2  0  -pi/2 0.5    0    0.5];
sendCartesianPose(ur,desPos,'EndTime',2)

Wait for the robot to complete the motion

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

Task Space Waypoint Tracking

Command robot to follow desired set of task waypoints

The expected input to followWaypoints function is set of cartesian waypoints and time from start to reach each waypoint. It also allows you to specify interpolation method to generate the trajectory to track the waypoints. Additionally, you can also define number of samples for the trajectory generation. More number of samples ensures smooth tracking in the task space, but takes more time to compute the trajectory.

taskWayPoints = [-pi/2    0  -pi/2 0.5     0  0.5;
                 -pi/4    0  -pi/2 0.5   0.3  0.6;
                 -pi/2    0  -pi/2 0.5     0  0.8;
                 -3*pi/4  0  -pi/2 0.5  -0.3  0.6;
                 -pi/2    0  -pi/2 0.5     0  0.5];

wayPointTimes = [0 2 4 6 8];

followWaypoints(ur,taskWayPoints,wayPointTimes,'InterpolationMethod','trapveltraj','NumberOfSamples',100);

Record the robot data during the motion

The recordRobotState function allows you to record key cobot data like joint configuration, joint velocity, end-effector pose and end effector velocity during the cobot's motion. The methods waits for the robot to start moving by observing maximum joint speed. When maximum joint speed crosses the velocity threshold (second optional position argument), the method starts recording the robot state data at the logging rate (first optional positional argument). Once the minimum joint speed falls below the velocity threshold, the method stops the logging.

data = recordRobotState(ur,100,0.5e-2)

Joint Space Waypoint Tracking

Command robot to follow desired set of joint waypoints

The followTrajectory function enables more lower-level control over the robot trajectory compared to followWaypoints. The expected input to followTrajectory is joint position, velocity, acceleration commands and time series for the robot motion.

jointwayPoints = [20  -95 -110  30 70 0;
                      10 -100 -120   0 70 0;
                       0 -110 -130 -50 70 0;
                      10 -100 -120   0 70 0;
                      20  -95 -110  30 70 0;]*pi/180;

trajTimes = linspace(0,wayPointTimes(end),20);
[q,qd,qdd] = bsplinepolytraj(jointwayPoints',[0 wayPointTimes(end)],trajTimes);

followTrajectory(ur,q,qd,qdd,trajTimes);

Wait for the robot to complete the motion

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

Kill the ros2 nodes

clear ur
system(device, 'pkill -f ros2');