This example shows how to design and validate real-life application workflows for a robotic manipulators like the UR5 cobot from
Universal Robots
, using Interactive Rigid Body Tree and Gazebo® physics simulator, Robotics Systems Toolbox™, and ROS Toolbox.
Overview
This example demonstrates motion planning and execution for a glue dispensing application using a cobot. A task space based trajectory algorithm is demonstrated in this example using Robotics System Toolbox™ features. The trajectory planning ensures that we get approximately constant TCP (Tool Center Point) linear velocity for uniform glue dispensing on the windshield object (considered as a reference object for the demonstration).
In this example, first we will use the rigid body tree models to plan and simulate the trajectory in MATLAB and later we will use the Gazebo physics simulator and
universalrobot
object to validate the cobot control and motion across the precalculated task space trajectory. All the necessary files required to setup Gazebo are included in the support package. Refer to
Install ROS Packages and Dependencies for ROS
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.
Required Products
Motion Planning and Rigid Body Tree Simulation
Load Robot Model
This example uses the Universal Robot cobot model UR5, a 6 degree-of-freedom robot manipulator. The
loadrobot
generates a
rigidBodyTree
model from a description stored in a Unified Robot Description Format (URDF) file.
Load rigid body tree model of Universal Robot UR5.
Add a Dummy Glue Dispenser Tool at the End-Effector Body
For the glue dispensing application, add a dummy glue dispenser at the end-effector of the cobot using
addBody
.
The visuals can be added by
addVisuals
using the STL file.
interactiveRigidBodyTree
functionalities are used in this example to set the orientation of the end-effector using marker body.
Define the inline function for rotations around x-axis to use in further code sections.
Add dispenser body to the 'tool0' (default end-effector body).
Add one more fixed-body at the tip of the dispenser gun to compute the desired inverse transform in further sections.
Visualize the Robot Model
Create an interactive rigid body tree object and associated figure.
Load Windshield STL model for Glue Dispensing Application in the Same Rigid Body Tree Environment
This section loads the windshield STL object with the Rigid Body Tree robot world. Attach the 'ButtonDownFcn' callback function to the patch object of the windshield body to enable waypoint selection from the object surface using mouse button click.
Select Desired Waypoints on Windshield Body Using Mouse Button Callback
Select waypoints on the object before executing this section. Use mouse button click to select the desired waypoints. You can notice that after each successful selection, a green marker point will appear on the windshield body.
Note: If you want to consider the default waypoints for coming sections, then you can run this section without selecting any waypoints.
Get the Selected Waypoints Data
Get the coordinates of the selected waypoints for the motion planning. If no waypoints are selected, the model uses the default waypoints as mentioned in the below code section. This section also removes the 'ButtonDownFcn' callback to avoid the issue with the unnecessary waypoint selection after this section.
Remove the 'ButtonDownFcn' callback from STL object.
Attach Keypress Callback to Set the Desired Orientation at Each Selected Waypoint
Add callback for keyboard keys to the GUI to enable the orientation setting using keyboard keys. The end-effector body interactive marker provided by
interactiveRigidBodyTree
is also available to set the desired orientation.
Press Up-Down arrow key to rotate about the X-axis. Press Left-Right arrow key to rotate about the Y-axis.
Attach the 'KeyPressFcn' callback to set the orientation using keyboard keys.
Press Up-Down arrow key to rotate about the X-axis. Press Left-Right arrow key to rotate about the Y-axis.
Press C to save orientation for current waypoint and press N to move to the next waypoint.
Set Orientation of Each Desired Waypoint
Adjust the orientation for the desired waypoints using the keyboard keys. Here are the details of the attached keyboard keys callback. Before using this keyboard button, ensure that no figure window tool is selected.
-
Up Arrow / Down Arrow: To change the orientation of the cobot end-effector tool about X-direction
-
Right Arrow / Left Arrow: To change the orientation of the cobot end-effector tool about Y-direction
-
C: To save the orientation data for the current waypoint
-
N: To move to the next waypoint from the current waypoint
If you want to skip the current waypoint and do not want to consider it in the trajectory computation, then just press the 'N' button without saving the orientation data.
Note: Use interactive marker only for the rotational adjustment of the end-effector.
To use an interactive marker for setting the orientation, you can click and rotate the body about an axis using the red, green, and blue circles. To disable the marker body. right-click on the tool body and click "Toggle marker on/off". It is advisable to use the marker for major corrections and keyboard keys for finer adjustments.
Note: If you want to consider default orientations for further sections, then you can run this section without setting orientation.
Get the Final Waypoint and Orientation Data
Get the desired orientation data for the motion planning. If orientation is not set for the desired waypoints, then it used the default orientation as mentioned in this code section. This section also removes the 'KeyPressFcn' callback.
Task Space Trajectory Planning Algorithm (Based on Inverse Kinematics)
The task space trajectory planning algorithm used in this example takes the input argument as user-defined TCP velocity and the time resolutions for the trajectory computation. These are the high-level steps implemented to generate the desired trajectory.
-
Get the desired waypoints and orientation array with the desired TCP velocity and time resolution.
-
Consider the path segment between two consecutive waypoints.
-
Find the distance of the path segment.
-
Find the time to traverse for the considered path segment from the distance calculated and the input TCP velocity.
-
Get intermediate waypoint transforms using
transformtraj
using computed trajectory time array, considering the time resolution that you defined.
-
Compute the joint data using
inverseKinematics
at each intermediate waypoint.
-
Compute the joint velocity and acceleration using
minjerkpolytra
j
.
For more details, refer to the helper function
exampleHelperURGenerateTrajectory
attached with this example.
Compute the Trajectory to Traverse Through the Desired Waypoints on the Windshield
Compute the second trajectory for traversing through the desired waypoints on the windshield.
Move Cobot Near to the Windshield at the First Selected Desired Waypoint
Compute the first trajectory which moves the cobot from the home position to the first selected desired waypoint.
Rigid Body Tree Simulation of the Computed Trajectory
Visualize the computed trajectory using Rigid Body Tree simulation.
Plot the Joint Position, Joint Velocity, Joint Acceleration, and TCP Linear Velocity of Computed Trajectory
eeSpeed = zeros(length(computedTrajForTask.position),6);
for i=1:length(computedTrajForTask.position)
% Calculate geometric jacobian
jacobian = geometricJacobian(ur5_RBT,computedTrajForTask.position(i,:),'dispenserEdge');
% Calculate end-effector velocity
eeSpeed(i,:) = (jacobian*computedTrajForTask.velocities(i,:)')';
close(findobj('type','figure','name','Desired joint position'));
hFig1 = figure('Name','Desired joint position');
set(hFig1,'Visible','on')
sgtitle("Desired joint position")
for i=1:6
subplot(2,3,i);
plot(trajTimes,computedTrajForTask.position(:,i));
titleString = "Actuator:" + num2str (i);
title(titleString);
ylabel("Angle(rad)");
xlabel("Time(second)");
grid on;
axis auto;
close(findobj('type','figure','name','Desired joint velocity'));
hFig2 = figure('Name','Desired joint velocity');
set(hFig2,'Visible','on')
sgtitle("Desired joint velocity")
for i=1:6
subplot(2,3,i);
plot(trajTimes,computedTrajForTask.velocities(:,i));
titleString = "Actuator:" + num2str (i);
title(titleString);
ylabel("Velocity(rad/s)");
xlabel("Time(second)");
grid on;
axis auto;
close(findobj('type','figure','name','Desired joint acceleration'));
hFig3 = figure('Name','Desired joint acceleration');
set(hFig3,'Visible','on')
sgtitle("Desired joint acceleration")
for i=1:6
subplot(2,3,i);
plot(trajTimes,computedTrajForTask.acceleration(:,i));
titleString = "Actuator:" + num2str (i);
title(titleString);
ylabel("Acceleration(rad/s^2)");
xlabel("Time(second)");
grid on;
axis auto;
close(findobj('type','figure','name','Desired end-effector linear velocity'));
hFig4 = figure('Name','Desired end-effector linear velocity');
set(hFig4,'Visible','on')
plot(trajTimes,vecnorm(eeSpeed(:,4:6)'));
title("Desired end-effector linear velocity")
ylabel("Velocity(m/s)");
xlabel("Time(second)");
grid on;
axis auto;
ROS-Gazebo Simulation
Gazebo simulation uses the ROS interface and
universalrobot
object to control the simulated robot in Gazebo. 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.
Press ‘y’ and press 'Enter' key on the keyboard in the MATLAB command window to continue with the Gazebo simulation or press 'n' to return. The Gazebo simulation requires the setup. Refer to
Install ROS Packages and Dependencies for ROS
for more details.
Connect to ROS Device and Launch Gazebo model
Modify the below ROS device parameters as per your system before running this section.
Connect to ROS device, copy required files, and build the workspace.
Launch the Gazebo world with required ROS nodes.
Initialize the
universalrobot
Interface
Create
universalrobot
object to communicate with Universal Robot's Gazebo model.
Command Cobot to Move to the First Selected Desired Waypoint from Home Position
Command the cobot to move the first selected waypoint using
sendJointConfiguration
method
.
Send the Computed Trajectory of the Glue Dispensing Application
Send the computed trajectory to traverse the cobot through the desired waypoints over the windshield body.
Use the previously computed time vector.
Send the followTrajectory command to the cobot's Gazebo model.
Kill the rosmaster and gazebo
