UP | HOME

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++

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.
  • 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 limits
      • prismatic: a 1-DOF translational joint with limits (specified by a <limit> element)
      • fixed: A transformation that does not change between two links
      • floating: A joint where all 6 DOF can move
      • planar: 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 fixed x, y, and z axes).
    • 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 the pr2_controller_manager and the ros_control packages.
  • 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
  • Gazebo (the primary ROS simulation tool) uses SDF files, but can convert URDF files to SDF automatically
    • Many tags in a URDF file are mainly used when Gazebo converts to SDF.
  • 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

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 the robot_description parameter to get the robot's kinematics and geometry)
      • Changes to the robot_description parameter will be published on the robot_description topic
    • 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
  • 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:

  1. Reads sensor_msgs/JointState messages from topics specified by source_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
  2. 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
  3. Mimic mode: publishes some joint states that follow the motion of other joints.
  4. The joint state publisher reads the robot_description topic so it knows the names of the joints to publish
  5. 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
  6. In typical operation, nodes connected to hardware publish joint states. However, joint_state_publisher is useful for combining different sources of joint state information

Debugging URDF hints

  1. Build up the URDF slowly. Test each time you add a new link or joint.
  2. Create a Launchfile to help with testing. This launchfile will likely evolve or parts of it will be used in your actual application
    1. It should run the robot_state_publisher to read your urdf file.
    2. It should run the joint_state_publihser with _use_gui:=true.
    3. It should launch rviz (and optionally) load an appropriate .rviz configuration file.
    4. 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.
  3. 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.
  4. If you name the first link of your robot "base_link" you can roslaunch urdf_tutorial display.launch model:=<path/to/urdf> to view your urdf.
    • If the first link is not called "base_link" you will need to change the fixed frame in rviz

Python Launchfile

  1. Remember to install the urdf file in setup.py
  2. Use a Node Action, along with the Command, PathJointSubsitution, ExecutableInPackage, and FindPackageShare 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

    1. Starts a node using the robot_state_publisher executable in the robot_state_publisher package
    2. Loads the robot_state_publisher robot_description parameter with the contents of the urdf created by running xacro on my.urdf.xacro
    3. 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 the xacro executable (2nd argument) within the xacro package (1st argument)
      • We add a space (" ") to separate the xacro command from its arguments
      • FindPackageShare() finds the share directory of mypackage
      • PathJoinSubstitution() concatenates the mypackage share directory with the location of the xacro file
    1. See Launch Substitutions for details.

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.
  • 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 other xacro 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.
  • xacro files can also be converted into urdf files at compile time via CMakeLists.txt.
    • First, find_package(xacro REQUIRED)
    • Then, xacro_add_files(myxacrofiles TARGET xacro_files)
    • To see updates to the urdf file you need to run catkin_make
  • How to introduce Xacro into a URDF
  • 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 Properties

  • Use the <xacro:property name="name" value="value"> to define a constant in xacro
    • 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']}" />
      
  • In ROS2, yaml files often start with the node name (or a pattern) and the ros__parameters keyword, as these parameters are associated with individual nodes

    node_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.

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 other xacro files using <xacro:include filename = "$(find package)/urdf/myfile.xacro">
  • Build your own library of useful xacro macros.
  • Keep the gazebo tags for your urdf in a separate file and include it. Thus you can maintain a clean separation between the gazebo and non-gazebo parts of your file.

Xacro If

  • Conditionally include a block using <xacro:if value = "expression">

Xacro Arguments

  • Pass arguments to xacro using xacro 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"/>

Author: Matthew Elwin