├── .gitignore
├── Kinematics.hpp
├── README.md
├── __init__.py
├── demo.py
├── ikfast.h
├── ikfast61.cpp
├── ikfast_wrapper.cpp
├── ikfastpy.pyx
├── images
    └── closed-loop-grasping.gif
├── setup.py
├── ur5.robot.xml
└── ur5e.robot.xml


/.gitignore:
--------------------------------------------------------------------------------
1 | build/*
2 | ikfastpy.cpp
3 | ikfastpy.so


--------------------------------------------------------------------------------
/Kinematics.hpp:
--------------------------------------------------------------------------------
1 | namespace robots { 
2 |     class Kinematics { 
3 |         public: int num_of_joints, num_free_parameters; 
4 |         Kinematics(); 
5 |         ~Kinematics(); 
6 |         std::vector<float> forward(std::vector<float> joint_config);
7 |         std::vector<float> inverse(std::vector<float> ee_pose);
8 |     }; 
9 | }


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # IKFastPy - UR5 IKFast Python Package
 2 | 
 3 | <img src="images/closed-loop-grasping.gif" height=200px align="right" />
 4 | 
 5 | This is a lightweight Python wrapper over [OpenRave's](http://openrave.org/) generated [IKFast](http://openrave.org/docs/0.8.2/openravepy/ikfast/) C++ executables for the UR5 robot arm (e-series XML files included). IKFast <i>"analytically solves robot inverse kinematics equations and generates optimized C++ files"</i> for fast runtime speeds (more about IKFast [here](http://openrave.org/docs/0.8.2/openravepy/ikfast/)). IKFast can be used in tandem with [URScript](http://www.sysaxes.com/manuels/scriptmanual_en_3.1.pdf) `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](http://orbit.dtu.dk/files/105275650/ur10_performance_analysis.pdf).
 6 | 
 7 | Note: this package can be [easily modified](#modifying-robot-kinematics-with-openrave) to support other robot arms.
 8 | 
 9 | ## Files
10 | 
11 |  * **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.
12 |  * **ikfast61.cpp** - C++ code at the heart of IKFast, generated by OpenRave using `ur5.robot.xml`. No need to modify this.
13 |  * **ikfast.h** - a C++ header file necessary for compiling `ikfast61.cpp`. No need to modify this.
14 |  * **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.
15 |  * **ikfastpy.pyx**, **Kinematics.hpp**, **setup.py** - Cython code to link C++ with Python.
16 |  * **demo.py** - a demo in Python to test FK and IK calls to IKFast.
17 | 
18 | ## Installation
19 | 
20 | This implementation requires the following dependencies (tested on Ubuntu 16.04.4 LTS):
21 | 
22 |  * [NumPy](http://www.numpy.org/), [Cython](http://cython.org/). You can quickly install/update these dependencies by running the following:
23 |     ```shell
24 |     pip install --user numpy Cython
25 |     ```
26 | 
27 | ## Quick Start
28 | 
29 | 1. Checkout this repository and compile the Cython wrapper:
30 |     ```shell
31 |     git clone https://github.com/andyzeng/ikfastpy.git
32 |     cd ikfastpy
33 |     python setup.py build_ext --inplace
34 |     ```
35 | 1. Run the demo in Python to test FK and IK calls to IKFast:
36 |     ```shell
37 |     python demo.py
38 |     ```
39 | 
40 |  **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. RR<sup>T</sup> = R<sup>T</sup>R = I, and det(R) = 1.
41 | 
42 |  **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.
43 | 
44 | ## Modifying Robot Kinematics with OpenRave
45 | 
46 | 1. Download and install [OpenRave](http://openrave.org/). See [these installation instructions](https://scaron.info/teaching/installing-openrave-on-ubuntu-16.04.html) for Ubuntu 16.04.
47 | 
48 | 1. 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](http://openrave.programmingvision.com/wiki/index.php/Format:XML).
49 | 
50 | 1. (Optional) Debug the kinematics using OpenRave's viewer:
51 |     ```shell
52 |     openrave ur5.robot.xml
53 |     ```
54 | 
55 | 1. (Optional) Check the links in your file:
56 |     ```shell
57 |     openrave-robot.py ur5.robot.xml --info links
58 |     ```
59 | 
60 | 1. Use OpenRave to re-generate the IKFast C++ code `ikfast61.cpp`. 
61 |     ```shell
62 |     python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=ur5.robot.xml --iktype=transform6d --baselink=0 --eelink=6 --savefile=ikfast61.cpp --maxcasedepth 1
63 |     ```
64 | 
65 | ## Citation
66 | 
67 | If you find [IKFast](http://openrave.org/docs/0.8.2/openravepy/ikfast/) useful, please cite [OpenRave](http://openrave.org/):
68 | 
69 | ```
70 | @phdthesis{diankov_thesis,
71 |   author = "Rosen Diankov",
72 |   title = "Automated Construction of Robotic Manipulation Programs",
73 |   school = "Carnegie Mellon University, Robotics Institute",
74 |   month = "August",
75 |   year = "2010",
76 |   number= "CMU-RI-TR-10-29",
77 |   url={http://www.programmingvision.com/rosen_diankov_thesis.pdf},
78 | }
79 | ```
80 | 
81 | This module was also a part of [Visual Pushing and Grasping](https://github.com/andyzeng/visual-pushing-grasping). If you find it useful in your work, please consider citing:
82 | 
83 | ```
84 | @inproceedings{zeng2018learning,
85 |   title={Learning Synergies between Pushing and Grasping with Self-supervised Deep Reinforcement Learning},
86 |   author={Zeng, Andy and Song, Shuran and Welker, Stefan and Lee, Johnny and Rodriguez, Alberto and Funkhouser, Thomas},
87 |   booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
88 |   year={2018}
89 | }
90 | ```
91 | 
92 | 


--------------------------------------------------------------------------------
/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/andyzeng/ikfastpy/7d2154288ad993710854bc32bc85ca50bc6b9860/__init__.py


--------------------------------------------------------------------------------
/demo.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import ikfastpy
 3 | 
 4 | # Initialize kinematics for UR5 robot arm
 5 | ur5_kin = ikfastpy.PyKinematics()
 6 | n_joints = ur5_kin.getDOF()
 7 | 
 8 | joint_angles = [-3.1,-1.6,1.6,-1.6,-1.6,0.] # in radians
 9 | 
10 | # Test forward kinematics: get end effector pose from joint angles
11 | print("\nTesting forward kinematics:\n")
12 | print("Joint angles:")
13 | print(joint_angles)
14 | ee_pose = ur5_kin.forward(joint_angles)
15 | ee_pose = np.asarray(ee_pose).reshape(3,4) # 3x4 rigid transformation matrix
16 | print("\nEnd effector pose:")
17 | print(ee_pose)
18 | print("\n-----------------------------")
19 | 
20 | # Test inverse kinematics: get joint angles from end effector pose
21 | print("\nTesting inverse kinematics:\n")
22 | joint_configs = ur5_kin.inverse(ee_pose.reshape(-1).tolist())
23 | n_solutions = int(len(joint_configs)/n_joints)
24 | print("%d solutions found:"%(n_solutions))
25 | joint_configs = np.asarray(joint_configs).reshape(n_solutions,n_joints)
26 | for joint_config in joint_configs:
27 |     print(joint_config)
28 | 
29 | # Check cycle-consistency of forward and inverse kinematics
30 | assert(np.any([np.sum(np.abs(joint_config-np.asarray(joint_angles))) < 1e-4 for joint_config in joint_configs]))
31 | print("\nTest passed!")


--------------------------------------------------------------------------------
/ikfast.h:
--------------------------------------------------------------------------------
  1 | // -*- coding: utf-8 -*-
  2 | // Copyright (C) 2012-2014 Rosen Diankov <rosen.diankov@gmail.com>
  3 | //
  4 | // Licensed under the Apache License, Version 2.0 (the "License");
  5 | // you may not use this file except in compliance with the License.
  6 | // You may obtain a copy of the License at
  7 | //     http://www.apache.org/licenses/LICENSE-2.0
  8 | //
  9 | // Unless required by applicable law or agreed to in writing, software
 10 | // distributed under the License is distributed on an "AS IS" BASIS,
 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | // See the License for the specific language governing permissions and
 13 | // limitations under the License.
 14 | /** \brief  Header file for all ikfast c++ files/shared objects.
 15 | 
 16 |     The ikfast inverse kinematics compiler is part of OpenRAVE.
 17 | 
 18 |     The file is divided into two sections:
 19 |     - <b>Common</b> - the abstract classes section that all ikfast share regardless of their settings
 20 |     - <b>Library Specific</b> - the library-specific definitions, which depends on the precision/settings that the library was compiled with
 21 | 
 22 |     The defines are as follows, they are also used for the ikfast C++ class:
 23 | 
 24 |    - IKFAST_HEADER_COMMON - common classes
 25 |    - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off
 26 |    - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility.
 27 |    - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY
 28 |    - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected.
 29 |    - IKFAST_REAL - Use to force a custom real number type for IkReal.
 30 |    - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded.
 31 | 
 32 |  */
 33 | #include <vector>
 34 | #include <list>
 35 | #include <stdexcept>
 36 | #include <cmath>
 37 | 
 38 | #ifndef IKFAST_HEADER_COMMON
 39 | #define IKFAST_HEADER_COMMON
 40 | 
 41 | /// should be the same as ikfast.__version__
 42 | /// if 0x10000000 bit is set, then the iksolver assumes 6D transforms are done without the manipulator offset taken into account (allows to reuse IK when manipulator offset changes)
 43 | #define IKFAST_VERSION 0x10000048
 44 | 
 45 | namespace ikfast {
 46 | 
 47 | /// \brief holds the solution for a single dof
 48 | template <typename T>
 49 | class IkSingleDOFSolutionBase
 50 | {
 51 | public:
 52 |     IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) {
 53 |         indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
 54 |     }
 55 |     T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset
 56 |     signed char freeind; ///< if >= 0, mimics another joint
 57 |     unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider
 58 |     unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself
 59 |     unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root
 60 | };
 61 | 
 62 | /// \brief The discrete solutions are returned in this structure.
 63 | ///
 64 | /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions.
 65 | /// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is:
 66 | template <typename T>
 67 | class IkSolutionBase
 68 | {
 69 | public:
 70 |     virtual ~IkSolutionBase() {
 71 |     }
 72 |     /// \brief gets a concrete solution
 73 |     ///
 74 |     /// \param[out] solution the result
 75 |     /// \param[in] freevalues values for the free parameters \se GetFree
 76 |     virtual void GetSolution(T* solution, const T* freevalues) const = 0;
 77 | 
 78 |     /// \brief std::vector version of \ref GetSolution
 79 |     virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
 80 |         solution.resize(GetDOF());
 81 |         GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
 82 |     }
 83 | 
 84 |     /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned
 85 |     ///
 86 |     /// 0 always points to the first value accepted by the ik function.
 87 |     /// \return vector of indices indicating the free parameters
 88 |     virtual const std::vector<int>& GetFree() const = 0;
 89 | 
 90 |     /// \brief the dof of the solution
 91 |     virtual const int GetDOF() const = 0;
 92 | };
 93 | 
 94 | /// \brief manages all the solutions
 95 | template <typename T>
 96 | class IkSolutionListBase
 97 | {
 98 | public:
 99 |     virtual ~IkSolutionListBase() {
100 |     }
101 | 
102 |     /// \brief add one solution and return its index for later retrieval
103 |     ///
104 |     /// \param vinfos Solution data for each degree of freedom of the manipulator
105 |     /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. The indices are of the configuration that the IK solver accepts rather than the entire robot, ie 0 points to the first value accepted.
106 |     virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
107 | 
108 |     /// \brief returns the solution pointer
109 |     virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
110 | 
111 |     /// \brief returns the number of solutions stored
112 |     virtual size_t GetNumSolutions() const = 0;
113 | 
114 |     /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated.
115 |     virtual void Clear() = 0;
116 | };
117 | 
118 | /// \brief holds function pointers for all the exported functions of ikfast
119 | template <typename T>
120 | class IkFastFunctions
121 | {
122 | public:
123 |     IkFastFunctions() : _ComputeIk(NULL), _ComputeIk2(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) {
124 |     }
125 |     virtual ~IkFastFunctions() {
126 |     }
127 |     typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
128 |     ComputeIkFn _ComputeIk;
129 |     typedef bool (*ComputeIk2Fn)(const T*, const T*, const T*, IkSolutionListBase<T>&, void*);
130 |     ComputeIk2Fn _ComputeIk2;
131 |     typedef void (*ComputeFkFn)(const T*, T*, T*);
132 |     ComputeFkFn _ComputeFk;
133 |     typedef int (*GetNumFreeParametersFn)();
134 |     GetNumFreeParametersFn _GetNumFreeParameters;
135 |     typedef int* (*GetFreeParametersFn)();
136 |     GetFreeParametersFn _GetFreeParameters;
137 |     typedef int (*GetNumJointsFn)();
138 |     GetNumJointsFn _GetNumJoints;
139 |     typedef int (*GetIkRealSizeFn)();
140 |     GetIkRealSizeFn _GetIkRealSize;
141 |     typedef const char* (*GetIkFastVersionFn)();
142 |     GetIkFastVersionFn _GetIkFastVersion;
143 |     typedef int (*GetIkTypeFn)();
144 |     GetIkTypeFn _GetIkType;
145 |     typedef const char* (*GetKinematicsHashFn)();
146 |     GetKinematicsHashFn _GetKinematicsHash;
147 | };
148 | 
149 | // Implementations of the abstract classes, user doesn't need to use them
150 | 
151 | /// \brief Default implementation of \ref IkSolutionBase
152 | template <typename T>
153 | class IkSolution : public IkSolutionBase<T>
154 | {
155 | public:
156 |     IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) {
157 |         _vbasesol = vinfos;
158 |         _vfree = vfree;
159 |     }
160 | 
161 |     virtual void GetSolution(T* solution, const T* freevalues) const {
162 |         for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
163 |             if( _vbasesol[i].freeind < 0 )
164 |                 solution[i] = _vbasesol[i].foffset;
165 |             else {
166 |                 solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset;
167 |                 if( solution[i] > T(3.14159265358979) ) {
168 |                     solution[i] -= T(6.28318530717959);
169 |                 }
170 |                 else if( solution[i] < T(-3.14159265358979) ) {
171 |                     solution[i] += T(6.28318530717959);
172 |                 }
173 |             }
174 |         }
175 |     }
176 | 
177 |     virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
178 |         solution.resize(GetDOF());
179 |         GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
180 |     }
181 | 
182 |     virtual const std::vector<int>& GetFree() const {
183 |         return _vfree;
184 |     }
185 |     virtual const int GetDOF() const {
186 |         return static_cast<int>(_vbasesol.size());
187 |     }
188 | 
189 |     virtual void Validate() const {
190 |         for(size_t i = 0; i < _vbasesol.size(); ++i) {
191 |             if( _vbasesol[i].maxsolutions == (unsigned char)-1) {
192 |                 throw std::runtime_error("max solutions for joint not initialized");
193 |             }
194 |             if( _vbasesol[i].maxsolutions > 0 ) {
195 |                 if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) {
196 |                     throw std::runtime_error("index >= max solutions for joint");
197 |                 }
198 |                 if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) {
199 |                     throw std::runtime_error("2nd index >= max solutions for joint");
200 |                 }
201 |             }
202 |             if( !std::isfinite(_vbasesol[i].foffset) ) {
203 |                 throw std::runtime_error("foffset was not finite");
204 |             }
205 |         }
206 |     }
207 | 
208 |     virtual void GetSolutionIndices(std::vector<unsigned int>& v) const {
209 |         v.resize(0);
210 |         v.push_back(0);
211 |         for(int i = (int)_vbasesol.size()-1; i >= 0; --i) {
212 |             if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) {
213 |                 for(size_t j = 0; j < v.size(); ++j) {
214 |                     v[j] *= _vbasesol[i].maxsolutions;
215 |                 }
216 |                 size_t orgsize=v.size();
217 |                 if( _vbasesol[i].indices[1] != (unsigned char)-1 ) {
218 |                     for(size_t j = 0; j < orgsize; ++j) {
219 |                         v.push_back(v[j]+_vbasesol[i].indices[1]);
220 |                     }
221 |                 }
222 |                 if( _vbasesol[i].indices[0] != (unsigned char)-1 ) {
223 |                     for(size_t j = 0; j < orgsize; ++j) {
224 |                         v[j] += _vbasesol[i].indices[0];
225 |                     }
226 |                 }
227 |             }
228 |         }
229 |     }
230 | 
231 |     std::vector< IkSingleDOFSolutionBase<T> > _vbasesol;       ///< solution and their offsets if joints are mimiced
232 |     std::vector<int> _vfree;
233 | };
234 | 
235 | /// \brief Default implementation of \ref IkSolutionListBase
236 | template <typename T>
237 | class IkSolutionList : public IkSolutionListBase<T>
238 | {
239 | public:
240 |     virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
241 |     {
242 |         size_t index = _listsolutions.size();
243 |         _listsolutions.push_back(IkSolution<T>(vinfos,vfree));
244 |         return index;
245 |     }
246 | 
247 |     virtual const IkSolutionBase<T>& GetSolution(size_t index) const
248 |     {
249 |         if( index >= _listsolutions.size() ) {
250 |             throw std::runtime_error("GetSolution index is invalid");
251 |         }
252 |         typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin();
253 |         std::advance(it,index);
254 |         return *it;
255 |     }
256 | 
257 |     virtual size_t GetNumSolutions() const {
258 |         return _listsolutions.size();
259 |     }
260 | 
261 |     virtual void Clear() {
262 |         _listsolutions.clear();
263 |     }
264 | 
265 | protected:
266 |     std::list< IkSolution<T> > _listsolutions;
267 | };
268 | 
269 | }
270 | 
271 | #endif // OPENRAVE_IKFAST_HEADER
272 | 
273 | // The following code is dependent on the C++ library linking with.
274 | #ifdef IKFAST_HAS_LIBRARY
275 | 
276 | // defined when creating a shared object/dll
277 | #ifdef IKFAST_CLIBRARY
278 | #ifdef _MSC_VER
279 | #define IKFAST_API extern "C" __declspec(dllexport)
280 | #else
281 | #define IKFAST_API extern "C"
282 | #endif
283 | #else
284 | #define IKFAST_API
285 | #endif
286 | 
287 | #ifdef IKFAST_NAMESPACE
288 | namespace IKFAST_NAMESPACE {
289 | #endif
290 | 
291 | #ifdef IKFAST_REAL
292 | typedef IKFAST_REAL IkReal;
293 | #else
294 | typedef double IkReal;
295 | #endif
296 | 
297 | /** \brief Computes all IK solutions given a end effector coordinates and the free joints.
298 | 
299 |    - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation.
300 |    - ``eerot``
301 |    - For **Transform6D** it is 9 values for the 3x3 rotation matrix.
302 |    - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
303 |    - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle.
304 |    - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system.
305 |  */
306 | IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
307 | 
308 | /** \brief Similar to ComputeIk except takes OpenRAVE boost::shared_ptr<RobotBase::Manipulator>*
309 |  */
310 | IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip);
311 | 
312 | /// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik.
313 | IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
314 | 
315 | /// \brief returns the number of free parameters users has to set apriori
316 | IKFAST_API int GetNumFreeParameters();
317 | 
318 | /// \brief the indices of the free parameters indexed by the chain joints
319 | IKFAST_API int* GetFreeParameters();
320 | 
321 | /// \brief the total number of indices of the chain
322 | IKFAST_API int GetNumJoints();
323 | 
324 | /// \brief the size in bytes of the configured number type
325 | IKFAST_API int GetIkRealSize();
326 | 
327 | /// \brief the ikfast version used to generate this file
328 | IKFAST_API const char* GetIkFastVersion();
329 | 
330 | /// \brief the ik type ID
331 | IKFAST_API int GetIkType();
332 | 
333 | /// \brief a hash of all the chain values used for double checking that the correct IK is used.
334 | IKFAST_API const char* GetKinematicsHash();
335 | 
336 | #ifdef IKFAST_NAMESPACE
337 | }
338 | #endif
339 | 
340 | #endif // IKFAST_HAS_LIBRARY
341 | 


