tasbolat1 / franka_analytical_ik

A fast analytical IK solver for Franka Emika Panda

An analytical Inverse Kinematics (IK) solver for Franka Emika Panda

You can support this project by referencing our paper:

@InProceedings{HeLiu2021,
  author    = {Yanhao He and Steven Liu},
  booktitle = {2021 9th International Conference on Control, Mechatronics and Automation (ICCMA2021)},
  title     = {Analytical Inverse Kinematics for {F}ranka {E}mika {P}anda -- a Geometrical Solver for 7-{DOF} Manipulators with Unconventional Design},
  year      = {2021},
  month     = nov,
  publisher = {{IEEE}},
  doi       = {10.1109/ICCMA54375.2021.9646185},
}

A preprint version is provided in this repository. Note that there is a mistake in the published paper on ieeexplore: ABB GoFa is actually not 7-axis. This has been corrected in the attached preprint.

Introduction

This project is an IK solver for the 7-axis Franka Emika Panda robot manipulator written in C++. The solver is analytical and completely based on the geometry of the robot, therefore it includes no numerical iteration and returns the results very fast. On a modern computer the computation usually costs only a few microseconds.

Advantages over some well-known methods:

1. Compared with numerical methods: Avoiding numerical iterations leads to generally faster computation. The analytical method is also inherently more accurate, because iterative methods terminate at an error threshold and the true result is never reached. Moreover, a numerical solver usually returns only 1 solution but this geometry-based solver can obtain all possible solutions, e.g. elbow-up and -down, etc. Theoretically 8 different solutions exist. Our solver returns at most 4 because the rest turns out impractical (see our paper for details).
2. Compared with the built-in IK solver of Franka: For safety considerations, the built-in solver only accepts an input Cartesian pose that is very close to the actual one, which makes it hard to use in some situations. There is no such limitation in our solver. Furthermore, since it does not require connection to the control box, it can be used in many offline applications.
3. Compared with IKFast: The solution generated by IKFast is hard to geometrically interpret. In contrast, our solver is well grounded in geometrical analysis of Franka Emika Panda and documented as a published paper so that the user can easily understand the principle and modify the code as needed.
4. Compared with some classic analytical methods: Some existing solutions do start from geometrical inspection but end up with a set of complicated high-order equations and still require numerical approaches to finish the computation. Some other methods fail to figure out multiple solutions and singularities. For 7-DOF arms, many analytical solvers rely on the intersection of the first and last 3 joint axes and characterise a "self-motion", which does not apply to Franka Emika Panda. This project aims to fill these gaps.

Since Franka Emika Panda has 7 axes, the last joint angle (wrist) is chosed as the redundant parameter. For the details of the geometrical derivation and performance data, please refer to our paper mentioned at the beginning of this ReadMe document.

Usage

The code is in C++ and has been tested with C++11 compiling standard in Ubuntu 16.04, 18.04 and 20.04. Two functions are provided in the header file "franka_ik_He.hpp". You can simply place it in your project, include it and call those functions.

This project depends on the Eigen library (MPL2 license). At the time of writing, its latest stable version is 3.4.0 and it is included in this repository. You can put the "Eigen" folder in the same directory as "franka_ik_He.hpp".

The first provided function attempts to obtain 4 different joint configurations for a given pose of the end effector:

std::array< std::array<double, 7>, 4 > franka_IK_EE ( std::array<double, 16> O_T_EE_array,
                                                      double q7,
                                                      std::array<double, 7> q_actual_array )

The input O_T_EE_array is the desired Cartesian pose, represented as a transformation matrix relative to the robot base frame, stored in an array in column-major format. q7 is the last joint angle as the redundant parameter, specified by the user in radian. q_actual_array is the actual joint configuration. At singularity (see the paper for details), the actual angle of the first joint will be assigned to the final output.

During computation the 4 theoretical solutions will be examined in terms of joint limits. If a solution contains an angle outside the mechanical joint limits (see Franka documentation), it will be set to an array of NAN. Self-collision is not checked in the provided code. When needed, you can implement your own algorithm either outside or at the end of the function.

The second function is a "case-consistent" IK solver:

std::array<double, 7> franka_IK_EE_CC ( std::array<double, 16> O_T_EE_array,
                                        double q7,
                                        std::array<double, 7> q_actual_array )

Different from franka_IK_EE(), this function only returns the solution belonging to the same "case" as the actual joint configuration q_actual_array (see the paper for details). The purpose is to prevent unexpected switching between different solution cases (e.g. elbow-up and -down) during continuous motion planning.

Note that both functions assume that Franka Hand is installed and the solution is regarding to the end effector frame. If the Cartesian pose of the flange frame is to be used as input, according to Franka documentation you can change the const variable d7e to 0.107 and remove the 45 degree offset in q7.

MODIFIED for GraspFlow

Run this to build the code

c++ -O3 -Wall -shared -std=c++11 -I../franka_analytical_ik/Eigen -fPIC $(python -m pybind11 --includes) franka_ik_pybind.cpp -o franka_ik_pybind$(python3-config --extension-suffix)

Test:

To test wether cpps are compiled correctly, please run:

python test.py

If the output is true, then it's successfully compiled.

Usage:

Copy any *.so files into the necessary directories to use it as a third party library. For GraspFlow case copy the files to

cp *.so ../pytorch_6dof-graspnet/
cp *.so ../graspflow/

TROUBLESHOOTING

In case

fatal error: Eigen/Core: No such file or directory

this measn pybind is using eigen3, append eigen3 directory in pybind11/include/pybind11/eigen.h:

# include <eigen3/Eigen/Core>

reference: check github issue