Step by step explanation of a dual-robot setup
This chapter explains all the steps necessary to create a dual-arm setup using the
ur_robot_driver
.
In order to create a multi-robot workcell we basically have to consider three aspects:
When integrating multiple robots into one robot_description we face one challenge: We can’t have multiple tf frames with the same name, e.g.
base_link
in the same robot_description. Therefore, we’ll have to use prefixes.Once the robot description uses tf prefixes, the joint names will also use those prefixes. Therefore, we’ll need to adapt our robot controllers that they are defined on those “modified” joint names.
As we will be starting multiple instances of the same ROS nodes, we’ll have to take care that their names (nodes, topics, services) do not collide. One easy way to achieve this is using ROS namespaces
Assembling the URDF
The ur_description package provides macro files to generate an instance of a Universal Robots arm. We’ll use this to assemble a description containing a Box with a UR3e and a UR10e ontop:
1<?xml version="1.0"?>
2<robot xmlns:xacro="http://wiki.ros.org/xacro" name="my_work_cell">
3 <!--Load the macro for creating a robot-->
4 <xacro:include filename="$(find ur_description)/urdf/inc/ur10e_macro.xacro"/>
5 <xacro:include filename="$(find ur_description)/urdf/inc/ur3e_macro.xacro"/>
6
7 <!--Instanciate the robots-->
8 <xacro:ur3e_robot prefix="bob_" kinematics_parameters_file="$(arg bob_kinematics)"/>
9 <xacro:ur10e_robot prefix="alice_" kinematics_parameters_file="$(arg alice_kinematics)" />
10
11 <!--common link where the tf tree originates from-->
12 <link name="world"/>
13
14 <!--Define the robot poses in the world-->
15 <joint name="world_to_bob" type="fixed">
16 <parent link="world" />
17 <child link = "bob_base_link" />
18 <origin xyz="-0.5 0 0" rpy="0 0 0" />
19 </joint>
20 <joint name="world_to_alice" type="fixed">
21 <parent link="world" />
22 <child link = "alice_base_link" />
23 <origin xyz="0.5 0 0" rpy="0 0 0" />
24 </joint>
25</robot>
Let’s break it down:
First, we’ll have to include the macros to generate the two arms:
4 <xacro:include filename="$(find ur_description)/urdf/inc/ur10e_macro.xacro"/>
5 <xacro:include filename="$(find ur_description)/urdf/inc/ur3e_macro.xacro"/>
The two include lines only loaded the macro for generating robots. Next, we can call the macros to actually create the arms.
8 <xacro:ur3e_robot prefix="bob_" kinematics_parameters_file="$(arg bob_kinematics)"/>
9 <xacro:ur10e_robot prefix="alice_" kinematics_parameters_file="$(arg alice_kinematics)" />
This creates the two robots alice
and bob
. We choose their names as a tf-prefix in order to
make the link and joint names unique. Note the trailing underscore in the prefixes, as the prefixes
will be added in front of the link and joint names without adding a further underscore.
The last thing to do is connecting the two robots to our common world
link by adding two fixed
joints.
15 <joint name="world_to_bob" type="fixed">
16 <parent link="world" />
17 <child link = "bob_base_link" />
18 <origin xyz="-0.5 0 0" rpy="0 0 0" />
19 </joint>
20 <joint name="world_to_alice" type="fixed">
21 <parent link="world" />
22 <child link = "alice_base_link" />
23 <origin xyz="0.5 0 0" rpy="0 0 0" />
24 </joint>
We can view our custom workcell by running
roslaunch ur_example_dual_robot view_dual_robot.launch
Use the sliders of the joint_state_publisher_gui to move the virtual robots around. It should look something like this:
Define controller configuration files for the two robots
In the last paragraph we created a description containing both robots. To avoid name clashes, both
robots got a prefix alice_
and bob_
respectively.
For using the ur_robot_driver
with this description we’ll have to make sure that we load
controllers containing the joint names with the prefixes.
To keep things simple, we’ll copy the controller configuration from the ur_robot_driver
and only
rename the joints inside. The following listing shows how that could be achieved, if you cloned this
repo you will already have the corresponding files.
cd <src-location-of-ur_example_dual_robot>
mkdir -p etc
roscp ur_robot_driver ur10e_controllers.yaml etc/alice_controllers.yaml
roscp ur_robot_driver ur3e_controllers.yaml etc/bob_controllers.yaml
With a text editor, we open the files and change the joint names that they contain the prefixes
5# Settings for ros_control hardware interface
6ur_hardware_interface:
7 joints: &robot_joints
8 - alice_shoulder_pan_joint
9 - alice_shoulder_lift_joint
10 - alice_elbow_joint
11 - alice_wrist_1_joint
12 - alice_wrist_2_joint
13 - alice_wrist_3_joint
5# Settings for ros_control hardware interface
6ur_hardware_interface:
7 joints: &robot_joints
8 - bob_shoulder_pan_joint
9 - bob_shoulder_lift_joint
10 - bob_elbow_joint
11 - bob_wrist_1_joint
12 - bob_wrist_2_joint
13 - bob_wrist_3_joint
Create a launchfile and start drivers for both robots
Now we have everything in place to startup two driver instances for alice and bob. The
ur_robot_driver
offers different levels of abstractions inside its launchfiles. Basically, we
need the following components:
load the description
Driver for alice
Driver for bob
robot_state_publisher(s?)
First of all, loading the description can be done straight-forward:
24 <include file="$(find ur_example_dual_robot)/launch/load_dual_description.launch">
25 <arg name="bob_kinematics" value="$(arg bob_kinematics)"/>
26 <arg name="alice_kinematics" value="$(arg alice_kinematics)"/>
27 </include>
If you want, ignore the two arguments passed to this launchfile for now. See Bonus: Use correct robot calibration with dual_robot setup for details.
To start the drivers we again need to watch out for name clashes. Both drivers start the same
controllers which are using the same topics to communicate. To avoid clashes, we will start each
driver in a separate namespace. We use the ur_control.launch
launchfile from ur_robot_driver
for that:
29 <group ns="alice">
30 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
31
32 <include file="$(find ur_robot_driver)/launch/ur_control.launch">
33 <arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
34 <arg name="controller_config_file" value="$(arg alice_controller_config_file)"/>
35 <arg name="robot_ip" value="$(arg alice_ip)"/>
36 <arg name="reverse_port" value="$(arg alice_reverse_port)"/>
37 <arg name="script_sender_port" value="$(arg alice_script_sender_port)"/>
38 <arg name="trajectory_port" value="$(arg alice_trajectory_port)"/>
39 <arg name="kinematics_config" value="$(arg alice_kinematics)"/>
40 <arg name="tf_prefix" value="alice_"/>
41 <arg name="controllers" value="$(arg controllers)"/>
42 <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
43 </include>
44 </group>
With this, each robot will have its joint_state_controller
running inside its namespace, meaning
that the joint_states
topic will be inside the respecive namespaces, namely
/alice/joint_states
and /bob/joint_states
. Therefore, we start a robot_state_publisher
(that will convert joint_states
messages into TF messages to produce up-to-date poses of each
link). This could also be done differently, e.g. by having one robot_state_publisher
in the
top-level namespace and adding a joint_state_publisher that collects the two topics from their
namespaces into /joint_states
. If you wish to have a /joint_states
topic, you might want to
take that route.
Run the demo
Now, we’ve got everyting together to actually run the full demo. For running the demo you will need a working Docker setup as explained in the Installation.
For startup you’ll need two shells: One for starting two simulated robots using docker + ursim and one for the ROS components.
In the first shell execute
rosrun ur_example_dual_robot docker_alice_bob.sh
This will use docker-compose to start two docker containers running a simulated robot. Wait, until the robots are started up. You can connect to the robots using their web interface:
Once the robots have booted, start them as you would with a normal robot using the red button in the lower left corner:
When the robots have booted, start the driver instances as follows
roslaunch ur_example_dual_robot dual_robot_startup.launch
This should startup the drivers, an RViz instance and an rqt_joint_trajectory_controller window.
The last thing we need to do is to start the external_control
program on the robots. They have
the respective URCap already installed and a program created and loaded. Simply press the play
button on each robot to start external control from ROS.
In the shell running the drivers, you should now see Robot connected to reverse interface.
Ready to receive control commands.
twice.
Using the rqt_joint_trajectory_controller window you can select one of the robots (1), select the controller to use (2, it should just be one) click on the big red button (3, it will turn green as shown in the image) and then use the sliders to move the robots around.