--------------------------------------------------------------------------------
/ikfast_wrapper.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * THIS IS A MODIFIED VERSION OF THE FOLLOWING:
  3 |  * 
  4 |  * IKFast Demo
  5 |  * 
  6 |  * Shows how to calculate FK from joint angles.
  7 |  * Calculates IK from rotation-translation matrix, or translation-quaternion pose.
  8 |  * Performance timing tests.
  9 |  *
 10 |  * Run the program to view command line parameters.
 11 |  * 
 12 |  * 
 13 |  * To compile, run:
 14 |  * g++ -lstdc++ -llapack -o compute ikfastdemo.cpp -lrt
 15 |  * (need to link with 'rt' for gettime(), it must come after the source file name)
 16 |  *
 17 |  * 
 18 |  * Tested with Ubuntu 11.10 (Oneiric)
 19 |  * IKFast54 from OpenRAVE 0.6.0
 20 |  * IKFast56/61 from OpenRave 0.8.2
 21 |  *
 22 |  * Author: David Butterworth, KAIST
 23 |  *         Based on code by Rosen Diankov
 24 |  * Date: November 2012
 25 |  */
 26 | 
 27 | /*
 28 |  * Copyright (c) 2012, David Butterworth, KAIST
 29 |  * All rights reserved.
 30 |  *
 31 |  * Redistribution and use in source and binary forms, with or without
 32 |  * modification, are permitted provided that the following conditions are met:
 33 |  *
 34 |  *     * Redistributions of source code must retain the above copyright
 35 |  *       notice, this list of conditions and the following disclaimer.
 36 |  *     * Redistributions in binary form must reproduce the above copyright
 37 |  *       notice, this list of conditions and the following disclaimer in the
 38 |  *       documentation and/or other materials provided with the distribution.
 39 |  *     * Neither the name of the Willow Garage, Inc. nor the names of its
 40 |  *       contributors may be used to endorse or promote products derived from
 41 |  *       this software without specific prior written permission.
 42 |  *
 43 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 44 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 45 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 46 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 47 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 48 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 49 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 50 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 51 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 52 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 53 |  * POSSIBILITY OF SUCH DAMAGE.
 54 |  */
 55 | 
 56 | #define IKFAST_HAS_LIBRARY // Build IKFast with API functions
 57 | #define IKFAST_NO_MAIN // Don't include main() from IKFast
 58 | 
 59 | /*
 60 | Set which IKFast version you are using
 61 | The API calls are slightly different for versions > 54
 62 | */
 63 | 
 64 | #define IK_VERSION 61
 65 | #include "ikfast61.cpp"
 66 | 
 67 | //#define IK_VERSION 56
 68 | //#include "ikfast56.Transform6D.0_1_2_3_4_5.cpp"
 69 | 
 70 | //#define IK_VERSION 54
 71 | //#include "output_ikfast54.cpp"
 72 | 
 73 | 
 74 | //----------------------------------------------------------------------------//
 75 | 
 76 | #include <stdio.h>
 77 | #include <stdlib.h>
 78 | #include <time.h> // for clock_gettime()
 79 | #include <vector>
 80 | 
 81 | float SIGN(float x);
 82 | float NORM(float a, float b, float c, float d);
 83 | 
 84 | #if IK_VERSION > 54
 85 | #define IKREAL_TYPE IkReal // for IKFast 56,61
 86 | #else
 87 | #define IKREAL_TYPE IKReal // for IKFast 54
 88 | #endif
 89 | 
 90 | namespace robots { 
 91 |     class Kinematics { 
 92 |         public: int num_of_joints, num_free_parameters; 
 93 |         Kinematics(); 
 94 |         ~Kinematics(); 
 95 |         std::vector<float> forward(std::vector<float> joint_config);
 96 |         std::vector<float> inverse(std::vector<float> ee_pose);
 97 |     }; 
 98 |     Kinematics::Kinematics() { 
 99 |         #if IK_VERSION > 54
100 |             // for IKFast 56,61
101 |             num_of_joints = GetNumJoints();
102 |             num_free_parameters = GetNumFreeParameters();
103 |         #else
104 |             // for IKFast 54
105 |             num_of_joints = getNumJoints();
106 |             num_free_parameters = getNumFreeParameters();
107 |         #endif
108 |     } 
109 |     Kinematics::~Kinematics() { } 
110 |     std::vector<float> Kinematics::forward(std::vector<float> joint_config) {
111 |         IKREAL_TYPE eerot[9],eetrans[3];
112 |         std::vector<float> ee_pose;
113 | 
114 |         if( joint_config.size() != num_of_joints ) {
115 |             printf("\nError: (forward kinematics) expects vector of %d values describing joint angles (in radians).\n\n", num_of_joints);
116 |             return ee_pose;
117 |         }
118 | 
119 |         // Put input joint values into array
120 |         IKREAL_TYPE joints[num_of_joints];
121 |         for (unsigned int i=0; i<num_of_joints; i++)
122 |         {
123 |             joints[i] = joint_config[i];
124 |         }
125 | 
126 | #if IK_VERSION > 54
127 |         // for IKFast 56,61
128 |         ComputeFk(joints, eetrans, eerot); // void return
129 | #else
130 |         // for IKFast 54
131 |         fk(joints, eetrans, eerot); // void return
132 | #endif
133 |         for (unsigned int i=0; i<3; i++) {
134 |             ee_pose.push_back(eerot[i*3+0]);
135 |             ee_pose.push_back(eerot[i*3+1]);
136 |             ee_pose.push_back(eerot[i*3+2]);
137 |             ee_pose.push_back(eetrans[i]);
138 |         }
139 | 
140 |         return ee_pose;
141 |     }
142 |     std::vector<float> Kinematics::inverse(std::vector<float> ee_pose) {
143 |         IKREAL_TYPE eerot[9],eetrans[3];
144 |         std::vector<float> joint_configs;
145 | 
146 |         if( ee_pose.size() == 7 )  // ik, given translation vector and quaternion pose
147 |         {
148 | #if IK_VERSION > 54
149 |             // for IKFast 56,61
150 |             IkSolutionList<IKREAL_TYPE> solutions;
151 | #else
152 |             // for IKFast 54
153 |             std::vector<IKSolution> vsolutions;
154 | #endif
155 |             std::vector<IKREAL_TYPE> vfree(num_free_parameters);
156 | 
157 |             eetrans[0] = ee_pose[0];
158 |             eetrans[1] = ee_pose[1];
159 |             eetrans[2] = ee_pose[2];
160 | 
161 |             // Convert input effector pose, in w x y z quaternion notation, to rotation matrix. 
162 |             // Must use doubles, else lose precision compared to directly inputting the rotation matrix.
163 |             double qw = ee_pose[3];
164 |             double qx = ee_pose[4];
165 |             double qy = ee_pose[5];
166 |             double qz = ee_pose[6];
167 |             const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw);
168 |             qw *= n;
169 |             qx *= n;
170 |             qy *= n;
171 |             qz *= n;
172 |             eerot[0] = 1.0f - 2.0f*qy*qy - 2.0f*qz*qz;  eerot[1] = 2.0f*qx*qy - 2.0f*qz*qw;         eerot[2] = 2.0f*qx*qz + 2.0f*qy*qw;
173 |             eerot[3] = 2.0f*qx*qy + 2.0f*qz*qw;         eerot[4] = 1.0f - 2.0f*qx*qx - 2.0f*qz*qz;  eerot[5] = 2.0f*qy*qz - 2.0f*qx*qw;
174 |             eerot[6] = 2.0f*qx*qz - 2.0f*qy*qw;         eerot[7] = 2.0f*qy*qz + 2.0f*qx*qw;         eerot[8] = 1.0f - 2.0f*qx*qx - 2.0f*qy*qy;
175 | 
176 |             // For debugging, output the matrix
177 |             /*
178 |             for (unsigned char i=0; i<=8; i++)
179 |             {   // detect -0.0 and replace with 0.0
180 |                 if ( ((int&)(eerot[i]) & 0xFFFFFFFF) == 0) eerot[i] = 0.0;
181 |             }
182 |             printf("     Rotation     %f   %f   %f  \n", eerot[0], eerot[1], eerot[2] );
183 |             printf("                  %f   %f   %f  \n", eerot[3], eerot[4], eerot[5] );
184 |             printf("                  %f   %f   %f  \n", eerot[6], eerot[7], eerot[8] );
185 |             printf("\n");
186 |             */
187 | 
188 |             // for(std::size_t i = 0; i < vfree.size(); ++i)
189 |             //     vfree[i] = atof(argv[13+i]);
190 | 
191 | #if IK_VERSION > 54
192 |             // for IKFast 56,61
193 |             bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
194 | #else
195 |             // for IKFast 54
196 |             bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
197 | #endif
198 |             if( !bSuccess ) {
199 |                 fprintf(stderr,"Error: (inverse kinematics) failed to get ik solution\n");
200 |                 return joint_configs;
201 |             }
202 | 
203 | #if IK_VERSION > 54
204 |             // for IKFast 56,61
205 |             unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
206 | #else
207 |             // for IKFast 54
208 |             unsigned int num_of_solutions = (int)vsolutions.size();
209 | #endif
210 |             // printf("Found %d ik solutions:\n", num_of_solutions ); 
211 | 
212 |             std::vector<IKREAL_TYPE> solvalues(num_of_joints);
213 |             for(std::size_t i = 0; i < num_of_solutions; ++i) {
214 | #if IK_VERSION > 54
215 |                 // for IKFast 56,61
216 |                 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
217 |                 int this_sol_free_params = (int)sol.GetFree().size(); 
218 | #else
219 |                 // for IKFast 54
220 |                 int this_sol_free_params = (int)vsolutions[i].GetFree().size();
221 | #endif
222 |                 // printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
223 |                 std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);
224 | 
225 | #if IK_VERSION > 54
226 |                 // for IKFast 56,61
227 |                 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
228 | #else
229 |                 // for IKFast 54
230 |                 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
231 | #endif
232 | 
233 |                 for( std::size_t j = 0; j < solvalues.size(); ++j)
234 |                     joint_configs.push_back(solvalues[j]);
235 |                     // printf("%.15f, ", solvalues[j]);
236 |                 // printf("\n");
237 |             }
238 | 
239 |         } 
240 |         else if( ee_pose.size() == 12 )  // ik, given rotation-translation matrix
241 |         {
242 | #if IK_VERSION > 54
243 |             // for IKFast 56,61
244 |             IkSolutionList<IKREAL_TYPE> solutions;
245 | #else
246 |             // for IKFast 54
247 |             std::vector<IKSolution> vsolutions;
248 | #endif
249 |             std::vector<IKREAL_TYPE> vfree(num_free_parameters);
250 | 
251 |             eerot[0] = ee_pose[0]; eerot[1] = ee_pose[1]; eerot[2] = ee_pose[2];  eetrans[0] = ee_pose[3];
252 |             eerot[3] = ee_pose[4]; eerot[4] = ee_pose[5]; eerot[5] = ee_pose[6];  eetrans[1] = ee_pose[7];
253 |             eerot[6] = ee_pose[8]; eerot[7] = ee_pose[9]; eerot[8] = ee_pose[10]; eetrans[2] = ee_pose[11];
254 |             // for(std::size_t i = 0; i < vfree.size(); ++i)
255 |             //     vfree[i] = atof(argv[13+i]);
256 | 
257 | #if IK_VERSION > 54
258 |             // for IKFast 56,61
259 |             bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
260 | #else
261 |             // for IKFast 54
262 |             bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
263 | #endif
264 |             if( !bSuccess ) {
265 |                 fprintf(stderr,"Error: (inverse kinematics) failed to get ik solution\n");
266 |                 return joint_configs;
267 |             }
268 | 
269 | #if IK_VERSION > 54
270 |             // for IKFast 56,61
271 |             unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
272 | #else
273 |             // for IKFast 54
274 |             unsigned int num_of_solutions = (int)vsolutions.size();
275 | #endif
276 |             // printf("Found %d ik solutions:\n", num_of_solutions ); 
277 | 
278 |             std::vector<IKREAL_TYPE> solvalues(num_of_joints);
279 |             for(std::size_t i = 0; i < num_of_solutions; ++i) {
280 | #if IK_VERSION > 54
281 |                 // for IKFast 56,61
282 |                 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
283 |                 int this_sol_free_params = (int)sol.GetFree().size(); 
284 | #else
285 |                 // for IKFast 54
286 |                 int this_sol_free_params = (int)vsolutions[i].GetFree().size();
287 | #endif
288 |                 // printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
289 |                 std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);
290 | 
291 | #if IK_VERSION > 54
292 |                 // for IKFast 56,61
293 |                 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
294 | #else
295 |                 // for IKFast 54
296 |                 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
297 | #endif
298 |                 for( std::size_t j = 0; j < solvalues.size(); ++j)
299 |                     joint_configs.push_back(solvalues[j]);
300 |                 //     printf("%.15f, ", solvalues[j]);
301 |                 // printf("\n");
302 |             }
303 | 
304 |         }
305 |         else {
306 |             printf("\nError: (inverse kinematics) please specify transformation of end effector with one of the following formats:\n"
307 |                    "    1) A vector of 7 values: a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k)\n"
308 |                    "    2) A (row-major) vector of 12 values: a 3x4 rigid transformation matrix with a 3x3 rotation R (rXX), and a 3x1 translation (tX)\n\n");
309 |             return joint_configs;
310 | 
311 |         }
312 | 
313 |         return joint_configs;
314 |     }
315 | }
316 | 
317 | int main(int argc, char** argv)
318 | {
319 |     IKREAL_TYPE eerot[9],eetrans[3];
320 | 
321 | #if IK_VERSION > 54
322 |     // for IKFast 56,61
323 |     unsigned int num_of_joints = GetNumJoints();
324 |     unsigned int num_free_parameters = GetNumFreeParameters();
325 | #else
326 |     // for IKFast 54
327 |     unsigned int num_of_joints = getNumJoints();
328 |     unsigned int num_free_parameters = getNumFreeParameters();
329 | #endif
330 |  
331 |     std::string cmd;
332 |     if (argv[1]) cmd = argv[1];
333 | 
334 |     //printf("command: %s \n\n", cmd.c_str() );
335 | 
336 |     if (cmd.compare("ik") == 0) // ik mode
337 |     {
338 |     } // endif ik mode
339 | 
340 |     else if (cmd.compare("fk") == 0) // fk mode
341 |     {
342 |         
343 | 
344 |     }
345 |     else if (cmd.compare("iktiming") == 0) // generate random ik and check time performance
346 |     {
347 |         if( argc != 2 ) {
348 |             printf("\n "
349 |                    "Usage: \n\n "
350 |                    "         ./compute iktiming \n\n"
351 |                    "         For fixed number of iterations, generates random joint angles, then  \n"
352 |                    "         calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 );
353 |             return 1;
354 |         }
355 |         printf("\n\n");
356 | 
357 | #if IK_VERSION > 54
358 |         // for IKFast 56,61
359 |         IkSolutionList<IKREAL_TYPE> solutions;
360 | #else
361 |         // for IKFast 54
362 |         std::vector<IKSolution> vsolutions;
363 | #endif
364 |         std::vector<IKREAL_TYPE> vfree(num_free_parameters);
365 | 
366 |         //for(std::size_t i = 0; i < vfree.size(); ++i)
367 |         //    vfree[i] = atof(argv[13+i]);
368 | 
369 |         srand( (unsigned)time(0) ); // seed random number generator
370 |         float min = -3.14;
371 |         float max = 3.14;
372 |         float rnd;
373 | 
374 |         IKREAL_TYPE joints[num_of_joints];
375 | 
376 |         timespec start_time, end_time;
377 |         unsigned int elapsed_time = 0;
378 |         unsigned int sum_time = 0;
379 | 
380 | #if IK_VERSION > 54
381 |         // for IKFast 56,61
382 |         unsigned int num_of_tests = 1000000; 
383 | #else
384 |         // for IKFast 54
385 |         unsigned int num_of_tests = 100000; 
386 | #endif
387 | 
388 |         for (unsigned int i=0; i < num_of_tests; i++)
389 |         {
390 |             // Measure avg time for whole process
391 |             //clock_gettime(CLOCK_REALTIME, &start_time); 
392 | 
393 |             // Put random joint values into array
394 |             for (unsigned int i=0; i<num_of_joints; i++)
395 |             {
396 |                 float rnd = (float)rand() / (float)RAND_MAX;
397 |                 joints[i] = min + rnd * (max - min);
398 |             }
399 |             /*
400 |             printf("Joint angles:  ");
401 |             for (unsigned int i=0; i<num_of_joints; i++)
402 |             {
403 |                 printf("%f  ", joints[i] );
404 |             }
405 |             printf("\n");
406 |             */
407 | 
408 | #if IK_VERSION > 54
409 |             // for IKFast 56,61
410 |             ComputeFk(joints, eetrans, eerot); // void return
411 | #else
412 |             // for IKFast 54
413 |             fk(joints, eetrans, eerot); // void return
414 | #endif
415 | 
416 |             // Measure avg time for IK
417 |             clock_gettime(CLOCK_REALTIME, &start_time);
418 | #if IK_VERSION > 54
419 |             // for IKFast 56,61
420 |             ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
421 | #else
422 |             // for IKFast 54
423 |             ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
424 | #endif
425 | 
426 |             /*
427 | #if IK_VERSION > 54
428 |             // for IKFast 56,61
429 |             unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
430 | #else
431 |             // for IKFast 54
432 |             unsigned int num_of_solutions = (int)vsolutions.size();
433 | #endif
434 |             printf("Found %d ik solutions:\n", num_of_solutions ); 
435 |             */
436 | 
437 |             clock_gettime(CLOCK_REALTIME, &end_time); 
438 |             elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec);
439 |             sum_time += elapsed_time;
440 |         } // endfor
441 | 
442 |         unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; 
443 |         printf("avg time: %f ms   over %d tests \n", (float)avg_time/1000.0, num_of_tests );
444 | 
445 |         return 1;
446 |   
447 |     }
448 |     else if (cmd.compare("iktiming2") == 0) // for fixed joint values, check time performance of ik
449 |     {
450 |         if( argc != 2 ) {
451 |             printf("\n "
452 |                    "Usage: \n\n "
453 |                    "         ./compute iktiming2 \n\n"
454 |                    "         For fixed number of iterations, with one set of joint variables, this  \n"
455 |                    "         finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 );
456 |             return 1;
457 |         }
458 |         printf("\n\n");
459 | 
460 | #if IK_VERSION > 54
461 |         // for IKFast 56,61
462 |         IkSolutionList<IKREAL_TYPE> solutions;
463 | #else
464 |         // for IKFast 54
465 |         std::vector<IKSolution> vsolutions;
466 | #endif
467 |         std::vector<IKREAL_TYPE> vfree(num_free_parameters);
468 | 
469 |         //for(std::size_t i = 0; i < vfree.size(); ++i)
470 |         //    vfree[i] = atof(argv[13+i]);
471 | 
472 |         IKREAL_TYPE joints[num_of_joints];
473 | 
474 |         timespec start_time, end_time;
475 |         unsigned int elapsed_time = 0;
476 |         unsigned int sum_time = 0;
477 | 
478 | #if IK_VERSION > 54
479 |         // for IKFast 56,61
480 |         unsigned int num_of_tests = 1000000; 
481 | #else
482 |         // for IKFast 54
483 |         unsigned int num_of_tests = 100000; 
484 | #endif
485 | 
486 |         // fixed rotation-translation matrix corresponding to an unusual robot pose
487 |         eerot[0] = 0.002569;  eerot[1] = -0.658044;  eerot[2] = -0.752975;  eetrans[0] = 0.121937;
488 |         eerot[3] = 0.001347;  eerot[4] = -0.752975;  eerot[5] = 0.658048;  eetrans[1] = -0.276022;
489 |         eerot[6] = -0.999996; eerot[7] = -0.002705; eerot[8] = -0.001047; eetrans[2] = 0.005685;
490 | 
491 |         for (unsigned int i=0; i < num_of_tests; i++)
492 |         {
493 |             clock_gettime(CLOCK_REALTIME, &start_time);
494 | 
495 | #if IK_VERSION > 54
496 |             // for IKFast 56,61
497 |             ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
498 | #else
499 |             // for IKFast 54
500 |             ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
501 | #endif
502 | 
503 |             /*
504 | #if IK_VERSION > 54
505 |             // for IKFast 56,61
506 |             unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
507 | #else
508 |             // for IKFast 54
509 |             unsigned int num_of_solutions = (int)vsolutions.size();
510 | #endif
511 |             printf("Found %d ik solutions:\n", num_of_solutions ); 
512 |             */
513 | 
514 |             clock_gettime(CLOCK_REALTIME, &end_time); 
515 |             elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec);
516 |             sum_time += elapsed_time;
517 |         } // endfor
518 | 
519 |         unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; 
520 |         printf("avg time: %f ms   over %d tests \n", (float)avg_time/1000.0, num_of_tests );
521 | 
522 |         return 1;
523 | 
524 |     } else {
525 |         printf("\n"
526 |                "Usage: \n\n");
527 |         printf("         ./compute fk j0 j1 ... j%d \n\n"
528 |                "         Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 );
529 |         printf("\n"
530 |                "         ./compute ik  t0 t1 t2  qw qi qj qk  free0 ... \n\n"
531 |                "         Returns the ik solutions given the transformation of the end effector specified by \n"
532 |                "         a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n"
533 |                "         There are %d free parameters that have to be specified. \n\n", num_free_parameters );
534 |         printf(" \n"
535 |                "         ./compute ik  r00 r01 r02 t0  r10 r11 r12 t1  r20 r21 r22 t2  free0 ...\n\n"
536 |                "         Returns the ik solutions given the transformation of the end effector specified by \n"
537 |                "         a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n"
538 |                "         There are %d free parameters that have to be specified. \n\n", num_free_parameters );
539 |         printf("\n"
540 |                "         ./compute iktiming \n\n"
541 |                "         For fixed number of iterations, generates random joint angles, then \n"
542 |                "         calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 );
543 |         printf("\n"
544 |                "         ./compute iktiming2 \n\n"
545 |                "         For fixed number of iterations, with one set of joint variables, this \n"
546 |                "         finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 );
547 | 
548 |         return 1;
549 |     }
550 | 
551 |     return 0;
552 | }
553 | 
554 | float SIGN(float x) {
555 |     return (x >= 0.0f) ? +1.0f : -1.0f;
556 | }
557 | 
558 | float NORM(float a, float b, float c, float d) {
559 |     return sqrt(a * a + b * b + c * c + d * d);
560 | }
561 | 
562 | 
563 | 


--------------------------------------------------------------------------------
/ikfastpy.pyx:
--------------------------------------------------------------------------------
 1 | #This is a Cython file and extracts the relevant classes from the C++ header file.
 2 | 
 3 | # distutils: language = c++
 4 | # distutils: sources = ikfast_wrapper.cpp
 5 | 
 6 | cdef extern from "<vector>" namespace "std":
 7 |     cdef cppclass vector[T]:
 8 |         cppclass iterator:
 9 |             T operator*()
10 |             iterator operator++()
11 |             bint operator==(iterator)
12 |             bint operator!=(iterator)
13 |         vector()
14 |         void push_back(T&)
15 |         T& operator[](int)
16 |         T& at(int)
17 |         iterator begin()
18 |         iterator end()
19 | 
20 | cdef extern from "Kinematics.hpp" namespace "robots":
21 |     cdef cppclass Kinematics:
22 |         Kinematics()
23 |         int num_of_joints, num_free_parameters
24 |         vector[float] forward(vector[float] joint_config);
25 |         vector[float] inverse(vector[float] ee_pose);
26 | 
27 | cdef class PyKinematics:
28 |     cdef Kinematics *thisptr      # hold a C++ instance which we're wrapping
29 |     def __cinit__(self):
30 |         self.thisptr = new Kinematics()
31 |     def __dealloc__(self):
32 |         del self.thisptr
33 |     def getDOF(self):
34 |         return self.thisptr.num_of_joints
35 |     def forward(self,vector[float] joint_config):
36 |         return self.thisptr.forward(joint_config)
37 |     def inverse(self,vector[float] ee_pose):
38 |         return self.thisptr.inverse(ee_pose)


--------------------------------------------------------------------------------
/images/closed-loop-grasping.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/andyzeng/ikfastpy/7d2154288ad993710854bc32bc85ca50bc6b9860/images/closed-loop-grasping.gif


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
1 | from distutils.core import setup
2 | from distutils.extension import Extension
3 | from Cython.Distutils import build_ext
4 | 
5 | setup(ext_modules=[Extension("ikfastpy", 
6 |                             ["ikfastpy.pyx", 
7 |                              "ikfast_wrapper.cpp"], language="c++", libraries=['lapack'])],
8 |       cmdclass = {'build_ext': build_ext})


--------------------------------------------------------------------------------
/ur5.robot.xml:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="utf-8"?>
  2 | 
  3 | <!-- UR5 kinematic body -->
  4 | 
  5 | <KinBody name="ur5">
  6 | 
  7 |   <Body name="link0" type="dynamic">
  8 |     <translation>0 0 0</translation>
  9 |     <rotationaxis>1 0 0 0</rotationaxis>
 10 |     <Geom type="box">
 11 |       <translation>0 0 0</translation>
 12 |       <Extents>0.05 0.03 0.01</Extents>
 13 |       <diffusecolor>1.0 0.341 0.349</diffusecolor>
 14 |     </Geom>
 15 |   </Body>
 16 | 
 17 |   <Body name="link1" type="dynamic">
 18 |     <offsetfrom>link0</offsetfrom>
 19 |     <translation>0 -0.13585 0.089159</translation>
 20 |     <quat>0 -0.7071 0 0.7071</quat>
 21 |     <Geom type="box">
 22 |       <translation>0 0 0</translation>
 23 |       <Extents>0.03 0.03 0.01</Extents>
 24 |       <diffusecolor>0.949 0.557 0.169</diffusecolor>
 25 |     </Geom>
 26 |   </Body>
 27 | 
 28 |   <Joint name="joint0" type="hinge">
 29 |     <Body>link0</Body>
 30 |     <Body>link1</Body>
 31 |     <offsetfrom>link0</offsetfrom>
 32 |     <axis>0 0 1</axis>
 33 |     <limitsdeg>-360 360</limitsdeg>
 34 |     <maxvel>3</maxvel>
 35 |     <weight>1</weight>
 36 |     <resolution>1</resolution>
 37 |   </Joint>
 38 | 
 39 |   <Body name="link2" type="dynamic">
 40 |     <offsetfrom>link1</offsetfrom>
 41 |     <translation>0 -0.1197 0.425</translation>
 42 |     <rotationaxis>1 0 0 0</rotationaxis>
 43 |     <Geom type="box">
 44 |       <translation>0 0 0</translation>
 45 |       <Extents>0.01 0.01 0.01</Extents>
 46 |       <diffusecolor>0.929 0.788 0.282</diffusecolor>
 47 |     </Geom>
 48 |   </Body>
 49 | 
 50 |   <Joint name="joint1" type="hinge">
 51 |     <Body>link1</Body>
 52 |     <Body>link2</Body>
 53 |     <offsetfrom>link1</offsetfrom>
 54 |     <axis>0 1 0</axis>
 55 |     <limitsdeg>-360 360</limitsdeg>
 56 |     <weight>1</weight>
 57 |     <maxvel>3</maxvel>
 58 |     <resolution>1</resolution>
 59 |   </Joint>
 60 | 
 61 |   <Body name="link3" type="dynamic">
 62 |     <offsetfrom>link2</offsetfrom>
 63 |     <translation>0 0 0.39225</translation>
 64 |     <rotationaxis>1 0 0 0</rotationaxis>
 65 |     <Geom type="box">
 66 |       <translation>0 0 0</translation>
 67 |       <Extents>0.01 0.01 0.01</Extents>
 68 |       <diffusecolor>0.349 0.631 0.310</diffusecolor>
 69 |     </Geom>
 70 |   </Body>
 71 | 
 72 |   <Joint name="joint2" type="hinge">
 73 |     <Body>link2</Body>
 74 |     <Body>link3</Body>
 75 |     <offsetfrom>link2</offsetfrom>
 76 |     <axis>0 1 0</axis>
 77 |     <limitsdeg>-360 360</limitsdeg>
 78 |     <weight>1</weight>
 79 |     <maxvel>3</maxvel>
 80 |     <resolution>1</resolution>
 81 |   </Joint>
 82 | 
 83 |   <Body name="link4" type="dynamic">
 84 |     <offsetfrom>link3</offsetfrom>
 85 |     <translation>0 0.093 0</translation>
 86 |     <rotationaxis>0 1 0 90</rotationaxis>
 87 |     <Geom type="box">
 88 |       <translation>0 0 0</translation>
 89 |       <Extents>0.01 0.01 0.01</Extents>
 90 |       <diffusecolor>0.306 0.475 0.655</diffusecolor>
 91 |     </Geom>
 92 |   </Body>
 93 | 
 94 |   <Joint name="joint3" type="hinge">
 95 |     <Body>link3</Body>
 96 |     <Body>link4</Body>
 97 |     <offsetfrom>link3</offsetfrom>
 98 |     <axis>0 1 0</axis>
 99 |     <limitsdeg>-360 360</limitsdeg>
100 |     <weight>1</weight>
101 |     <maxvel>3</maxvel>
102 |     <resolution>1</resolution>
103 |   </Joint>
104 | 
105 |   <Body name="link5" type="dynamic">
106 |     <offsetfrom>link4</offsetfrom>
107 |     <translation>0 0 0.09465</translation>
108 |     <rotationaxis>1 0 0 0</rotationaxis>
109 |     <Geom type="box">
110 |       <translation>0 0 0</translation>
111 |       <Extents>0.01 0.01 0.01</Extents>
112 |       <diffusecolor>0.690 0.478 0.631</diffusecolor>
113 |     </Geom>
114 |   </Body>
115 | 
116 |   <Joint name="joint4" type="hinge">
117 |     <Body>link4</Body>
118 |     <Body>link5</Body>
119 |     <offsetfrom>link4</offsetfrom>
120 |     <axis>0 0 1</axis>
121 |     <limitsdeg>-360 360</limitsdeg>
122 |     <weight>1</weight>
123 |     <maxvel>3</maxvel>
124 |     <resolution>1</resolution>
125 |   </Joint>
126 | 
127 |   <Body name="link6" type="dynamic">
128 |     <offsetfrom>link5</offsetfrom>
129 |     <translation>0 0.0823 0</translation> <!-- end effector offset can be added here -->
130 |     <rotationaxis>-1 0 0 90</rotationaxis>
131 |     <Geom type="box">
132 |       <translation>0 0 0</translation>
133 |       <Extents>0.01 0.01 0.01</Extents>
134 |       <diffusecolor>1.0 0.616 0.655</diffusecolor>
135 |     </Geom>
136 |   </Body>
137 | 
138 |   <Joint name="joint5" type="hinge">
139 |     <Body>link5</Body>
140 |     <Body>link6</Body>
141 |     <offsetfrom>link5</offsetfrom>
142 |     <axis>0 1 0</axis>
143 |     <limitsdeg>-360 360</limitsdeg>
144 |     <weight>1</weight>
145 |     <maxvel>3</maxvel>
146 |     <resolution>1</resolution>
147 |   </Joint>
148 | 
149 | </KinBody>
150 | 


--------------------------------------------------------------------------------
/ur5e.robot.xml:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="utf-8"?>
  2 | 
  3 | <!-- UR5 kinematic body -->
  4 | 
  5 | <KinBody name="ur5">
  6 | 
  7 |   <Body name="link0" type="dynamic">
  8 |     <translation>0 0 0</translation>
  9 |     <rotationaxis>1 0 0 0</rotationaxis>
 10 |     <Geom type="box">
 11 |       <translation>0 0 0</translation>
 12 |       <Extents>0.05 0.03 0.01</Extents>
 13 |       <diffusecolor>1.0 0.341 0.349</diffusecolor>
 14 |     </Geom>
 15 |   </Body>
 16 | 
 17 |   <Body name="link1" type="dynamic">
 18 |     <offsetfrom>link0</offsetfrom>
 19 |     <translation>0 -0.138 0.163</translation>
 20 |     <quat>0 -0.7071 0 0.7071</quat>
 21 |     <Geom type="box">
 22 |       <translation>0 0 0</translation>
 23 |       <Extents>0.03 0.03 0.01</Extents>
 24 |       <diffusecolor>0.949 0.557 0.169</diffusecolor>
 25 |     </Geom>
 26 |   </Body>
 27 | 
 28 |   <Joint name="joint1" type="hinge">
 29 |     <Body>link0</Body>
 30 |     <Body>link1</Body>
 31 |     <offsetfrom>link0</offsetfrom>
 32 |     <axis>0 0 1</axis>
 33 |     <limitsdeg>-360 360</limitsdeg>
 34 |     <maxvel>3</maxvel>
 35 |     <weight>1</weight>
 36 |     <resolution>1</resolution>
 37 |   </Joint>
 38 | 
 39 |   <Body name="link2" type="dynamic">
 40 |     <offsetfrom>link1</offsetfrom>
 41 |     <translation>0 -0.131 0.425</translation>
 42 |     <rotationaxis>1 0 0 0</rotationaxis>
 43 |     <Geom type="box">
 44 |       <translation>0 0 0</translation>
 45 |       <Extents>0.01 0.01 0.01</Extents>
 46 |       <diffusecolor>0.929 0.788 0.282</diffusecolor>
 47 |     </Geom>
 48 |   </Body>
 49 | 
 50 |   <Joint name="joint2" type="hinge">
 51 |     <Body>link1</Body>
 52 |     <Body>link2</Body>
 53 |     <offsetfrom>link1</offsetfrom>
 54 |     <axis>0 1 0</axis>
 55 |     <limitsdeg>-360 360</limitsdeg>
 56 |     <weight>1</weight>
 57 |     <maxvel>3</maxvel>
 58 |     <resolution>1</resolution>
 59 |   </Joint>
 60 | 
 61 |   <Body name="link3" type="dynamic">
 62 |     <offsetfrom>link2</offsetfrom>
 63 |     <translation>0 0 0.392</translation>
 64 |     <rotationaxis>1 0 0 0</rotationaxis>
 65 |     <Geom type="box">
 66 |       <translation>0 0 0</translation>
 67 |       <Extents>0.01 0.01 0.01</Extents>
 68 |       <diffusecolor>0.349 0.631 0.310</diffusecolor>
 69 |     </Geom>
 70 |   </Body>
 71 | 
 72 |   <Joint name="joint3" type="hinge">
 73 |     <Body>link2</Body>
 74 |     <Body>link3</Body>
 75 |     <offsetfrom>link2</offsetfrom>
 76 |     <axis>0 1 0</axis>
 77 |     <limitsdeg>-360 360</limitsdeg>
 78 |     <weight>1</weight>
 79 |     <maxvel>3</maxvel>
 80 |     <resolution>1</resolution>
 81 |   </Joint>
 82 | 
 83 |   <Body name="link4" type="dynamic">
 84 |     <offsetfrom>link3</offsetfrom>
 85 |     <translation>0 0.127 0</translation>
 86 |     <rotationaxis>0 1 0 90</rotationaxis>
 87 |     <Geom type="box">
 88 |       <translation>0 0 0</translation>
 89 |       <Extents>0.01 0.01 0.01</Extents>
 90 |       <diffusecolor>0.306 0.475 0.655</diffusecolor>
 91 |     </Geom>
 92 |   </Body>
 93 | 
 94 |   <Joint name="joint4" type="hinge">
 95 |     <Body>link3</Body>
 96 |     <Body>link4</Body>
 97 |     <offsetfrom>link3</offsetfrom>
 98 |     <axis>0 1 0</axis>
 99 |     <limitsdeg>-360 360</limitsdeg>
100 |     <weight>1</weight>
101 |     <maxvel>3</maxvel>
102 |     <resolution>1</resolution>
103 |   </Joint>
104 | 
105 |   <Body name="link5" type="dynamic">
106 |     <offsetfrom>link4</offsetfrom>
107 |     <translation>0 0 0.100</translation>
108 |     <rotationaxis>1 0 0 0</rotationaxis>
109 |     <Geom type="box">
110 |       <translation>0 0 0</translation>
111 |       <Extents>0.01 0.01 0.01</Extents>
112 |       <diffusecolor>0.690 0.478 0.631</diffusecolor>
113 |     </Geom>
114 |   </Body>
115 | 
116 |   <Joint name="joint5" type="hinge">
117 |     <Body>link4</Body>
118 |     <Body>link5</Body>
119 |     <offsetfrom>link4</offsetfrom>
120 |     <axis>0 0 1</axis>
121 |     <limitsdeg>-360 360</limitsdeg>
122 |     <weight>1</weight>
123 |     <maxvel>3</maxvel>
124 |     <resolution>1</resolution>
125 |   </Joint>
126 | 
127 |   <Body name="link6" type="dynamic">
128 |     <offsetfrom>link5</offsetfrom>
129 |     <translation>0 0.100 0</translation> <!-- end effector offset can be added here -->
130 |     <rotationaxis>-1 0 0 90</rotationaxis>
131 |     <Geom type="box">
132 |       <translation>0 0 0</translation>
133 |       <Extents>0.01 0.01 0.01</Extents>
134 |       <diffusecolor>1.0 0.616 0.655</diffusecolor>
135 |     </Geom>
136 |   </Body>
137 | 
138 |   <Joint name="joint6" type="hinge">
139 |     <Body>link5</Body>
140 |     <Body>link6</Body>
141 |     <offsetfrom>link5</offsetfrom>
142 |     <axis>0 1 0</axis>
143 |     <limitsdeg>-360 360</limitsdeg>
144 |     <weight>1</weight>
145 |     <maxvel>3</maxvel>
146 |     <resolution>1</resolution>
147 |   </Joint>
148 | 
149 | </KinBody>
150 | 


--------------------------------------------------------------------------------