How to Build a High-Performance UR5 Inverse Kinematics Solver With IK-Geo

Inverse kinematics (IK) is a fundamental problem for robot arms: What joint angles achieve a desired hand pose?

IK remains a challenge for many practitioners using UR-style robots in industry and research, but we will show how to write a high-performance IK solver based on IK-Geo:

• Fastest and most accurate IK solver in our testing with independently verified performance: >40x faster than IKFast, ~200x faster if only returning one solution, and errors on the order of machine epsilon (\(10^{-16}\)).
• Returns all solutions, allowing for optimal choices based on joint limits, singularities, obstacles, and dynamic performance.
• Returns singular solutions, enabling smooth robot motion by avoiding transitions to joint-level control.
• Easy to implement, debug, and modify.
• Available in MATLAB, C++, Rust, and Python.

The speed, accuracy, and reliability unlock new capabilities for real-time control, path planning, simulation, and analysis.

This tutorial builds on one part of the methods I developed in IK-Geo: Unified robot inverse kinematics using subproblem decomposition, where I introduced an IK method that finds all solutions for any 6-DOF revolute robot.

Want to skip the derivation and see the code? Check out the single-file MATLAB implementation.

Introduction

The UR5 and newer UR5e, part of the UR series, are collaborative robot arms from Universal Robots. Robots with similar kinematics are also made by Standard Bots, Techman Robot/Omron, and Dobot. UR-style robots like the UR5 have three parallel joint axes (2, 3, 4) and two pairs of intersecting joint axes (1, 2) and (5, 6).

Before solving the IK problem, we will first solve the forward kinematics problem using the Product of Exponentials (POE) convention. This simpler problem allows us to model the robot kinematics, confirm our model matches the UR robot simulator, and verify our IK solution is correct.

Next, we will solve the IK problem using subproblem decomposition. The intersecting and parallel joint axes allow us to rewrite the IK equations in closed form using three canonical geometric subproblems.

Finally, we will evaluate the solver's performance (execution time, accuracy, and singularity robustness) and compare it to other approaches including IKFast.

This tutorial will take you from forward kinematics, through IK with subproblem decomposition, to benchmarking against industry methods.

Kinematic Parameters and Forward Kinematics

Before we can solve the inverse kinematics problem, we have to create the robot model and ensure the forward kinematics matches the robot simulator.

We will use the product of exponentials (POE) convention to describe the robot kinematics. Although the Denavit–Hartenberg convention is more common, POE leads to much simpler equations.

The robot motion consists of a series of rotations and translations along the robot's joints and links. We can model this using several coordinate frames: one placed at the robot base (frame 0), one placed at each joint (frames 1–6), and one placed at the end effector (tool frame \(T\)). The joint angle vector \(q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]^\top\) consists of the angles of each joint in the robot arm relative to the zero configuration \(q=0\).

For a 6-DOF robot, the kinematic parameters consist of the joint axis vectors

\[H = [h_1\ h_2\ h_3\ h_4\ h_5\ h_6]\]

and the link displacement vectors

\[P = [p_{01}\ p_{12}\ p_{23}\ p_{34}\ p_{45}\ p_{56}\ p_{6T}].\]

Each \(h_i\) is the unit vector along the axis of rotation for joint \(i\) represented in the base frame when the robot is in the zero configuration. Similarly, each \(p_{ij}\) is the displacement from the origin of frame \(i\) to the origin of frame \(j\) represented in the base frame in the zero configuration.

The final kinematic parameter is \(R_{6T}\), the rotation matrix representing the orientation of the end effector (tool frame \(T\)) with respect to the last joint frame (frame 6). This is given by the orientation of the tool frame when the robot is in the zero configuration.

If we know the kinematic parameters \(H\), \(P\), and \(R_{6T}\) and the joint vector \(q\) then we can compute the rotation and position of the end effector:

\[\begin{align*} R_{0T} ={}&R_{01} R_{12} R_{23} R_{34} R_{45} R_{56} R_{6T},\\ p_{0T} ={}& p_{01} + R_{01} p_{12} + R_{02} p_{23}+ R_{03} p_{34}\\ &+ R_{04} p_{45} + R_{05} p_{56} + R_{06} p_{6T}. \end{align*}\]

