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.
00035 #include <vector>
00036 #include <list>
00037 #include <stdexcept>
00038 
00039 #ifndef IKFAST_HEADER_COMMON
00040 #define IKFAST_HEADER_COMMON
00041 
00043 #define IKFAST_VERSION 61
00044 
00045 namespace ikfast
00046 {
00048 template <typename T>
00049 class IkSingleDOFSolutionBase
00050 {
00051 public:
00052   IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1)
00053   {
00054     indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
00055   }
00056   T fmul, foffset;             
00057   signed char freeind;         
00058   unsigned char jointtype;     
00059   unsigned char maxsolutions;  
00060   unsigned char indices[5];  
00061 
00062 };
00063 
00069 template <typename T>
00070 class IkSolutionBase
00071 {
00072 public:
00073   virtual ~IkSolutionBase()
00074   {
00075   }
00080   virtual void GetSolution(T* solution, const T* freevalues) const = 0;
00081 
00083   virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
00084   {
00085     solution.resize(GetDOF());
00086     GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
00087   }
00088 
00092   virtual const std::vector<int>& GetFree() const = 0;
00093 
00095   virtual const int GetDOF() const = 0;
00096 };
00097 
00099 template <typename T>
00100 class IkSolutionListBase
00101 {
00102 public:
00103   virtual ~IkSolutionListBase()
00104   {
00105   }
00106 
00112   virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
00113 
00115   virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
00116 
00118   virtual size_t GetNumSolutions() const = 0;
00119 
00122   virtual void Clear() = 0;
00123 };
00124 
00126 template <typename T>
00127 class IkFastFunctions
00128 {
00129 public:
00130   IkFastFunctions()
00131     : _ComputeIk(NULL)
00132     , _ComputeFk(NULL)
00133     , _GetNumFreeParameters(NULL)
00134     , _GetFreeParameters(NULL)
00135     , _GetNumJoints(NULL)
00136     , _GetIkRealSize(NULL)
00137     , _GetIkFastVersion(NULL)
00138     , _GetIkType(NULL)
00139     , _GetKinematicsHash(NULL)
00140   {
00141   }
00142   virtual ~IkFastFunctions()
00143   {
00144   }
00145   typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
00146   ComputeIkFn _ComputeIk;
00147   typedef void (*ComputeFkFn)(const T*, T*, T*);
00148   ComputeFkFn _ComputeFk;
00149   typedef int (*GetNumFreeParametersFn)();
00150   GetNumFreeParametersFn _GetNumFreeParameters;
00151   typedef int* (*GetFreeParametersFn)();
00152   GetFreeParametersFn _GetFreeParameters;
00153   typedef int (*GetNumJointsFn)();
00154   GetNumJointsFn _GetNumJoints;
00155   typedef int (*GetIkRealSizeFn)();
00156   GetIkRealSizeFn _GetIkRealSize;
00157   typedef const char* (*GetIkFastVersionFn)();
00158   GetIkFastVersionFn _GetIkFastVersion;
00159   typedef int (*GetIkTypeFn)();
00160   GetIkTypeFn _GetIkType;
00161   typedef const char* (*GetKinematicsHashFn)();
00162   GetKinematicsHashFn _GetKinematicsHash;
00163 };
00164 
00165 // Implementations of the abstract classes, user doesn't need to use them
00166 
00168 template <typename T>
00169 class IkSolution : public IkSolutionBase<T>
00170 {
00171 public:
00172   IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
00173   {
00174     _vbasesol = vinfos;
00175     _vfree = vfree;
00176   }
00177 
00178   virtual void GetSolution(T* solution, const T* freevalues) const
00179   {
00180     for (std::size_t i = 0; i < _vbasesol.size(); ++i)
00181     {
00182       if (_vbasesol[i].freeind < 0)
00183         solution[i] = _vbasesol[i].foffset;
00184       else
00185       {
00186         solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset;
00187         if (solution[i] > T(3.14159265358979))
00188         {
00189           solution[i] -= T(6.28318530717959);
00190         }
00191         else if (solution[i] < T(-3.14159265358979))
00192         {
00193           solution[i] += T(6.28318530717959);
00194         }
00195       }
00196     }
00197   }
00198 
00199   virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
00200   {
00201     solution.resize(GetDOF());
00202     GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
00203   }
00204 
00205   virtual const std::vector<int>& GetFree() const
00206   {
00207     return _vfree;
00208   }
00209   virtual const int GetDOF() const
00210   {
00211     return static_cast<int>(_vbasesol.size());
00212   }
00213 
00214   virtual void Validate() const
00215   {
00216     for (size_t i = 0; i < _vbasesol.size(); ++i)
00217     {
00218       if (_vbasesol[i].maxsolutions == (unsigned char)-1)
00219       {
00220         throw std::runtime_error("max solutions for joint not initialized");
00221       }
00222       if (_vbasesol[i].maxsolutions > 0)
00223       {
00224         if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions)
00225         {
00226           throw std::runtime_error("index >= max solutions for joint");
00227         }
00228         if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions)
00229         {
00230           throw std::runtime_error("2nd index >= max solutions for joint");
00231         }
00232       }
00233     }
00234   }
00235 
00236   virtual void GetSolutionIndices(std::vector<unsigned int>& v) const
00237   {
00238     v.resize(0);
00239     v.push_back(0);
00240     for (int i = (int)_vbasesol.size() - 1; i >= 0; --i)
00241     {
00242       if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1)
00243       {
00244         for (size_t j = 0; j < v.size(); ++j)
00245         {
00246           v[j] *= _vbasesol[i].maxsolutions;
00247         }
00248         size_t orgsize = v.size();
00249         if (_vbasesol[i].indices[1] != (unsigned char)-1)
00250         {
00251           for (size_t j = 0; j < orgsize; ++j)
00252           {
00253             v.push_back(v[j] + _vbasesol[i].indices[1]);
00254           }
00255         }
00256         if (_vbasesol[i].indices[0] != (unsigned char)-1)
00257         {
00258           for (size_t j = 0; j < orgsize; ++j)
00259           {
00260             v[j] += _vbasesol[i].indices[0];
00261           }
00262         }
00263       }
00264     }
00265   }
00266 
00267   std::vector<IkSingleDOFSolutionBase<T> > _vbasesol;  
00268   std::vector<int> _vfree;
00269 };
00270 
00272 template <typename T>
00273 class IkSolutionList : public IkSolutionListBase<T>
00274 {
00275 public:
00276   virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
00277   {
00278     size_t index = _listsolutions.size();
00279     _listsolutions.push_back(IkSolution<T>(vinfos, vfree));
00280     return index;
00281   }
00282 
00283   virtual const IkSolutionBase<T>& GetSolution(size_t index) const
00284   {
00285     if (index >= _listsolutions.size())
00286     {
00287       throw std::runtime_error("GetSolution index is invalid");
00288     }
00289     typename std::list<IkSolution<T> >::const_iterator it = _listsolutions.begin();
00290     std::advance(it, index);
00291     return *it;
00292   }
00293 
00294   virtual size_t GetNumSolutions() const
00295   {
00296     return _listsolutions.size();
00297   }
00298 
00299   virtual void Clear()
00300   {
00301     _listsolutions.clear();
00302   }
00303 
00304 protected:
00305   std::list<IkSolution<T> > _listsolutions;
00306 };
00307 }
00308 
00309 #endif  // OPENRAVE_IKFAST_HEADER
00310 
00311 // The following code is dependent on the C++ library linking with.
00312 #ifdef IKFAST_HAS_LIBRARY
00313 
00314 // defined when creating a shared object/dll
00315 #ifdef IKFAST_CLIBRARY
00316 #ifdef _MSC_VER
00317 #define IKFAST_API extern "C" __declspec(dllexport)
00318 #else
00319 #define IKFAST_API extern "C"
00320 #endif
00321 #else
00322 #define IKFAST_API
00323 #endif
00324 
00325 #ifdef IKFAST_NAMESPACE
00326 namespace IKFAST_NAMESPACE
00327 {
00328 #endif
00329 
00330 #ifdef IKFAST_REAL
00331 typedef IKFAST_REAL IkReal;
00332 #else
00333 typedef double IkReal;
00334 #endif
00335 
00347 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
00348                           ikfast::IkSolutionListBase<IkReal>& solutions);
00349 
00351 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
00352 
00354 IKFAST_API int GetNumFreeParameters();
00355 
00357 IKFAST_API int* GetFreeParameters();
00358 
00360 IKFAST_API int GetNumJoints();
00361 
00363 IKFAST_API int GetIkRealSize();
00364 
00366 IKFAST_API const char* GetIkFastVersion();
00367 
00369 IKFAST_API int GetIkType();
00370 
00372 IKFAST_API const char* GetKinematicsHash();
00373 
00374 #ifdef IKFAST_NAMESPACE
00375 }
00376 #endif
00377 
00378 #endif  // IKFAST_HAS_LIBRARY


katana_moveit_ikfast_plugin
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:43:58