How to set up KDL (Kinematics and Dynamics Library) for Solving Kinematics of Serial Chain Manipulators in C++ with ROS2
- Karan Bhakuni
- Oct 2
- 11 min read
Updated: Oct 6
Introduction
If you’re a beginner with serial-chain manipulators, one thing you’ll struggle with early on is writing the kinematics — especially once you move to 6-DOF and 7-DOF robots. It’s not straightforward to get solutions for such complicated serial chains. Broadly, there are two ways to solve kinematics for manipulators:
Analytical method — This gives you a closed-form solution when the system is exactly determined (number of DOF matches the number of constraints). That’s typically the case for many 6-DOF industrial arms. Analytical solutions are great because they are exact, but they’re also hard to derive and only exist for certain robot geometries (think PUMA-style arms, spherical wrists, etc.). For robots with more than 6 DOF, you must add extra constraints (for example, a desired elbow posture or a particular joint value) to pick a single solution from the infinite family — and even then deriving a closed form can be painful.
Numerical methods — These give approximate solutions based on constraints and an initial guess. The main advantage is practicality: if you choose reasonable constraints and a decent initial guess, numerical solvers will give you a working solution. Typical numerical approaches include Jacobian pseudo-inverse, damped least squares (DLS), Newton-Raphson, and Levenberg–Marquardt. (Note: Recursive Newton–Euler, or RNE, is a method for dynamics/inverse dynamics — not an IK solver — so keep that separate.) Numerical methods can fail or stall if your initial guess is poor, if you end up near a singularity, or if the solver falls into a local minimum.
Orocos (specifically the Kinematics and Dynamics Library — KDL) provides tools to handle kinematics and dynamics for chained manipulators. The workflow is simple:
Build a chain between your chosen start link and end link from the robot’s URDF.
KDL assigns appropriate FK and IK solvers to that chain.
For forward kinematics, you give joint angles and get the end-effector pose.
For inverse kinematics, you supply a Cartesian target (and usually the current joint angles or a guess) and KDL iteratively finds joint values that reach that target.
Pre-Requisites for KDL
Before diving into KDL, it helps to understand some of the key concepts it uses. First, there’s the tree. You can think of the tree as a full representation of your robot — it contains all the links and joints, showing how everything is connected from the base to every end-effector, gripper, or tool. The tree gives a global view, but often we only care about one specific arm or manipulator, which is where the chain comes in. A chain is basically a path through the tree from a starting link to an ending link, including all the joints along the way. That’s the part of the robot you’ll use for kinematics calculations.
Within a chain, there are segments. A segment is a single piece of the chain and usually represents a rigid link plus the joint that connects it to the next link. Each segment has a joint, and the joint can be different types — revolute, prismatic, or fixed. Segments are what the FK and IK solvers step through when they calculate motion.
Then you have joints, which are pretty much what you expect: they define how two links can move relative to each other. Joints can rotate, slide, or stay fixed, and they are what you’ll actually command when moving the robot.
Finally, there are solvers. Solvers are the tools KDL provides to calculate kinematics. Forward kinematics solvers take joint angles and tell you where the end-effector is in space. Inverse kinematics solvers work the other way — given a desired end-effector pose, they try to find the joint angles that achieve it. Some solvers work iteratively and rely on an initial guess, while others can give exact solutions if the robot geometry allows it.
Step-By-Step process for solving kinematics using KDL
When working with arms like a UR5, Franka Emika Panda, or a custom robot, the first requirement is to set up the kinematic chain: parse the URDF, build the KDL chain, and prepare FK/IK solvers. Below, I’ll walk through that process in detail.
A) Decide on the Robot Type and Define Links
The first step is to decide which robot you’re targeting. Different robots use different naming conventions for their URDF links. Technically, it’s not the robot that matters — it’s the URDF links defined in your URDF file. So, I’ll show a glimpse of my URDF file below(If you want to learn how to make urdf for your custom robot you can visit Understanding URDF for Manipulators).
B) URDF to KDL Tree
The URDF (Unified Robot Description Format) describes the robot’s joints, links, and their relationships. To use KDL, you first convert this URDF string into a KDL tree.
URDF → KDL tree: The treeFromString function parses the URDF XML string into a hierarchical structure.
So basically this functions reads urdf and make a tree from there. A tree is something with child-parent relationship.
kdl_parser::treeFromString(urdf, tree_);
Here urdf is a string coming from robot description.
Below is the output from tree_.
========== KDL Tree Structure ==========
Root: base_link
Segment: base_link
Joint: NoName (Fixed)
Segment: l0
Joint: j0 (Fixed)
Segment: l1
Joint: j1 (RotAxis)
Axis: [0, 0, 1]
Segment: l2
Joint: j_dummy_1_2 (Fixed)
Segment: l3
Joint: j2 (RotAxis)
Axis: [1, 0, 2.22045e-16]
Segment: l4
Joint: j3 (RotAxis)
Axis: [0, 0, 1]
Segment: l5
Joint: j_dummy (Fixed)
Segment: l6
Joint: j4 (RotAxis)
Axis: [0, 0, 1]
Segment: le
Joint: j5 (RotAxis)
Axis: [-1, 2.44929e-16,2.2e-16]
Segment: le_1
Joint: j6 (RotAxis)
Axis: [1, 0, 2.22045e-16]
========================================kdl_parser::treeFromString(urdf, tree_)Failure modes: If the URDF is invalid (missing joints, bad formatting, etc.), you’ll hit the error here. Always double-check URDFs with check_urdf before using them.
C) KDL Tree to KDL Chain
Understand that KDL needs a starting and ending link to create the kinematic chain. Now you might be wodering what exactly is a kinematic chain. A simple definition that can also get in Wikipedia is kinematic chain is an assembly of rigid bodies connected by joints to provide constrained motion. But today i will hsow you how it looks like for a serial manipulator.
Also If you design your own robot, make sure your URDF uses consistent link names, otherwise the chain extraction will fail.
You can see below that i starting link in my urdf is base_link and the end link is le_1.
<!-- Fixed Link -->
<link name="base_link"></link>
.
.
.
.
.
<link name="le_1">
<inertial>
<origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
<mass value="1e+00"/>
inertia>
<visual>
<geometry>
<cylinder radius="0.03" length="0.1"/>
</geometry>
<origin xyz="0.0 0.0 0.05 " rpy="0.0 0.0 0.0"/>
<material name="gray"/>
</link>This is how i will define it in my code:
start_link_ = "base_link";
end_link_ = "le_1";
tree_.getChain(start_link_, end_link_, chain_);So how it works basically is the getChain function of KDL library will build a chain from start link to end link and it will update that in chain_ variable which is basically passed as reference.
Remember: Most of the functions in KDL takes arguments by reference so you will pass an empty variable of same type and it gets updated with the value. 5. This is how the chain looks like after we use this functions:
Parsed URDF to KDL tree.
Chain Structure: l0 -> l1 -> l2 -> l3 -> l4 -> l5 -> l6 -> le -> le_1
Kinematic Chain from base_link to le_1:
Segment 0: l0
Joint Name: j0
Joint Type: Fixed
Segment 1: l1
Joint Name: j1
Joint Type: RotAxis
Segment 2: l2
Joint Name: j_dummy_1_2
Joint Type: Fixed
Segment 3: l3
Joint Name: j2
Joint Type: RotAxis
Segment 4: l4
Joint Name: j3
Joint Type: RotAxis
Segment 5: l5
Joint Name: j_dummy
Joint Type: Fixed
Segment 6: l6
Joint Name: j4
Joint Type: RotAxis
Segment 7: le
Joint Name: j5
Joint Type: RotAxis
Segment 8: le_1
Joint Name: j6
Joint Type: RotAxis
You can clearly see above that we got different segments from the base to the last link. Also, at the start, you can see the chain structure. You can observe that it has also retrieved the information of joints from the URDF, so you have to make sure you are using the correct URDF and that all the link and joint names are consistent with it. But a question might come to your mind: Why chains:Because a tree could describe the entire robot (including grippers, cameras, etc.). The chain isolates the specific manipulator arm you care about.
D) Collect the Ordered Joint Names and Skip Fixed Joints
Now, one of the most important parts, according to me, in this entire process is making an ordered list of joint names and skipping the fixed joints. In most cases, the joint order for KDL and the order in which you want to publish to your robot in your custom topic might be the same or might differ, but we have to make sure we create an order such that we are 100% sure we are publishing the correct value to the correct joint. For that, we will extract the joints one by one from the kinematic chain, and while doing this process, I will skip fixed segments, which will be determined based on the type of joint in each segment. Then I will take only the joints that are actuated and skip the rest. You can see the output below for my custom robot:
Ordered joint names:
- Skipped fixed segment: l0
- j1
- Skipped fixed segment: l2
- j2
- j3
- Skipped fixed segment: l5
- j4
- j5
- j6
Number of joints: 6
E) Set Up KDL Solvers
Now, the next part is to set up the solvers to solve the kinematics for my robot. There are two types of kinematics for serial chain manipulators, i.e., forward and inverse kinematics. We will set up the solvers for both:
fk_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain_);
ik_vel_solver_ = std::make_shared<KDL::ChainIkSolverVel_pinv>(chain_);
solver_ = std::make_unique<KDL::ChainIkSolverPos_NR>(chain_, *fk_solver_, *ik_vel_solver_, 100, 1e-6);Now lets deep dive into them to understand how these solvers work and what are their purpose.
Forward kinematics solver: Computes the end effector pose from joint angles. Its very simple and has direct mathematics , give the joint angles, and the solver computes where the end effector is in space (position + orientation).
What it does: It goes link by link, multiplying transformation matrices recursively along the kinematic chain. These transformation matrices comes from urdf as we have the information in kinematic chain.
Input: A vector of joint positions (KDL::JntArray).
Output: A KDL::Frame (which contains both rotation matrix and translation vector).
Why it’s used: It’s reliable, simple, and fast — perfect for most robot manipulators.
Alternatives: KDL doesn’t have many FK solver variants; the recursive solver is standard because FK computation is deterministic and not numerically heavy.
Inverse Kinematics Solvers in KDL: Maps Cartesian velocities to joint velocities. But it might be tricky beacasue inverse kinematics is not a one to one map. A single end effector position can come from n number of joint configurations and that why the maths of inverse kinematics is not as simple as of forward kinematics which is just sries of matrix multiplications.
KDL provides two layers of solvers for IK:
Velocity-level solvers (ChainIkSolverVel_...): Velocity IK computes how joint velocities should change to achieve a desired end-effector velocity.
KDL::ChainIkSolverVel_pinv: Uses a pseudo-inverse of the Jacobian (via SVD). Used for normal robots with clean kinematics (like UR5 or Panda). Handles redundant manipulators (more joints than needed).
KDL::ChainIkSolverVel_wdls (Weighted Damped Least Squares): Adds a damping term to avoid instability near singularities. Used when you expect singularities or noisy inputs.
Position IK Solvers (KDL::ChainIkSolverPos_...): They find the actual joint angles to reach a target pose. They usually rely on FK and IK velocity solvers together.
KDL::ChainIkSolverPos_NR: Here NR means Newton-Raphson method. Its an iterative solver that repeatedly:
Computes the FK from current joint estimate.
Finds the pose error between desired and current.
Uses the IK velocity solver to update joint values.
Stops when the error is below a threshold or after max iterations.
KDL::ChainIkSolverPos_NR_JL
Its Like ChainIkSolverPos_NR, but also enforces joint limits.
Safer for physical robots since it avoids unrealistic joint angles.
KDL::ChainIkSolverPos_LMA
Uses Levenberg–Marquardt optimization (a damped least-squares approach).
More robust and better convergence, especially for complex kinematics.
Slightly heavier computationally.
So Here’s what’s happening conceptually in my code:
The FK solver computes the current end-effector pose for given joints.
The velocity solver computes how much each joint should move (in velocity space) to reduce the pose error.
The NR position solver combines both — it iteratively updates joint angles, until the pose error is small enough.
To explore other solvers you can visit the link provided in references.
F) Solving Forward and Inverse Kinematics using above solvers
To solve the forward kinematics, we will pass the current joint angles of my robot and use the function JntToCart, through which the end effector position gets updated. While initializing the fk_solver_, I have already passed the kinematic chain to it so that it now has access to the transformation matrices of my robot.
KDL::Frame ee_pose;
KDL::JntArray joint_angles_kdl;
int result = fk_solver_->JntToCart(joint_angles_kdl, ee_pose);To solve the inverse kinematics, we have to pass the current end effector position in terms of a KDL frame and an initial guess in terms of joint angles. This initial guess is very important while using any numerical method because it helps the solution to converge. For that, I am providing the current joint angles as the initial guess. Therefore, if I have to solve for the (i+1)th end effector position, I am giving the ith joint angle as my initial guess. This makes sure that my solution always converges to the correct value and remains fast enough.
KDL::JntArray q_init;
KDL::Frame p_in(rot, trans);
KDL::JntArray q_out;
int ik_result = solver_->CartToJnt(q_init, p_in, q_out);KDL Solvers with ROS2 in Rviz and Gazebo Simulation
Thats it with theory, now we will move to actual robot and i will show you how i have implememnted it for three robots.
UR5 Robot with 6 DOF
This GIF shows the UR5 arm moving smoothly through a sequence of positions. Using KDL, the forward and inverse kinematics are calculated in real time, allowing the arm to reach end-effector poses with minimal error. It's moving in cartesian space with IK solved by KDL.
Franka Panda with 7 DOF
Here, the Panda arm which is a redundant manipulator, performs controlled motions while avoiding joint limits. KDL’s solvers handle both the position and orientation of the end effector, demonstrating how numerical IK can manage more complex manipulators reliably.
My Custom Robot with 6 DOF
This demonstrates my own 6-DOF robot following a planned trajectory. The kinematics are computed entirely using KDL, showing how even custom robots can benefit from KDL’s chain-based FK and iterative IK solvers for accurate motion execution. You just need to make a proper URDF and that's it, you are done.
Frequently Asked Questions(FAQ)
Q1: Can we integrate KDL solvers with ROS2 in Python and CPP?
Ans: YES, you can integrate KDL solvers in ROS2 with both Python and CPP. Orocos provides compatibility for both.
Q2: What happens if my initial guess for IK is bad?
Ans: Numerical IK solvers rely on the initial guess to converge. If the guess is too far from a valid solution, the solver might fail to converge, giving an unrealistic solution, or get stuck in a local minimum. That’s why i have used the previous joint states as a starting point.
Q3: How does KDL know which joint names correspond to which angles?
Ans: When you extract a chain from the URDF, we need to create an ordered list of joint names. This ensures that when you provide a JntArray of joint angles, each value matches the correct joint in the chain. It's a very common mistake to give values in wrong order and then it becomes hard to debug.
Q4: Do I need to write my own FK/IK equations for each robot?
Ans: NO, if you use KDL. That is the whole point of using KDL, it parses the URDF and builds a chain automatically, then uses recursive and iterative solvers to compute FK and IK. This makes it much easier to work with both standard robots like UR5 or Panda, and custom manipulators.
Comments