The rotation matrix \(R_{ij}\) is the orientation of frame \(j\) with respect to frame \(i\), which we can calculate by multiplying the individual rotation matrices of each joint. For example,

\[R_{03} = R_{01} R_{12} R_{23} = \text{R}(h_1, q_1) \text{R}(h_2, q_2) \text{R}(h_3, q_3).\]

We need numerical values for the kinematic parameters to calculate the forward and inverse kinematics. For some robots, we can find the parameters using diagrams in the documentation. In the case of the UR5e, we can instead use the Denavit–Hartenberg parameters provided by Universal Robots and convert them to POE parameters. We will use the automatic IK functions provided with IK-Geo both to convert from DH parameters to POE parameters and to detect and display cases of intersecting and parallel joint axes.

alpha_vec = [sym(pi)/2 0 0 sym(pi)/2 -sym(pi)/2 0]; % rad
a_vec = [0 -0.425 -0.3922 0 0 0]; % m
d_vec = [0.1625 0 0 0.1333 0.0997 0.0996]; % m

kin = dh_to_kin(alpha_vec, a_vec, d_vec);

[is_int, is_int_nonconsecutive, is_parallel, is_spherical] =...
    detect_intersecting_parallel_axes(kin);
print_intersecting_parallel_axes(...
    is_int, is_int_nonconsecutive, is_parallel, is_spherical);

disp("P=");disp(vpa(kin.P)) % Display with decimals rather than fractions
disp("H=");disp(kin.H)

Intersecting Joints: (1, 2) (4, 5) (5, 6) 
Intersecting Nonconsecutive Joints: 
Parallel Joints: (2, 3) (3, 4) 
Spherical Joints: 

P=
[0,      0, -0.425, -0.3922,       0,       0,       0]
[0,      0,      0,       0, -0.1333,       0, -0.0996]
[0, 0.1625,      0,       0,       0, -0.0997,       0]
 
H=
[0,  0,  0,  0,  0,  0]
[0, -1, -1, -1,  0, -1]
[1,  0,  0,  0, -1,  0]

To simplify inverse kinematics, we will use the fact that axes 1 and 2 intersect, axes 5 and 6 intersect, and axes 2, 3, and 4 are parallel. The parallel axes mean that

\[h_2 = h_3 = h_4,\]

and the intersecting axes means that we can adjust the kinematic parameters so that

\[p_{12} = p_{56} = 0.\]

In the POE convention, we are free to choose each joint frame origin anywhere along the corresponding joint axis. To make joint origins 1 and 2 coincident, we can slide the origin of joint 1 along the first joint axis and make the opposite adjustment to \(p_{01}\). Similarly, we can adjust the origin of joint 5 along its axis to coincide with joint 6 by updating both \(p_{45}\) and \(p_{56}\). We have made the choice to make joint origins 5 and 6 coincident rather than joint origins 4 and 5.

zv = [0;0;0];
ex = [1;0;0];
ey = [0;1;0];
ez = [0;0;1];

kin.H = [ez ey ey ey -ez ey];
kin.P = [0.1625*ez zv -0.425*ex -0.3922*ex...
         -0.1333*ey-0.0997*ez zv -0.0996*ey];
kin.joint_type = zeros([6 1]);

We can verify we have the correct POE kinematic parameters by comparing the forward kinematics results with the UR robot simulator. We choose three test configurations, including the zero configuration. The forward kinematics results match between our calculations and the simulator, confirming the accuracy of our model.

q_test_0 = zeros([6 1]);
q_test_1 = deg2rad(90)*ones([6 1]);
q_test_2 = deg2rad([10 20 30 40 50 60]);

[R_06_0, p_0T_0] = fwdkin(kin, q_test_0);
[R_06_1, p_0T_1] = fwdkin(kin, q_test_1);
[R_06_2, p_0T_2] = fwdkin(kin, q_test_2);

p_test_0 = [-817.2 -232.9 62.8]'/1000;
R_test_0 = rot(ex, deg2rad(90)); % Zero pose, so same as R_6T

