Robot Modeling
Unified Robot Description Format
urdf
is the main description format for robots in ROS- It is an XML format with special tags for defining a robot
- The urdf ROS package that enables the file to be parsed from C++
- The URDF Tutorials introduce this file format interactively.
URDF XML
The urdf XML specification contains nine sub-specifications. Many of the specifications are not used or only used with specific packages. The important specifications are described below.
- model - This specification provides a general guide to the
.urdf
format. - robot - The
<robot>
element is the top-level element in all urdf files. It contains links and joints. - link - Describes properties of a rigid-body in your robot
- Has a
name
attribute to name the link. <inertial>
Optionally specify mass and rotational inertia properties of the link.<visual>
Optionally describes how to draw the link. There are basic primitives or you can use a 3D model from CAD.<geometry>
specifies the shape of the object. Can be<box>
,<cylinder>
,<sphere>
or<mesh>
.<material>
specifies the color of the object
<collision>
Optionally describe geometry used for collision detection. Used by motion planning and simulation tools.
- Has a
- joint - Connects links together with various types of joint freedoms.
- Has a
name
attribute to name the joint. - Has a
type
attribute which determines what freedoms and constraints are imposed on the joint.revolute
: a 1-degree of freedom (DOF) rotational joint with upper and lower joint limits (specified by a<limit>
element)continuous
: a 1-DOF rotational joint without limitsprismatic
: a 1-DOF translational joint with limits (specified by a<limit>
element)fixed
: A transformation that does not change between two linksfloating
: A joint where all 6 DOF can moveplanar
: A 2-DOF joint allowing translation in the plane
- Joints have a
<parent>
link and a<child>
link and determine where the<child>
link is relative to the parent. <origin>
specifies the location of the child in the parent link frame- The child frame is translated by
xyz
- The child frame is rotated by
rpy
, roll ,pitch, yaw (which are rotations about the fixedx
,y
, andz
axes).
- The child frame is translated by
- Other joints can be created by locating multiple joints at the same location. For example: a universal joint is two co-located
continuous
joints. Creating such joints may require using "dummy" links that lack inertial, visualization, or collision properties. - There are many optional tags. The
<safety_controller>
tag is only used by thepr2_controller_manager
and theros_control
packages.
- Has a
- gazebo Gazebo extensions to URDF files, which it uses when converting to its
SDF
file format. For more information see Gazebo Tutorial for Using URDF. - Go through the ROS URDF Tutorials to learn more.
- Even though the documentations linked here are for ROS 1, the specification is the same for ROS 2
Related Formats
There are other formats used in robotics (not just ROS) that relate to URDF files.
- Kinematics and Dynamics Library (KDL)
- The kdl_parser converts
urdf
to a format used by KDL
- The kdl_parser converts
- Gazebo (the primary ROS simulation tool) uses
SDF
files, but can convertURDF
files toSDF
automatically- Many tags in a URDF file are mainly used when Gazebo converts to
SDF
.
- Many tags in a URDF file are mainly used when Gazebo converts to
- Collada is a generic 3D file format.
- SolidWorks is a 3D computer aided design (CAD) program.
- If using solidworks (or other CAD program) it often makes sense to export models as
.vrml
files, edit them in Blender to reduce the polygon count for visualization purposes
- If using solidworks (or other CAD program) it often makes sense to export models as
robot_state_publisher
The robot_state_publisher is used to publish the transforms that compose your urdf
file on /tf
and
integrate it with the transform tree.
- The
robot_state_publisher
performs forward kinematics in ROS- It reads
urdf
data (as specified by therobot_description
parameter to get the robot's kinematics and geometry)- Changes to the
robot_description
parameter will be published on therobot_description
topic
- Changes to the
- It listens on the
joint_states
topic to get (sensor_msgs/msg/JointState)- Each message describes the position, velocity and effort (torque/force) of the joints
- See documentation about the sensor_msgs packages
- It uses the
sensor_msgs/JointState
data to publish transforms on/tf
that describe the positions of the robot's links. - It publishes the
urdf
to the/robot_description
topic - Frequency at which tfs are published are controlled by a parameter to the node
- Almost all robots in ROS run a
robot_state_publisher
- It reads
- The
joint_state
messages typically come from sensors (such as encoders) on the robot, but can also be generated by other nodes, which is useful for testing the robot model without needing the physical robot.
joint_state_publisher
The joint_state_publisher publishes sensor_msgs/JointState
messages on the joint_states
topic.
It operates in a few different modes:
- Reads
sensor_msgs/JointState
messages from topics specified bysource_list
and publishes the combined information on a single topic- Useful if various sensor nodes are providing readings and you want to use
robot_state_publisher
to show transforms for the whole system
- Useful if various sensor nodes are providing readings and you want to use
- Default value: publishes a fixed default value for the joint.
- Useful for keeping the
/tf
tree in a valid state when other parts of your system are not running. - Used as a stand-in for the actual robot during visualization tasks
- Useful for keeping the
- Mimic mode: publishes some joint states that follow the motion of other joints.
- The joint state publisher reads the
robot_description
topic so it knows the names of the joints to publish - The
joint_state_publisher_gui
node creates a user interface that allows a user to manipulate the joint states that are published- This gui does not actually control a robot, rather it is a way of simulating fake encoder data in an interactive manner
- In typical operation, nodes connected to hardware publish joint states. However,
joint_state_publisher
is useful for combining different sources of joint state information. - The
/joint_states
topic andsensor_msgs/JointState
messages are for sensor data (that is measurements of the joint states). They are not typically used to control the joint states.
Debugging URDF hints
- Build up the URDF slowly. Test each time you add a new link or joint.
- Create a Launchfile to help with testing. This launchfile will likely evolve or parts of it will be used in your actual application
- It should run the
robot_state_publisher
to read yoururdf
file. - It should run the
joint_state_publihser_gui
by default, to allow you to manipulate the joints - It should launch
rviz
(and optionally) load an appropriate.rviz
configuration file. rviz
needs to be configured to- Have the proper fixed_frame.
- Add the "RobotModel" view to see the links of your robot
- Add the "tf" view to your rviz configuration.
- It should run the
- This setup allows you to run the launchfile and visualize the robot. You can also change the joint angles easily to see how it moves.
- If you name the first link of your robot "base_link" you can
ros2 launch urdf_tutorial display.launch model:=<path/to/urdf>
to view your urdf.- The
<path/to/urdf>
is relative to theurdf_tutorials
installation directory. Use an absolute path for your urdf file. - If the first link is not called "base_link" you will need to change the fixed frame in rviz manually
- The
Python Launchfile
- Remember to install the
urdf
file insetup.py
- Use a
Node
Action, along with theCommand
,PathJointSubsitution
,ExecutableInPackage
, andFindPackageShare
substitutions
Node( package="robot_state_publisher", executable="robot_state_publisher", parameters=[ {"robot_description" : Command([ExecutableInPackage("xacro", "xacro"), " ", PathJoinSubstitution( [FindPackageShare("mypackage"), "my.urdf.xacro"])])} ] ),
This Action
- Starts a node using the
robot_state_publisher
executable in therobot_state_publisher
package - Loads the
robot_state_publisher
robot_description
parameter with the contents of theurdf
created by runningxacro
onmy.urdf.xacro
- The
Command
substitution executes whatever string is created from concatenating elements in its list argument and substitutes it with the output of the command.- So we are essentially building a command to run whose output will be filling the
robot_description
ExecutableInPackage()
finds thexacro
executable (2nd argument) within thexacro
package (1st argument)- We add a space (
" "
) to separate the xacro command from its arguments FindPackageShare()
finds the share directory ofmypackage
PathJoinSubstitution()
concatenates themypackage
share directory with the location of the xacro file
- So we are essentially building a command to run whose output will be filling the
- See Launch Substitutions for details.
- Starts a node using the
XML Launchfile
<node pkg="robot_state_publisher" exec="robot_state_publisher"> <param name="robot_description" value="$(command '$(exec-in-pkg xacro xacro) $(find-pkg-share pkg)/my.urdf.xacro')" /> </node>
Example
An example package that creates a URDF file and launches rviz to display it can be found at https://github.com/m-elwin/me495_urdf
xacro
- xacro is a macro language for XML, designed to make creating URDF files easier.
- Xacro Tutorial
- Variables can store values and basic math can be performed
- Blocks of
xml
code can be given names and used multiple times xacro
files can include otherxacro
files.
For example, the Baxter robot has two identical arms. With URDF you would
need to define each of those arms separately, even though they have the same structure.
Using xacro
you could create a macro for a Baxter arm and then use that
macro to create two instances of the arm.
- Most robots use
xacro
files instead of urdf files. - The
xacro
package contains axacro
executable that parsesxacro
files and producesurdf
files.- All
urdf
files are validxacro
files.xacro
can then be used as an add-on to theurdf
- All
xacro
files can also be converted intourdf
files at compile time viaCMakeLists.txt
.- First,
find_package(xacro REQUIRED)
- Then,
xacro_add_files(myxacrofiles TARGET xacro_files)
- To see updates to the
urdf
file you need to runcatkin_make
- First,
- Using
xacro
effectively makes your robot description much more robust and easy to change because it cuts down on repetition and makes your file more modular.
Xacro Robot
- The top-level tag in
urdf
files is the<robot>
tag. - To indicate that you want to use
xacro
add thexmlns:xacro attribute
to<robot>
(e.g.,<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
)
Xacro Properties
- Use the
<xacro:property name="name" value="value">
to define a constant inxacro
- Basic constants (like
pi
) are already defined. - You can use any python expression anywhere using the
${}
syntax It often makes sense to load non-changeable robot parameters directly from a yaml file
<!-- Get the path to the yaml file relative to the package --> <!-- config.yaml should have been installed in setup.py --> <xacro:property name="yaml_file" value="$(find package)/config.yaml" /> <!-- load values from yaml file which are stored in a dictionary called props--> <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" /> <!-- access an individual variable --> <xacro:property name="myprop" value="${props['myprop']}" />
- Basic constants (like
In ROS2,
yaml
files often start with the node name (or a pattern) and theros__parameters
keyword, as these parameters are associated with individual nodesnode_name_or_wildcards: ros__parameters:
- When loading the yaml file from a
xacro
file, you can access the elements by indexing: for example:<xacro:property name="props" value="${xacro.load_yaml(yaml_file)['/**']['ros__parameters']}" />
will retrieve a dictionary of the parameters that you actually wish to use.
- When loading the yaml file from a
Xacro Macros
- Macro's are the most useful tools when creating
urdf
files for eventual use with gazebo A common case: you want the visual, inertial, and collision elements of a link to correspond to each other:
<!-- Create a link with visual, inertial, and collision for a z-axis aligned cylinder. Also enables specifying a material block. Assumes the default cylinder orientation --> <xacro:macro name="cylinder_link" params="name length radius mass *material"> <link name="${name}"> <visual> <geometry> <cylinder length="${length}" radius="${radius}" /> </geometry> <xacro:insert_block name="material" /> </visual> <collision> <geometry> <cylinder length="${length}" radius="${radius}" /> </geometry> </collision> <inertial> <mass value="${mass}"/> <inertia ixx="${(1.0/12.0)*mass*(3*radius**2 + length**2)}" ixy="0" ixz="0" iyy="${(1.0/12.0)*mass*(3*radius**2 + length**2)}" iyz="0" izz="${0.5*mass*radius**2}" /> </inertial> </link> </xacro:macro>
Xacro Include
- You can include
xacro
files in otherxacro
files using<xacro:include filename = "$(find package)/urdf/myfile.xacro">
- Build your own library of useful
xacro
macros. - Keep the
gazebo
tags for yoururdf
in a separate file and include it. Thus you can maintain a clean separation between thegazebo
and non-gazebo parts of your file.
Xacro If
- Conditionally include a block using
<xacro:if value = "expression">
Xacro Arguments
- Pass arguments to
xacro
usingxacro model variable:=value
syntax.. - Access arguments within
xacro
using$(arg name)
- Declare that an argument is expected and provide a default value with
<xacro:arg name="argname" default="default"/>