After last time’s venture into Kalman filtering, today a hands-on subject: KDL - the reference inverse kinematics library in robotics.

Introduction

Forward and inverse kinematics challenges - such as Given these joint velocities, what’s my endpoint velocity? and The endpoint should be in position x in cartesian space, what should the joint angles be? - pop up sooner or later, perhaps in a robot arm for factory automation, or in a cute quadruped such as Boston Dynamics’ Spot. KDL, or in full the Orocos Kinematics and Dynamics Library, provides a toolbox for solving these. It’s is a popular choice for these tasks, and underpins state-of-the-art motion planning frameworks such as MoveIt. ROS 2 integration is fairly straightforward, as not only is it available as the orocos_kdl_vendor ROS 2 package, it also supports typical ROS formats such as URDF.

In this tutorial/blogpost, we will go through constructing the KDL kinematic tree from the /robot_description topic (i.e. using URDF), extracting the kinematic chain and instantiating a inverse kinematics (IK) solver, and finish by solving an inverse kinematics case.

Let’s go!

The full code of this tutorial is available here as a ROS 2 package, where you’ll find the full source file. It’s suggested to keep the latter open while following along.

Construct a KDL::Tree from the URDF on /robot_description

The kinematic tree expresses the kinematic structure of the system as a graph of joints and segments. Information-wise it’s similar to the URDF format, and as such it is preferable to construct the tree from the robot’s URDF (instead of redefining it in the source code). There are two common approaches to do so - the first is to load it directly from the URDF file (using kdl_parser::treeFromFile), the second is to subscribe to the /robot_description topic, which broadcasts the URDF as a string. In this tutorial, we’ll go with the latter.

Let’s start with the subscription:

subscription_ = this->create_subscription<std_msgs::msg::String>(
  "robot_description",
  rclcpp::QoS(rclcpp::KeepLast(1))
    .durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL),
  std::bind(&LegIK::robotDescriptionCallback, this, _1));

This subscription definition is longer than usual because the quality of service (QoS) durability is set to transient local - this means that the publishing node (i.e. the robot_state_publisher node, in this case) is asked to retransmit previously published messages. If this would be omitted, the callback would only be called when a new URDF is published.

Upon receiving a URDF string, the method robotDescriptionCallback is called, which stores the tree into the member variable tree_:

//!
//! Construct KDL IK solver from URDF string
void robotDescriptionCallback(const std_msgs::msg::String& msg)
{
  // Construct KDL tree from URDF
  const std::string urdf = msg.data;
  kdl_parser::treeFromString(urdf, tree_);
  // Print basic information about the tree
  std::cout << "nb joints:        " << tree_.getNrOfJoints() << std::endl;
  std::cout << "nb segments:      " << tree_.getNrOfSegments() << std::endl;
  std::cout << "root segment:     " << tree_.getRootSegment()->first
            << std::endl;

A KDL::Joint has a similar interpretation as a URDF joint (although it omits joint limits). A KDL::Segment, on the other hand, corresponds best with a URDF link.

The URDF used in this example is pretty simple, it has three links and two revolute joints and represents a simplified humanoid leg. It’s structure is base_link -> hip joint -> upper_leg -> knee joint -> lower_leg -> fixed joint -> foot, with the knee and the hip joint revoluting around the y-axis and both leg parts having a length of 0.5 m. As such, the foot can move in the xz-plane up to a distance of 1.0 m from the origin.

Create the inverse kinematics solver

KDL provides several inverse kinematics (IK) solvers, here we will use the Levenberg-Marquardt (LM) one as it is the most concise to use. To define the solver, it is necessary to have the kinematic chain available - this is a structure that defines the chain of segments and joints that we are interested in solving for. In this case we are interested in the chain that defines the leg, so base_link to foot:

  // Get kinematic chain of the leg
  tree_.getChain("base_link", "foot", chain_);

Next and final step is then to initialize the solver:

  // Create IK solver
  solver_ = std::make_unique<KDL::ChainIkSolverPos_LMA>(chain_);

Run the inverse kinematics solver

Running the IK solver is done by calling its CartToJnt method; as it takes KDL types, we write a small wrapper method that takes cartesian coordinates (x, z) and writes out the hip and knee joint angles:

//!
//! Cartesian x, z => hip and knee joint angles
void getJointAngles(
  const double x, const double z, double & hip_angle, double & knee_angle)
{
  // Prepare IK solver input variables
  KDL::JntArray q_init(chain_.getNrOfJoints());
  q_init(0) = -0.1;
  q_init(1) = 0.2;
  const KDL::Frame p_in(KDL::Vector(x, 0, z));
  KDL::JntArray q_out(chain_.getNrOfJoints());
  // Run IK solver
  solver_->CartToJnt(q_init, p_in, q_out);
  // Write out
  hip_angle = q_out(0);
  knee_angle = q_out(1);
}

The solver is initialized with joint angles (-0.1, 0.2). Initializing the joints such that they are not aligned tends to result in faster convergence.

To finish this tutorial, the method getJointAngles is called to do the conversion. Example conversion from {x: 0.3, z: -0.6} m to joint angles:

getJointAngles(0.3, -0.6, hip_angle, knee_angle);

which writes -74.4 ° and 95.7 ° to respectively the hip and the knee angle.

Conclusion

In this tutorial, we dipped our toes in inverse kinematics with KDL and ROS 2. Starting with constructing the kinematic tree from the URDF, the kinematic chain of interest was extracted, the inverse kinematics (IK) solver was created and subsequently a basic IK problem was solved.

If you go further, the most useful KDL documentation is the API reference and the code repository. Next to that, there’s also the Orocos documentation. This tutorial just scratches the surface of what KDL is capable of, and in its codebase you’ll find several (inverse) kinematics solvers, with for example support to handle joint limits, underdetermined kinematic systems (more DoF than strictly required), as well as much more.

Liked it, or any questions? Share in the comments below!