p_test_1 = [133.3 292.5 -162.9]'/1000;
R_test_1 = rot(ez, deg2rad(90));

p_test_2 = [-509.12 -290.14 -359.6]'/1000;
rot_vec = deg2rad([55.76 -153.20 58.44]);
R_test_2 = rot(rot_vec/norm(rot_vec), norm(rot_vec));

disp("[p_test_0 p_0T_0] ="); disp([p_test_0 p_0T_0])
disp("[R_test_0 R_0T_0] ="); disp([R_test_0 R_06_0*R_6T])

disp("[p_test_1 p_0T_1] ="); disp([p_test_1 p_0T_1])
disp("[R_test_1 R_0T_1] ="); disp([R_test_1 R_06_1*R_6T])

disp("[p_test_2 p_0T_2] ="); disp([p_test_2 p_0T_2])
disp("[R_test_2 R_0T_2] ="); disp([R_test_2 R_06_2*R_6T])

[p_test_0 p_0T_0] =
   -0.8172   -0.8172
   -0.2329   -0.2329
    0.0628    0.0628

[R_test_0 R_0T_0] =
    1.0000         0         0    1.0000         0         0
         0    0.0000   -1.0000         0    0.0000   -1.0000
         0    1.0000    0.0000         0    1.0000    0.0000

[p_test_1 p_0T_1] =
    0.1333    0.1333
    0.2925    0.2925
   -0.1629   -0.1629

[R_test_1 R_0T_1] =
    0.0000   -1.0000         0    0.0000   -1.0000    0.0000
    1.0000    0.0000         0    1.0000    0.0000    0.0000
         0         0    1.0000   -0.0000   -0.0000    1.0000

[p_test_2 p_0T_2] =
   -0.5091   -0.5091
   -0.2901   -0.2901
   -0.3596   -0.3596

[R_test_2 R_0T_2] =
   -0.7864   -0.6076    0.1116   -0.7864   -0.6076    0.1116
   -0.5276    0.5665   -0.6330   -0.5276    0.5665   -0.6330
    0.3214   -0.5567   -0.7660    0.3214   -0.5567   -0.7660

Having built and validated the forward kinematics model, the next step is to introduce the subproblems we will use to solve the UR5 inverse kinematics problem.

Solutions to Three Canonical Geometric Subproblems

In the IK-Geo paper, we discuss the solutions to six geometric subproblems that are used to solve the IK problem for any 6-DOF robot. For the UR5, only three subproblems are required: Subproblems 1, 3, and 4.

The algebraic and geometric formulations of these three subproblems are shown below. Each subproblem finds the angle(s) to rotate a vector to intersect with a point, a sphere, or a plane. If there is no intersection, the subproblem returns the least-squares solution.

Subproblem 1: Circle and Point

Given vectors \(p_1\), \(p_2\) and a unit vector \(k\), find \(\theta\) to minimize \(\lVert \text{R}(k, \theta)p_1 - p_2\rVert \). If there is an exact solution, then \(\theta\) solves the equation \(\text{R}(k, \theta)p_1 = p_2\). The angle \(\theta\) rotates the point \(p_1\) around the axis \(k\) to align with the point \(p_2\).

Subproblem 3: Circle and Sphere

Given vectors \(p_1\), \(p_2\), a unit vector \(k\), and nonnegative scalar \(d\), find \(\theta\) to minimize \(\lvert \lVert \text{R}(k, \theta)p_1 - p_2\rVert -d \rVert\). If there is an exact solution, then \(\theta\) solves the equation \(\lVert \text{R}(k, \theta)p_1 - p_2\rVert = d\). The angle \(\theta\) rotates the point \(p_1\) around the axis \(k\) to align with the surface of the sphere defined by the distance \(d\) from the point \(p_2\).

Subproblem 4: Circle and Plane

Given a vector \(p\), unit vectors \(k\), \(h\), and a scalar \(d\), find \(\theta\) to minimize \(\lvert h^\top \text{R}(k,\theta)p-d \rvert\). If there is an exact solution, then \(\theta\) solves the equation \(h^\top \text{R}(k,\theta)p = d \). The angle \(\theta\) rotates the point \(p\) around the axis \(k\) to align with the plane defined by the normal vector \(h\) and the distance \(d\).

