URDF, Kinematics, and KDL
The goal of this post was to plan for the transition the robot sim from using DH parameters which are very clunky and limited, to an expressive format like URDF; the inspiration for this change is KDL. This post is currently very much not done, but I decided to push it anyway because I got sidetracked and am not sure when I’ll have time to return to it.
In ROS, kinematics are handled as follows. The URDF (XML) file is first loaded into one big string parameter called “robot_description” in the ROS parameter server, and then the robot_state_publisher node is used to populate the transform tree.
Within the robot_state_publisher node, the string is first used initialize a URDF model with the parseURDF() function. This basically crawls through the XML string and pulls out the various elements (links, joints, materials, etc) and stores them in a urdf::ModelInterface object (or maybe actually a urdf::Model object instead? Model derives from ModelInterface, but I didn’t look into the distinctions). Along the way, parseURDF() also builds a “tree” as a std::map<std::string, std::string> object to store link relationships using the initTree() function. The root link of the tree is then initialized and stored in the returned model.
Once the model has been initialized, it’s used to build a KDL tree using the kdl_parser package’s functionality. There are several functions to build the KDL tree directly from a string or parameter here as well, but they all first convert to a urdf::Model as above and then pass the model to the treeFromUrdfModel() function. This initializes a KDL kinematic tree and then recursively walks the tree to populate it with children. (NOTE: this is very similar to what we saw the pytorch_kinematics library does). Note that KDL has great documentation explaining how it relates Joint, Segment, Chain, and Tree objects under the hood, with diagrams.
Popping off the stack trace as it were back to robot_state_publisher, we see that a robot_state_publisher::JointStateListener object actually gets created from the KDL tree and URDF model (and something weird called a mimic map?), which in turn creates the actual core RobotStatePublisher object as a shared pointer. In the RSP’s constructor, we see that the KDL tree’s segments are iterated over and added into maps of strings to SegmentPairs, where SegmentPair is a simple class to hold the KDL segment itself with the “root” and “tip” names which define it. With the RSP defined inside the JointStateListener, the listener subscribes to the “joint_states” ROS message which is assumed to be published by some other node which reads the joint values. In the joint state message callback, the listener checks the joint states before finally passing the joint state vector to RobotStatePublisher::publishTransforms(). This function uses the joint state name to look up the appropriate SegmentPair, and pass the joint state value to its KDL::Segment member, returning the pose of that segment’s “tip” in its “root” frame; this is then converted from a KDL::Frame to a TF2 transform stamped message using the tf2::kdlToTransform() conversion function. The transform for each segment is pushed back into the tf tree, and broadcasted all at once. Note that for “fixed” transforms (those which do not change with changes in the joint state, e.g., rigid sensor mounts) there is a separate RSP function to compute them (passing zero as the joint angle to the KDL::Segment for each) and broadcast.
That’s it! So now you know how ROS parses a URDF all the way from XML to tf2 transform messages. It’s important to note that the tf2 library doesn’t actually deal with computing the transforms – this is done entirely by the RSP and underlying KDL functionality. Instead, tf2 is just managing the transforms and buffering them, exposing them to the user via answering frame transform queries between specific links at specific times, supplying the tree to RViz for robot visualization, etc. Of course, you can compute and broadcast transforms yourself with tf2 instead of using the URDF to KDL pipeline via the RSP, and in some cases you may want to do this in order to override transform values.
Written on September 4th, 2021 by Nick Rotella