Robot Modeling
Unified Robot Description Format
urdfis 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 
.urdfformat. - 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 
nameattribute 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 
nameattribute to name the joint. - Has a 
typeattribute 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, andzaxes). 
- 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 
continuousjoints. 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_managerand theros_controlpackages. 
 - Has a 
 - gazebo Gazebo extensions to URDF files, which it uses when converting to its 
SDFfile 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
 
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_publisherperforms forward kinematics in ROS- It reads 
urdfdata (as specified by therobot_descriptionparameter to get the robot's kinematics and geometry)- Changes to the 
robot_descriptionparameter will be published on therobot_descriptiontopic 
 - Changes to the 
 - It listens on the 
joint_statestopic 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/JointStatedata to publish transforms on/tfthat describe the positions of the robot's links. - It publishes the 
urdfto the/robot_descriptiontopic - 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_statemessages 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/JointStatemessages from topics specified bysource_listand publishes the combined information on a single topic- Useful if various sensor nodes are providing readings and you want to use 
robot_state_publisherto 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 
/tftree 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_descriptiontopic so it knows the names of the joints to publish - The 
joint_state_publisher_guinode 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_publisheris useful for combining different sources of joint state information. - The 
/joint_statestopic andsensor_msgs/JointStatemessages 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_publisherto read yoururdffile. - It should run the 
joint_state_publihser_guiby default, to allow you to manipulate the joints - It should launch 
rviz(and optionally) load an appropriate.rvizconfiguration file. rvizneeds 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_tutorialsinstallation 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 
urdffile insetup.py - Use a 
NodeAction, along with theCommand,PathJointSubsitution,ExecutableInPackage, andFindPackageSharesubstitutions 
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_publisherexecutable in therobot_state_publisherpackage - Loads the 
robot_state_publisherrobot_descriptionparameter with the contents of theurdfcreated by runningxacroonmy.urdf.xacro - The 
Commandsubstitution 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 thexacroexecutable (2nd argument) within thexacropackage (1st argument)- We add a space (
" ") to separate the xacro command from its arguments FindPackageShare()finds the share directory ofmypackagePathJoinSubstitution()concatenates themypackageshare 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 
xmlcode can be given names and used multiple times xacrofiles can include otherxacrofiles.
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 
xacrofiles instead of urdf files. - The 
xacropackage contains axacroexecutable that parsesxacrofiles and producesurdffiles.- All 
urdffiles are validxacrofiles.xacrocan then be used as an add-on to theurdf 
 - All 
 xacrofiles can also be converted intourdffiles at compile time viaCMakeLists.txt.- First, 
find_package(xacro REQUIRED) - Then, 
xacro_add_files(myxacrofiles TARGET xacro_files) - To see updates to the 
urdffile you need to runcatkin_make 
- First, 
 - Using 
xacroeffectively 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 
urdffiles is the<robot>tag. - To indicate that you want to use 
xacroadd thexmlns:xacro attributeto<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,
yamlfiles often start with the node name (or a pattern) and theros__parameterskeyword, as these parameters are associated with individual nodesnode_name_or_wildcards: ros__parameters:
- When loading the yaml file from a 
xacrofile, 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 
urdffiles 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 
xacrofiles in otherxacrofiles using<xacro:include filename = "$(find package)/urdf/myfile.xacro"> - Build your own library of useful 
xacromacros. - Keep the 
gazebotags for yoururdfin a separate file and include it. Thus you can maintain a clean separation between thegazeboand non-gazebo parts of your file. 
Xacro If
- Conditionally include a block using 
<xacro:if value = "expression"> 
Xacro Arguments
- Pass arguments to 
xacrousingxacro model variable:=valuesyntax.. - Access arguments within 
xacrousing$(arg name) - Declare that an argument is expected and provide a default value with 
<xacro:arg name="argname" default="default"/>