The MATLAB solution for each subproblem is shown below. Least-squares solutions are indicated by the is_LS flag. The only inverse trigonometric function used is atan2(), which is more numerically stable than acos(), asin(), or atan().

function [theta, is_LS] = sp_1(p1, p2, k)
KxP = cross(k, p1);
A = [KxP -cross(k,KxP)];
x = A'*p2;
theta = atan2(x(1),x(2));

is_LS = abs(norm(p1) - norm(p2)) > 1e-8 ...
    || abs(dot(k,p1) - dot(k,p2)) > 1e-8;
end

function [theta, is_LS] = sp_3(p1, p2, k, d)
[theta, is_LS] = sp_4(p2, p1, k, 1/2 * (dot(p1,p1)+dot(p2,p2)-d^2));
end

function [theta, is_LS] = sp_4(h, p, k, d)
A_11 = cross(k,p);
A_1 = [A_11 -cross(k,A_11)];
A = h'*A_1;
b = d - h'*k*(k'*p);
norm_A_2 = dot(A,A);
x_ls_tilde = A_1'*(h*b);

if norm_A_2 > b^2
    xi = sqrt(norm_A_2-b^2);
    x_N_prime_tilde = [A(2); -A(1)];

    sc_1 = x_ls_tilde + xi*x_N_prime_tilde;
    sc_2 = x_ls_tilde - xi*x_N_prime_tilde;

    theta = [atan2(sc_1(1), sc_1(2)) atan2(sc_2(1), sc_2(2))];
    is_LS = false;
else
    theta = atan2(x_ls_tilde(1), x_ls_tilde(2));
    is_LS = true;
end
end

With Subproblems 1, 3, and 4 solved, we now have the tools needed to derive the full UR5 inverse kinematics solution.

Inverse Kinematics

In the IK problem, \(R_{0T}\) and \(p_{0T}\) are known and each \(q_i\) is unknown and must be solved for. Because the UR5 has the geometric simplifications \(h_2 = h_3 = h_4\) and \(p_{12} = p_{56} = 0\), we can rewrite the forward kinematics equations in the form of Subproblems 1, 3, and 4 and solve for one joint angle at a time.

First, we will simplify the forward kinematics equations by rewriting them in terms of \((R_{06}, p_{06})\) and by assuming \(p_{01}=0\). This is possible for any robot, not just the UR5. We can immediately write

\[R_{06} = R_{0T}R_{6T}^\top = R_{01} R_{12} R_{23} R_{34} R_{45} R_{56},\]

\[p_{06} = p_{0T}-R_{06}p_{6T} = R_{01} p_{12} + R_{02} p_{23}+ R_{03} p_{34} + R_{04} p_{45} + R_{05} p_{56}. \]

If \(p_{01}\neq 0\), as is the case for the kinematic parameters we have derived, we can subtract it from \(p_{06}\).

Now let's use the geometric simplifications of the UR5. Since \(h_2 = h_3 = h_4\), the cumulative rotation about these three joint axes can be written as a single rotation over an angle \(\theta_{14}\). It also means that projecting onto the \(h_2\) direction can greatly simplify the equations since

\[h_2^\top R_{12} = h_2^\top R_{23} = h_2^\top R_{34} = h_2^\top.\]

The second simplification, \(p_{12} = p_{56} = 0\), means that certain terms simply drop out. (We actually only need one of these terms to drop out to be able to decompose the problem into Subproblems 1, 3, and 4.)

We can rewrite the forward kinematics equations as

\[R_{06} = R_{01} \underbrace{R_{12} R_{23} R_{34}}_{R_{14}=\text{R}(h_2, \theta_{14})} R_{45} R_{56},\]

\[p_{06} = R_{02} p_{23}+ R_{03} p_{34} + R_{04} p_{45}. \]

Projecting onto \(h_2\) eliminates the variables \(q_2\), \(q_3\), and \(q_4\):

\[h_2^\top R_{10} p_{06} = h_2^\top(p_{23}+p_{34}+p_{45}),\]

\[h_2^\top R_{45} h_6 = h_2^\top R_{10} R_{06}h_6.\]

