Overview

This example uses functionalities offered by the 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

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 Interface and Define IP Address of ROS Enabled Device and Robot

The below diagram shows the naming convention of the various IP addresses used in this example.

Note: The above diagram is only applicable for the URSim simulator or UR Series hardware. MATLAB can be present inside the same ROS machine or It can also be on any external machine.

The ROSDeviceAddress is the IP address of your PC/VM where you have installed the ROS and the other required ROS drivers.

The robotAddress is the IP address of your robot hardware or simulated robot(in case of URSim). If you have installed URSim in the same ROS device, then the robot IP will be the same as the local host IP i.e. '127.0.0.1'. Hence robotAddress for URSim will be 127.0.0.1

interface = "Gazebo"

If you select Gazebo:

If you select URSim:

If you select Hardware:

Specify the below address parameters based on the above given diagram.

% IP of ROS enabled machine
ROSDeviceAddress = '192.168.92.132';

% IP of the robot (in case of URSim, this IP address will be 127.0.0.1)
robotAddress = '192.168.1.10'; 

Generate Automatic Launch Script and Transfer it to ROS Host Computer

Provide parameters of the host machine with ROS.

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

username = 'user';
password = 'password';
ROSFolder = '/opt/ros/melodic';
WorkSpaceFolder = '~/ur_ws';

Connect to ROS device.

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

Generate launch script and transfer it to ROS host computer.

generateAndTransferLaunchScriptGettingStarted(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

Optional: Launch Required ROS Driver Manually

Manually launch the required ROS driver in your ROS machine

Follow the below-given instruction on how to launch the required ROS drivers in the ROS enabled machine.

Step:- 1

First, open the terminal in your ROS machine and cd to the catkin workspace you built for the UR ROS drivers. (for example, "cd ~/ur_ws")

Step:- 2

Now source the setup file using the command,

source devel/setup.bash

Now, follow the next setup based on your interface selection.

Step:- 3

Export the ROS_IP and ROS_MASTER_URI by running below command inside the same terminal,

Replace the ROSDeviceAddress in below command by its value. (Use direct value here and not the char array i.e. export ROS_IP=192.168.92.132)

export ROS_IP=<ROSDeviceAddress>
export ROS_MASTER_URI=http://$ROS_IP:11311

Step:- 4

For Gazebo:

If you are using Gazebo then use the below command to launch the simulated robot with the required ROS drivers in Gazebo. Note that launching the UR robot in Gazebo does not require the robot's IP address.

roslaunch ur_gazebo ur5e_bringup.launch

For URSim:

This step assumes that you have opened the URSim and also configured the external control program node.

If you are using URSim then use the below-given command to launch the required ROS drivers. (As you have installed URSim in the same ROS machine then robot_ip will be localhost 127.0.0.1).

roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=127.0.0.1

For UR hardware:

This step assumes that you have configured the external control program node on the teach pendant and that also robot is in remote control mode.

If you are using UR hardware then use below given command to launch the required ROS drivers. Use the defined robotAddress as a value to robot_ip in the below command.

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

roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=<robotAddress>

Initialize Universalrobot Interface

ur = universalrobot(ROSDeviceAddress,'RigidBodyTree',ur5e)

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)

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)

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)

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)

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)

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

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

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)

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