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:
Launch URSim with the desktop shortcut of a desired robot model (by default this example uses UR5e).
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:
Ensure that the external control program is configured and loaded in the program section. Refer to the setup document for more details.
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