Now that we have applied the geometric simplifications, we can start solving for a few joint angles at a time using subproblems! First, we can solve for \(q_1\) using Subproblem 4 and the position equation which we projected onto \(h_2\):

\[h_2^\top \boxed{R_{10}} p_{06} = h_2^\top(p_{23}+p_{34}+p_{45}).\]

Then, we can solve for \(q_5\) using Subproblem 4 and the rotation equation which we projected onto \(h_2\):

\[h_2^\top \boxed{R_{45}} h_6 = h_2^\top R_{10} R_{06}h_6.\]

By projecting the rotation equation onto \(h_6\) or \(h_2\), we can solve for \(\theta_{14} = q_2+q_3+q_4\) and \(q_6\) using Subproblem 1:

\[\boxed{R_{14}}R_{45}h_6 = R_{10}R_{06}h_6,\]

\[\boxed{R_{65}}R_{54}h_2 = R_{06}^\top R_{01}h_2.\]

Moving some terms around in the position equation and taking the norm of both sides (\(\lVert R p\rVert = \lVert p\rVert\)) lets us solve for \(q_3\) using Subproblem 3:

\[ \lVert p_{23} + \boxed{R_{23}}p_{34}\rVert = \lVert R_{10}p_{06} - p_{12} - R_{14}p_{45} - R_{15}p_{56} \rVert.\]

Next, we solve for \(q_2\) using Subproblem 1 and the position equation:

\[ \boxed{R_{12}}(p_{23}+R_{23}p_{34}) = R_{10}p_{06} - p_{12} - R_{14}p_{45} - R_{15}p_{56}.\]

Finally, we find \(q_4\) by subtraction. We should also wrap the result to the range \([-\pi, \pi]\):

\[\boxed{q_4} = \text{wrapToPi}(\theta_{14} - q_2 - q_3).\]

And with that, we have found all joint angles \(q_1, q_2, q_3, q_4, q_5, q_6\) which form the joint angle vector. Each time we solve Subproblem 3 or 4, there are up to 2 solutions found. This means the IK solutions branch into up to 8 possible configurations for the same end effector pose.

Sometimes, one of the subproblems may not have an exact solution. Instead, we can use the least-squares subproblem solution (indicated by is_LS) to find an approximate continuous solution for that branch. This continuous approximate IK solution means that we can robustly find IK solutions even when the desired end effector pose is at or slightly outside the workspace.

The MATLAB code implementation is shown below.

function [Q, is_LS_vec] = IK_UR(R_06, p_0T, kin)
% h_2 = h_3 = h_4 and p_12 = p_56 = 0

P = kin.P;
H = kin.H;
Q = [];
is_LS_vec = [];

p_06 = p_0T - P(:,1) - R_06*P(:,7);

