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.

• 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.

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:

  pip install --user numpy Cython

1. Checkout this repository and compile the Cython wrapper:

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

2. Run the demo in Python to test FK and IK calls to IKFast:

   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.

1. Download and install OpenRave. See these installation instructions for Ubuntu 16.04.

2. 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.

3. (Optional) Debug the kinematics using OpenRave's viewer:

   openrave ur5.robot.xml

4. (Optional) Check the links in your file:

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

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

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

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}
}