ikfast.h
Go to the documentation of this file.
00001 // -*- coding: utf-8 -*-
00002 // Copyright (C) 2012 Rosen Diankov <rosen.diankov@gmail.com>
00003 //
00004 // Licensed under the Apache License, Version 2.0 (the "License");
00005 // you may not use this file except in compliance with the License.
00006 // You may obtain a copy of the License at
00007 //     http://www.apache.org/licenses/LICENSE-2.0
00008 //
00009 // Unless required by applicable law or agreed to in writing, software
00010 // distributed under the License is distributed on an "AS IS" BASIS,
00011 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00012 // See the License for the specific language governing permissions and
00013 // limitations under the License.
00033 #include <vector>
00034 #include <list>
00035 #include <stdexcept>
00036 
00037 #ifndef IKFAST_HEADER_COMMON
00038 #define IKFAST_HEADER_COMMON
00039 
00041 #define IKFAST_VERSION 61
00042 
00043 namespace ikfast {
00044 
00046 template <typename T>
00047 class IkSingleDOFSolutionBase
00048 {
00049 public:
00050     IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) {
00051         indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
00052     }
00053     T fmul, foffset; 
00054     signed char freeind; 
00055     unsigned char jointtype; 
00056     unsigned char maxsolutions; 
00057     unsigned char indices[5]; 
00058 };
00059 
00064 template <typename T>
00065 class IkSolutionBase
00066 {
00067 public:
00068     virtual ~IkSolutionBase() {
00069     }
00074     virtual void GetSolution(T* solution, const T* freevalues) const = 0;
00075 
00077     virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
00078         solution.resize(GetDOF());
00079         GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
00080     }
00081 
00085     virtual const std::vector<int>& GetFree() const = 0;
00086 
00088     virtual const int GetDOF() const = 0;
00089 };
00090 
00092 template <typename T>
00093 class IkSolutionListBase
00094 {
00095 public:
00096     virtual ~IkSolutionListBase() {
00097     }
00098 
00103     virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
00104 
00106     virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
00107 
00109     virtual size_t GetNumSolutions() const = 0;
00110 
00112     virtual void Clear() = 0;
00113 };
00114 
00116 template <typename T>
00117 class IkFastFunctions
00118 {
00119 public:
00120     IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) {
00121     }
00122     virtual ~IkFastFunctions() {
00123     }
00124     typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
00125     ComputeIkFn _ComputeIk;
00126     typedef void (*ComputeFkFn)(const T*, T*, T*);
00127     ComputeFkFn _ComputeFk;
00128     typedef int (*GetNumFreeParametersFn)();
00129     GetNumFreeParametersFn _GetNumFreeParameters;
00130     typedef int* (*GetFreeParametersFn)();
00131     GetFreeParametersFn _GetFreeParameters;
00132     typedef int (*GetNumJointsFn)();
00133     GetNumJointsFn _GetNumJoints;
00134     typedef int (*GetIkRealSizeFn)();
00135     GetIkRealSizeFn _GetIkRealSize;
00136     typedef const char* (*GetIkFastVersionFn)();
00137     GetIkFastVersionFn _GetIkFastVersion;
00138     typedef int (*GetIkTypeFn)();
00139     GetIkTypeFn _GetIkType;
00140     typedef const char* (*GetKinematicsHashFn)();
00141     GetKinematicsHashFn _GetKinematicsHash;
00142 };
00143 
00144 // Implementations of the abstract classes, user doesn't need to use them
00145 
00147 template <typename T>
00148 class IkSolution : public IkSolutionBase<T>
00149 {
00150 public:
00151     IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) {
00152         _vbasesol = vinfos;
00153         _vfree = vfree;
00154     }
00155 
00156     virtual void GetSolution(T* solution, const T* freevalues) const {
00157         for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
00158             if( _vbasesol[i].freeind < 0 )
00159                 solution[i] = _vbasesol[i].foffset;
00160             else {
00161                 solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset;
00162                 if( solution[i] > T(3.14159265358979) ) {
00163                     solution[i] -= T(6.28318530717959);
00164                 }
00165                 else if( solution[i] < T(-3.14159265358979) ) {
00166                     solution[i] += T(6.28318530717959);
00167                 }
00168             }
00169         }
00170     }
00171 
00172     virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
00173         solution.resize(GetDOF());
00174         GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
00175     }
00176 
00177     virtual const std::vector<int>& GetFree() const {
00178         return _vfree;
00179     }
00180     virtual const int GetDOF() const {
00181         return static_cast<int>(_vbasesol.size());
00182     }
00183 
00184     virtual void Validate() const {
00185         for(size_t i = 0; i < _vbasesol.size(); ++i) {
00186             if( _vbasesol[i].maxsolutions == (unsigned char)-1) {
00187                 throw std::runtime_error("max solutions for joint not initialized");
00188             }
00189             if( _vbasesol[i].maxsolutions > 0 ) {
00190                 if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) {
00191                     throw std::runtime_error("index >= max solutions for joint");
00192                 }
00193                 if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) {
00194                     throw std::runtime_error("2nd index >= max solutions for joint");
00195                 }
00196             }
00197         }
00198     }
00199 
00200     virtual void GetSolutionIndices(std::vector<unsigned int>& v) const {
00201         v.resize(0);
00202         v.push_back(0);
00203         for(int i = (int)_vbasesol.size()-1; i >= 0; --i) {
00204             if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) {
00205                 for(size_t j = 0; j < v.size(); ++j) {
00206                     v[j] *= _vbasesol[i].maxsolutions;
00207                 }
00208                 size_t orgsize=v.size();
00209                 if( _vbasesol[i].indices[1] != (unsigned char)-1 ) {
00210                     for(size_t j = 0; j < orgsize; ++j) {
00211                         v.push_back(v[j]+_vbasesol[i].indices[1]);
00212                     }
00213                 }
00214                 if( _vbasesol[i].indices[0] != (unsigned char)-1 ) {
00215                     for(size_t j = 0; j < orgsize; ++j) {
00216                         v[j] += _vbasesol[i].indices[0];
00217                     }
00218                 }
00219             }
00220         }
00221     }
00222 
00223     std::vector< IkSingleDOFSolutionBase<T> > _vbasesol;       
00224     std::vector<int> _vfree;
00225 };
00226 
00228 template <typename T>
00229 class IkSolutionList : public IkSolutionListBase<T>
00230 {
00231 public:
00232     virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
00233     {
00234         size_t index = _listsolutions.size();
00235         _listsolutions.push_back(IkSolution<T>(vinfos,vfree));
00236         return index;
00237     }
00238 
00239     virtual const IkSolutionBase<T>& GetSolution(size_t index) const
00240     {
00241         if( index >= _listsolutions.size() ) {
00242             throw std::runtime_error("GetSolution index is invalid");
00243         }
00244         typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin();
00245         std::advance(it,index);
00246         return *it;
00247     }
00248 
00249     virtual size_t GetNumSolutions() const {
00250         return _listsolutions.size();
00251     }
00252 
00253     virtual void Clear() {
00254         _listsolutions.clear();
00255     }
00256 
00257 protected:
00258     std::list< IkSolution<T> > _listsolutions;
00259 };
00260 
00261 }
00262 
00263 #endif // OPENRAVE_IKFAST_HEADER
00264 
00265 // The following code is dependent on the C++ library linking with.
00266 #ifdef IKFAST_HAS_LIBRARY
00267 
00268 // defined when creating a shared object/dll
00269 #ifdef IKFAST_CLIBRARY
00270 #ifdef _MSC_VER
00271 #define IKFAST_API extern "C" __declspec(dllexport)
00272 #else
00273 #define IKFAST_API extern "C"
00274 #endif
00275 #else
00276 #define IKFAST_API
00277 #endif
00278 
00279 #ifdef IKFAST_NAMESPACE
00280 namespace IKFAST_NAMESPACE {
00281 #endif
00282 
00283 #ifdef IKFAST_REAL
00284 typedef IKFAST_REAL IkReal;
00285 #else
00286 typedef double IkReal;
00287 #endif
00288 
00298 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
00299 
00301 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
00302 
00304 IKFAST_API int GetNumFreeParameters();
00305 
00307 IKFAST_API int* GetFreeParameters();
00308 
00310 IKFAST_API int GetNumJoints();
00311 
00313 IKFAST_API int GetIkRealSize();
00314 
00316 IKFAST_API const char* GetIkFastVersion();
00317 
00319 IKFAST_API int GetIkType();
00320 
00322 IKFAST_API const char* GetKinematicsHash();
00323 
00324 #ifdef IKFAST_NAMESPACE
00325 }
00326 #endif
00327 
00328 #endif // IKFAST_HAS_LIBRARY


fanuc_m430ia_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Wed Aug 2 2017 03:13:50