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

This example shows how to use the universalrobot 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.

By default, the example uses the Universal Robots UR5e cobot model to simulate. You can choose between Gazebo simulator, URSim simulator or UR Hardware using the dropdown option provided in this example.

Overview

This example uses functionalities offered by the universalrobot object from Robotics Systems Toolbox™ support package for Universal Robots UR Series Manipulators to connect with simulated or physical cobot using the ROS network. Once the connection is established with the cobot, you can use various object functions of universalrobot to manipulate the cobot. The universalrobot 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 can select the Gazebo physics simulator, 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 Packages and Dependencies for ROS and Set Up URSim Offline Simulator for more details. Refer to Hardware Setup for UR Series Cobots 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

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

Select the Interface

interface = "Gazebo"

interface = 
"Gazebo"

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 the setup document for more details.

2. Ensure that the polyscope is in remote control mode. You can set thi 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. The RobotAddress is only required if you have selected hardware in the previous section. The RobotAddress is the IP address of the robot as specified in the local network setup document.

deviceAddress = '192.168.92.132';
username = 'user';
password = 'password';
ROSFolder = '/opt/ros/melodic';
WorkSpaceFolder = '~/ur_ws';
RobotAddress = '192.168.1.10'; % Only required if controlling UR Series hardware

Connect to ROS device.

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

Generate launch script and transfer it to ROS host computer.

generateAndTransferLaunchScript(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; ' ...
                   './launchUR.sh &']);
pause(10);
end

Initialize the universalrobot Interface

ur = universalrobot(device.DeviceAddress,'RigidBodyTree',ur5e)

Using user-defined rigid body tree model.

ur = 
  universalrobot with properties:

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

The universalrobot 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 message and ROS action respectively, which are used to communicate with the cobot via the ROS 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.0005    0.0118    0.0002    0.0006   -0.0000    0.0000

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

    3.1411   -0.0127    1.5708    0.8160    0.2325    0.0531

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.0001    0.0376    0.0007    0.0021   -0.0001    0.0001

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.0000    0.0404   -0.0001   -0.0044   -0.0001   -0.0309

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'. Defualt 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)

result = logical
   1

state = 
'succeeded'

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'. Defualt value of 'EndTime' is 5 seconds.

if isequal(interface,'Gazebo')
    jointWaypoints = [-30 -100 70 -150 -60 0]*pi/180;
elseif isequal(interface,'URSim') || isequal(interface,'Hardware')
    jointWaypoints = [20 -90 -70 -20 70 0]*pi/180;
end

Visulize 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'. Defualt 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)

result = logical
   1

state = 
'succeeded'

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'. Defualt 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)

Robot motion detected. Recording Data....
Robot motion stopped.

data = struct with fields:
    JointConfiguration: [877×6 double]
         JointVelocity: [877×6 double]
                  Pose: [877×6 double]
            EEVelocity: [877×6 double]

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.

if isequal(interface,'Gazebo')
    jointwayPoints = [-20 -85 110 -210 -70 0;
                      -10 -70  80 -160 -70 0;
                        0 -40  90 -110 -70 0;
                      -10 -70  80 -180 -70 0;
                      -20 -85 110 -210 -70 0;]*pi/180;

elseif isequal(interface,'URSim') || isequal(interface,'Hardware')
    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;
end

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 rosmaster and gazebo

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

function generateAndTransferLaunchScript(device,WorkSpaceFolder,interface,RobotAddress)
    
    % Open a file to write set of commands to lauunch interface with
    % simulated UR5 in gazebo or URSim
    fid=fopen(fullfile(tempdir,"launchUR.sh"),"w+");
    fprintf(fid,"#!/bin/sh\n");
    fprintf(fid,"export SVGA_VGPU10=0\n");
    fprintf(fid,"export ROS_IP=%s\n",device.DeviceAddress);
    fprintf(fid,"export ROS_MASTER_URI=http://$ROS_IP:11311\n");

    if isequal(interface,'Gazebo')
        fprintf(fid,"gnome-terminal --title=\42Simulated UR5 Robot\42 -- /bin/bash -c 'source %s/setup.bash; source %s/devel/setup.bash; roslaunch ur_gazebo ur5e_bringup.launch'",device.ROSFolder,WorkSpaceFolder);
    elseif isequal(interface,'URSim')
        fprintf(fid,"gnome-terminal --title=\42Simulated UR5 Robot\42 -- /bin/bash -c 'source %s/setup.bash; source %s/devel/setup.bash; roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=127.0.0.1'",device.ROSFolder,WorkSpaceFolder);
    elseif isequal(interface,'Hardware')
        fprintf(fid,"gnome-terminal --title=\42UR5e Robot Driver\42 -- /bin/bash -c 'source %s/setup.bash; source %s/devel/setup.bash; roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=%s'",device.ROSFolder,WorkSpaceFolder,RobotAddress);
    end
    
    fclose(fid);

    % Copy file into ROS device
    putFile(device,fullfile(tempdir,'launchUR.sh'),'~/')

    % Make the shell script executable
    system(device,'chmod a+x ~/launchUR.sh');
end

×

Open Example

You have a modified version of this example. Do you want to open this example with your edits?

No, overwrite the modified version Yes