C++ implementation with Python bindings of analytic forward and inverse kinematics for the Universal Robots based on Alternative Inverse Kinematic Solution of the UR5 Robotic Arm.

This project is still very experimental, the API will likely still change.

The main advantages of using analytic IK:

• extremely fast. FK calls from python take +- 4 µs, IK calls +- 18 µs.
• finds all solutions at once, allowing you to select the most convenient one.
• no need to provide initial guess, as opposed to numerical IK solutions.

Warning: this repo uses the default DH-parameters for the UR robots. But every robot is slightly different and is factory-calibrated to provide very accurate DH parameters, which are also used by the robot controlbox. From a few tests we have run, using the default DH-parameters typically results in 1-2mm differences in the FK for a given joint configuration. See here for details. If you need very high precision, you might want to use your robot's DH-parameters for FK/IK.

pre-built wheels are availabe on PyPI and can be installed with pip:

pip install ur_analytic_ik

To install from source, see the Developer section.

Afterwards, you should be able to issue the FK and IK functions like this:

import numpy as np
from ur_analytic_ik import ur5e

eef_pose = np.identity(4)
X = np.array([-1.0, 0.0, 0.0])
Y = np.array([0.0, 1.0, 0.0])
Z = np.array([0.0, 0.0, -1.0])
top_down_orientation = np.column_stack([X, Y, Z])
translation = np.array([-0.2, -0.2, 0.2])

eef_pose[:3, :3] = top_down_orientation
eef_pose[:3, 3] = translation

solutions = ur5e.inverse_kinematics(eef_pose)

More examples:

import numpy as np
from ur_analytic_ik import ur3e

joints = np.zeros(6)
eef_pose = np.identity(4)
eef_pose[2, 3] = 0.4
tcp_transform = np.identity(4)
tcp_transform[2, 3] = 0.1

ur3e.forward_kinematics(0, 0, 0, 0, 0, 0)
ur3e.forward_kinematics(*joints)
tcp_pose = ur3e.forward_kinematics_with_tcp(*joints, tcp_transform)

joint_solutions = ur3e.inverse_kinematics(eef_pose)
joint_solutions = ur3e.inverse_kinematics_closest(eef_pose, *joints)
joint_solutions = ur3e.inverse_kinematics_with_tcp(eef_pose, tcp_transform)

This codebase uses nanobind to provide python bindings for the FK/IK functions.

python package building

This is the easiest option. It leverages scikit-build to create a python package and build the bindings. This flow is based on https://github.com/wjakob/nanobind_example

• Create a conda environment for the project: conda env create -f environment.yaml
• to create the python package, including the bindings: pip install . (this uses scikit-build to build the C++ from the top-level CMakelist.txt)
• you can now import the library in python.

C++ building

if you want to build the C++ code without building the bindings or creating a python package:

• make sure you have a C++ compiler available.
• make sure you have the Eigen package available, if not run apt install libeigen3-dev.

Some linux users have eigen installed at /usr/include/eigen3 instead of /usr/include/Eigen. Symlink it:

sudo ln -sf /usr/include/eigen3/Eigen /usr/include/Eigen
sudo ln -sf /usr/include/eigen3/unsupported /usr/include/unsupported

• run cmake -S . -B & cmake --build build from the src/ dir.
• execute ./build/main

run pytest -v .

Tests are also automatically executed in github for each commit.

Wheels are built automatically for all PRs, you can check them on test PyPI.

• bump the version in the pyproject.toml file. We use semantic versioning. Use pre-releases if you want to test changes.
• create a new tag, corresponding to the version: git tag vX.Y.Z-...
• push the tag git push --tag, this will already trigger a build of the wheels on test PyPI
• once you have verified the wheels work and are built properly, create a new release with the same name as the semantic version for the tag on github. This will trigger an upload to PyPI.

Adding an IK function that returns the closest solution and accepts a TCP transform.

Reducing the amount of separate IK functions, e.g. replacing:

ur3e.inverse_kinematics_with_tcp(eef_pose)
# with
ur3e.inverse_kinematics(eef_pose, tcp=tcp_transform)

The same holds for functions ending with _closest().

Currently IK runs at about 10 μs / EEF pose on my laptop. However, before I implemented the filtering of the solutions, it was closer to 3 μs. Part of this is because I adapted the bindings in ur_analytic_ik_ext.cpp to return vectors with the solutions.

• Adding more technical documentation.
• ur_analytic_ik_ext.cpp should be made much more readable.
• Reducing some duplication e.g. when defining the IK/FK functions and bindings for the different robots.