% Find q1 using Subproblem 4
[theta1, theta1_is_ls] = sp_4(...
    H(:,2), p_06, -H(:,1), H(:,2)'*sum(P(:,2:5), 2));

for i_t1 = 1:length(theta1)
    q_1 = theta1(i_t1);
    R_01 = rot(H(:,1), q_1);

    % Find q5 using Subproblem 4
    [theta5, theta5_is_ls] = sp_4(H(:,2),H(:,6),H(:,5), ...
        H(:,2)' * R_01' * R_06 * H(:,6));
    
    for i_t5 = 1:length(theta5)
        q_5 = theta5(i_t5);
        R_45 = rot(H(:,5), q_5);
    
        % solve for R_14 using Subproblem 1
        [theta_14, theta_14_is_LS] = sp_1(...
            R_45*H(:,6),R_01'*R_06*H(:,6), H(:,2));

        % solve for q_6 using Subproblem 1
        [q_6, q_6_is_LS] = sp_1(R_45'*H(:,2), R_06'*R_01*H(:,2), -H(:,6));
    
        % solve for q3 using Subproblem 3
        d_inner = R_01'*p_06-P(:,2) - rot(H(:,2), theta_14)*P(:,5);
        d = norm(d_inner);
        [theta_3, theta_3_is_LS] = sp_3(-P(:,4), P(:,3), H(:,2), d);
    
        for i_q3 = 1:length(theta_3)
            q_3 = theta_3(i_q3);
            
            % solve for q2 using Subproblem 1
            [q_2, q_2_is_LS] = sp_1(...
                P(:,3) + rot(H(:,2), q_3)*P(:,4), d_inner, H(:,2));
    
            % find q4 by subtraction
            q_4 = wrapToPi(theta_14 - q_2 - q_3);
            
            q_i = [q_1; q_2; q_3; q_4; q_5; q_6];
            Q = [Q q_i];
            is_LS_vec = [is_LS_vec theta1_is_ls||theta5_is_ls...
                ||theta_14_is_LS||theta_3_is_LS||q_2_is_LS||q_6_is_LS];
        end
    end
end
end

With this MATLAB implementation of the UR5 inverse kinematics solver, the next step is to evaluate its performance.

Evaluation

We evaluate the performance of the IK method by measuring the execution time and solution accuracy. We will also confirm the robustness of the algorithm by testing its ability to find solutions at and slightly outside the workspace boundary.

To measure solution accuracy, we generate 10,000 random joint angles and calculate the end effector poses using forward kinematics. Then, we calculate the difference between the original joint angle vector and the closest IK solution. We also measure the end effector pose errors in orientation and position.

The median accuracy of the joint error, position error, and orientation error are \(9.15 \times 10^{-16}\) rad, \(3.30 \times 10^{-16}\) m, and \(1.24 \times 10^{-16}\) rad, respectively. This is slightly better than the \(10^{-15}\) rad and \(10^{-15}\) m task-space accuracy of IK-Geo reported in the EAIK paper from TU Munich. The machine epsilon (smallest \(\epsilon\) such that \(1 + \epsilon > 1\)) for double-precision floating-point numbers is \(2^{-52} \approx 2.22 \times 10^{-16}\), which reflects the spacing of representable numbers near 1. The IK solutions returned by IK-Geo have accuracy close to this practical limit of double-precision arithmetic.

We also see that IK using IK-Geo results in 100% success rate over the 10,000 random poses.

To measure execution time, we follow the advice from the MATLAB Performance Measurement whitepaper. The execution time of the IK algorithm compiled to C++ with MEX Coder was found to be 2.93 μs per end effector pose. This is close to the 2.85 μs per end effector pose reported in our IK-Geo paper and faster than the ~3.9 μs reported by the graph in the EAIK paper.

Returning just one solution decreases the MATLAB execution time to 0.64 μs per pose, a 4.7 times speedup.

One important benefit of the IK-Geo IK algorithm is that it returns continuous approximate solutions for any branch which does not have an exact solution, meaning that the algorithm works even when the desired pose is at the boundary of the workspace. The algorithm also returns continuous approximate solutions when the desired pose is outside the workspace, either because of small numerical issues or in applications like teleoperation.

In the zero pose used by the Denavit–Hartenberg convention, the robot is at a singularity because all joint axes are parallel to one plane. This means we can use the end effector pose corresponding to \(q=0\) to test the singularity robustness of the IK algorithm. In the code example below, we confirm the IK algorithm recovers the \(q=0\) IK solution.

% Generate singular pose
q_0 = zeros([6 1]);
[R_06, p_0T] = fwdkin(kin, q_0);

% Confirm singularity
J = robotjacobian(kin, q_0);
disp("Min singular value: " + min(svd(J)))

% IK still works at singularity
[Q, is_LS] = hardcoded_ur5_IK(R_06, p_0T)

Min singular value: 5.7183e-17

Q =
         0         0   -2.8182   -2.8182   -2.8182
    0.0000   -0.0000    2.9023   -3.1416    3.1416
   -0.0000    0.0000         0   -0.0000    0.0000
    0.0000   -0.0000    0.2393   -3.1416    3.1416
         0         0    2.8182   -2.8182   -2.8182
         0         0    3.1416         0         0

is_LS =
     1     1     1     0     0    

In this example, the 8 solutions have merged into 5, and the first 2 solutions are very close to merging. It's easy to modify the IK algorithm to always return 8 solutions by repeating any merged solutions.

Although it is out of scope for this tutorial, we can also find all IK solutions when the robot is at an internal singularity where there is an entire continuum of IK solutions, as shown in the video below.

With accuracy near machine precision, microsecond-level timing, and robustness at singularities, we can now compare this solver to other IK methods such as IKFast.

Comparison with Other IK Methods

Perhaps the most common IK method for robots like the UR5 is IKFast. This is a python script that generates C++ code based on a COLLADA XML description file. It is part of the OpenRAVE project but is also used in ROS planners like MoveIt and Tesseract.

The code generation by IKFast is highly dependent on the joint origin placement in the COLLADA file, even though the forward kinematics is the same. Given a COLLADA file with no coincident joint axes based on the Denavit–Hartenberg parameters, IKFast fails to generate IK code. If the COLLADA file has one or two coincident joint origins, the resulting C++ file has 932 lines of code, taking 57 sec to generate. With a slightly different home position, the resulting C++ file has 118,312 lines of code, taking 40 min 36 sec to generate. Previous results have also shown IKFast generating code that does not compile or does not return the correct IK solutions.

Our previous results showed that the execution time of IKFast for the UR5 is 121.71 μs per end effector pose. Results from the EAIK paper found much faster performance at around 54.7 μs per pose. This difference in performance may be due to the inconsistency in code generated depending on the COLLADA file.

Results from the EAIK paper show the median position accuracy of IKFast is \(10^{-12}\) m, which is 3 or 4 orders of magnitude worse than IK-Geo.

Like many other IK methods, IKFast fails to find solutions when the desired end effector pose is at or near the workspace boundary.

One common method for IK is to use iterative methods based on the kinematic Jacobian. These methods only return one solution close to the initial guess rather than all possible solutions. The timing results from the EAIK paper demonstrate that these methods have execution times 1 to 3 orders of magnitude slower than IK-Geo, larger variability in execution time, and accuracy 11 orders of magnitude worse. Iterative methods also require careful tuning of parameters and can struggle with convergence near singularities.

IK methods based on machine learning have similar performance issues as iterative methods and require retraining for each new robot.

Conclusion

In this tutorial, we showed how the subproblem decomposition approach can be used to derive a fast, accurate, and robust IK solver for the UR5 robot arm.

Because the MATLAB code was not heavily optimized, there is still room for further performance improvements.

Although this tutorial focused on the UR5, very similar IK methods can be applied to any 6-DOF revolute robot with three intersecting or parallel joint axes. While the UR5 needed Subproblems 1, 3, and 4, other robots may need Subproblems 2, 5, or 6 depending on their kinematic configuration. With the addition of a 1D search, this method can solve the IK problem for any 6-DOF revolute robot with at least one pair of intersecting or parallel joint axes, which includes all industrial 6-DOF revolute robots.

All code used in this tutorial is in the aelias36/ik-geo-ur5 GitHub repo.

Further Reading

In IK-Geo: Unified robot inverse kinematics using subproblem decomposition, we demonstrated how to use the subproblem decomposition approach to solve the IK problem for any 6-DOF revolute robot. Robots with three intersecting or parallel joint axes are solved in closed form. All other robots with at least one pair of intersecting or parallel joint axes are solved using a 1D search. This covers all industrial 6-DOF robots. General 6-DOF revolute robots with no intersecting or parallel joint axes can be solved using a 2D search.

In Redundancy parameterization and inverse kinematics of 7-DOF revolute manipulators, we used the same subproblem decomposition approach to solve the IK problem for any 7-DOF revolute robot parameterized by the shoulder-elbow-wrist (SEW) angle. We also proposed a new redundancy parameterization called the stereographic SEW angle to enlarge the singularity-free workspace.

In Automatic Geometric Decomposition for Analytical Inverse Kinematics, researchers from TU Munich extended IK-Geo to automate the IK derivation process for any 6-DOF revolute robot with three intersecting or parallel joint axes.

In Path Planning and Optimization for Cuspidal 6R Manipulators, we applied IK-Geo to cuspidal robotics, which are a surprising and increasingly common class of manipulator that can switch IK solutions without encountering a singularity. Using the speed and generality of IK-Geo, we were able to efficiently identify new cuspidal robots and perform path planning and optimization.