IKFastPy - UR5 IKFast Python Package

This is a lightweight Python wrapper over OpenRave's generated IKFast C++ executables for the UR5 robot arm (e-series XML files included). IKFast "analytically solves robot inverse kinematics equations and generates optimized C++ files" for fast runtime speeds (more about IKFast here). IKFast can be used in tandem with URScript speedj commands on UR robot arms for real-time motion planning, which was used to create the visual servoing demo shown on the right (part of an ongoing project on closed-loop grasping with deep learning). Why speedj? See this UR performance analysis report.

Note: this package can be easily modified to support other robot arms.

Files

ur5.robot.xml - a custom OpenRave XML file describing the kinematics of the UR5 robot arm. Modify this if you change the arm or tool center point (TCP) position.
ikfast61.cpp - C++ code at the heart of IKFast, generated by OpenRave using ur5.robot.xml. No need to modify this.
ikfast.h - a C++ header file necessary for compiling ikfast61.cpp. No need to modify this.
ikfast_wrapper.cpp - a C++ wrapper around ikfast61.cpp. Includes forward kinematics in addition to the inverse kinematics provided by ikfast61.cpp. Modify this to change how FK and IK results are passed to your code.
ikfastpy.pyx, Kinematics.hpp, setup.py - Cython code to link C++ with Python.
demo.py - a demo in Python to test FK and IK calls to IKFast.

Installation

This implementation requires the following dependencies (tested on Ubuntu 16.04.4 LTS):

NumPy, Cython. You can quickly install/update these dependencies by running the following:
```
shell
pip install --user numpy Cython
```

Quick Start

Checkout this repository and compile the Cython wrapper:

shell
git clone https://github.com/andyzeng/ikfastpy.git
cd ikfastpy
python setup.py build_ext --inplace

Run the demo in Python to test FK and IK calls to IKFast:
```
shell
python demo.py
```

Important: ensure all rotation matrices are valid before feeding into IKFast, otherwise no IK solution will be detected. R is a rotation matrix if and only if R is orthogonal, i.e. RRT = RTR = I, and det(R) = 1.

Note: IKFast does not return solutions for singularities. In most cases, an approximate IK solution can be found for singularities by slightly perturbing the target end effector pose before re-computing IK solutions.

Modifying Robot Kinematics with OpenRave

Download and install OpenRave. See these installation instructions for Ubuntu 16.04.
Modify the kinematics of the arm or TCP position (link6) by changing ur5.robot.xml respectively. You can find a description of the OpenRave XML file format here.
(Optional) Debug the kinematics using OpenRave's viewer:
```
shell
openrave ur5.robot.xml
```

(Optional) Check the links in your file:

shell
openrave-robot.py ur5.robot.xml --info links

Use OpenRave to re-generate the IKFast C++ code ikfast61.cpp.

shell
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=ur5.robot.xml --iktype=transform6d --baselink=0 --eelink=6 --savefile=ikfast61.cpp --maxcasedepth 1

Citation

If you find IKFast useful, please cite OpenRave:

@phdthesis{diankov_thesis,
  author = "Rosen Diankov",
  title = "Automated Construction of Robotic Manipulation Programs",
  school = "Carnegie Mellon University, Robotics Institute",
  month = "August",
  year = "2010",
  number= "CMU-RI-TR-10-29",
  url={http://www.programmingvision.com/rosen_diankov_thesis.pdf},
}

This module was also a part of Visual Pushing and Grasping. If you find it useful in your work, please consider citing:

@inproceedings{zeng2018learning,
  title={Learning Synergies between Pushing and Grasping with Self-supervised Deep Reinforcement Learning},
  author={Zeng, Andy and Song, Shuran and Welker, Stefan and Lee, Johnny and Rodriguez, Alberto and Funkhouser, Thomas},
  booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  year={2018}
}