1 // -*- coding: utf-8 -*- 2 // Copyright (C) 2012 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 #define IKFAST_VERSION 71 43 44 #ifdef _MSC_VER 45 //#ifndef isfinite 46 //#define isfinite _isfinite 47 //#endif 48 #endif // _MSC_VER 49 50 namespace ikfast { 51 52 /// \brief holds the solution for a single dof 53 template <typename T> 54 class IkSingleDOFSolutionBase 55 { 56 public: IkSingleDOFSolutionBase()57 IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) { 58 indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; 59 } 60 T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset 61 signed char freeind; ///< if >= 0, mimics another joint 62 unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider 63 unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself 64 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 65 }; 66 67 /// \brief The discrete solutions are returned in this structure. 68 /// 69 /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. 70 /// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is: 71 template <typename T> 72 class IkSolutionBase 73 { 74 public: ~IkSolutionBase()75 virtual ~IkSolutionBase() { 76 } 77 /// \brief gets a concrete solution 78 /// 79 /// \param[out] solution the result 80 /// \param[in] freevalues values for the free parameters \se GetFree 81 virtual void GetSolution(T* solution, const T* freevalues) const = 0; 82 83 /// \brief std::vector version of \ref GetSolution GetSolution(std::vector<T> & solution,const std::vector<T> & freevalues)84 virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const { 85 solution.resize(GetDOF()); 86 GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); 87 } 88 89 /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned 90 /// 91 /// 0 always points to the first value accepted by the ik function. 92 /// \return vector of indices indicating the free parameters 93 virtual const std::vector<int>& GetFree() const = 0; 94 95 /// \brief the dof of the solution 96 virtual int GetDOF() const = 0; 97 }; 98 99 /// \brief manages all the solutions 100 template <typename T> 101 class IkSolutionListBase 102 { 103 public: ~IkSolutionListBase()104 virtual ~IkSolutionListBase() { 105 } 106 107 /// \brief add one solution and return its index for later retrieval 108 /// 109 /// \param vinfos Solution data for each degree of freedom of the manipulator 110 /// \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. 111 virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0; 112 113 /// \brief returns the solution pointer 114 virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0; 115 116 /// \brief returns the number of solutions stored 117 virtual size_t GetNumSolutions() const = 0; 118 119 /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated. 120 virtual void Clear() = 0; 121 }; 122 123 /// \brief holds function pointers for all the exported functions of ikfast 124 template <typename T> 125 class IkFastFunctions 126 { 127 public: IkFastFunctions()128 IkFastFunctions() : _ComputeIk(NULL), _ComputeIk2(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) { 129 } ~IkFastFunctions()130 virtual ~IkFastFunctions() { 131 } 132 typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&); 133 ComputeIkFn _ComputeIk; 134 typedef bool (*ComputeIk2Fn)(const T*, const T*, const T*, IkSolutionListBase<T>&, void*); 135 ComputeIk2Fn _ComputeIk2; 136 typedef void (*ComputeFkFn)(const T*, T*, T*); 137 ComputeFkFn _ComputeFk; 138 typedef int (*GetNumFreeParametersFn)(); 139 GetNumFreeParametersFn _GetNumFreeParameters; 140 typedef int* (*GetFreeParametersFn)(); 141 GetFreeParametersFn _GetFreeParameters; 142 typedef int (*GetNumJointsFn)(); 143 GetNumJointsFn _GetNumJoints; 144 typedef int (*GetIkRealSizeFn)(); 145 GetIkRealSizeFn _GetIkRealSize; 146 typedef const char* (*GetIkFastVersionFn)(); 147 GetIkFastVersionFn _GetIkFastVersion; 148 typedef int (*GetIkTypeFn)(); 149 GetIkTypeFn _GetIkType; 150 typedef const char* (*GetKinematicsHashFn)(); 151 GetKinematicsHashFn _GetKinematicsHash; 152 }; 153 154 // Implementations of the abstract classes, user doesn't need to use them 155 156 /// \brief Default implementation of \ref IkSolutionBase 157 template <typename T> 158 class IkSolution : public IkSolutionBase<T> 159 { 160 public: IkSolution(const std::vector<IkSingleDOFSolutionBase<T>> & vinfos,const std::vector<int> & vfree)161 IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) { 162 _vbasesol = vinfos; 163 _vfree = vfree; 164 } 165 GetSolution(T * solution,const T * freevalues)166 virtual void GetSolution(T* solution, const T* freevalues) const { 167 for(std::size_t i = 0; i < _vbasesol.size(); ++i) { 168 if( _vbasesol[i].freeind < 0 ) 169 solution[i] = _vbasesol[i].foffset; 170 else { 171 solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset; 172 if( solution[i] > T(3.14159265358979) ) { 173 solution[i] -= T(6.28318530717959); 174 } 175 else if( solution[i] < T(-3.14159265358979) ) { 176 solution[i] += T(6.28318530717959); 177 } 178 } 179 } 180 } 181 GetSolution(std::vector<T> & solution,const std::vector<T> & freevalues)182 virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const { 183 solution.resize(GetDOF()); 184 GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); 185 } 186 GetFree()187 virtual const std::vector<int>& GetFree() const { 188 return _vfree; 189 } GetDOF()190 virtual int GetDOF() const { 191 return static_cast<int>(_vbasesol.size()); 192 } 193 Validate()194 virtual void Validate() const { 195 for(size_t i = 0; i < _vbasesol.size(); ++i) { 196 if( _vbasesol[i].maxsolutions == (unsigned char)-1) { 197 throw std::runtime_error("max solutions for joint not initialized"); 198 } 199 if( _vbasesol[i].maxsolutions > 0 ) { 200 if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) { 201 throw std::runtime_error("index >= max solutions for joint"); 202 } 203 if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) { 204 throw std::runtime_error("2nd index >= max solutions for joint"); 205 } 206 } 207 if( !std::isfinite(_vbasesol[i].foffset) ) { 208 throw std::runtime_error("foffset was not finite"); 209 } 210 } 211 } 212 GetSolutionIndices(std::vector<unsigned int> & v)213 virtual void GetSolutionIndices(std::vector<unsigned int>& v) const { 214 v.resize(0); 215 v.push_back(0); 216 for(int i = (int)_vbasesol.size()-1; i >= 0; --i) { 217 if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) { 218 for(size_t j = 0; j < v.size(); ++j) { 219 v[j] *= _vbasesol[i].maxsolutions; 220 } 221 size_t orgsize=v.size(); 222 if( _vbasesol[i].indices[1] != (unsigned char)-1 ) { 223 for(size_t j = 0; j < orgsize; ++j) { 224 v.push_back(v[j]+_vbasesol[i].indices[1]); 225 } 226 } 227 if( _vbasesol[i].indices[0] != (unsigned char)-1 ) { 228 for(size_t j = 0; j < orgsize; ++j) { 229 v[j] += _vbasesol[i].indices[0]; 230 } 231 } 232 } 233 } 234 } 235 236 std::vector< IkSingleDOFSolutionBase<T> > _vbasesol; ///< solution and their offsets if joints are mimiced 237 std::vector<int> _vfree; 238 }; 239 240 /// \brief Default implementation of \ref IkSolutionListBase 241 template <typename T> 242 class IkSolutionList : public IkSolutionListBase<T> 243 { 244 public: AddSolution(const std::vector<IkSingleDOFSolutionBase<T>> & vinfos,const std::vector<int> & vfree)245 virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) 246 { 247 size_t index = _listsolutions.size(); 248 _listsolutions.push_back(IkSolution<T>(vinfos,vfree)); 249 return index; 250 } 251 GetSolution(size_t index)252 virtual const IkSolutionBase<T>& GetSolution(size_t index) const 253 { 254 if( index >= _listsolutions.size() ) { 255 throw std::runtime_error("GetSolution index is invalid"); 256 } 257 typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin(); 258 std::advance(it,index); 259 return *it; 260 } 261 GetNumSolutions()262 virtual size_t GetNumSolutions() const { 263 return _listsolutions.size(); 264 } 265 Clear()266 virtual void Clear() { 267 _listsolutions.clear(); 268 } 269 270 protected: 271 std::list< IkSolution<T> > _listsolutions; 272 }; 273 274 } 275 276 #endif // OPENRAVE_IKFAST_HEADER 277 278 // The following code is dependent on the C++ library linking with. 279 #ifdef IKFAST_HAS_LIBRARY 280 281 // defined when creating a shared object/dll 282 #ifdef IKFAST_CLIBRARY 283 #ifdef _MSC_VER 284 #define IKFAST_API extern "C" __declspec(dllexport) 285 #else 286 #define IKFAST_API extern "C" 287 #endif 288 #else 289 #define IKFAST_API 290 #endif 291 292 #ifdef IKFAST_NAMESPACE 293 namespace IKFAST_NAMESPACE { 294 #endif 295 296 #ifdef IKFAST_REAL 297 typedef IKFAST_REAL IkReal; 298 #else 299 typedef double IkReal; 300 #endif 301 302 /** \brief Computes all IK solutions given a end effector coordinates and the free joints. 303 304 - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. 305 - ``eerot`` 306 - For **Transform6D** it is 9 values for the 3x3 rotation matrix. 307 - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. 308 - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle. 309 - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. 310 */ 311 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions); 312 313 /** \brief Similar to ComputeIk except takes OpenRAVE boost::shared_ptr<RobotBase::Manipulator>* 314 */ 315 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip); 316 317 /// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. 318 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); 319 320 /// \brief returns the number of free parameters users has to set apriori 321 IKFAST_API int GetNumFreeParameters(); 322 323 /// \brief the indices of the free parameters indexed by the chain joints 324 IKFAST_API int* GetFreeParameters(); 325 326 /// \brief the total number of indices of the chain 327 IKFAST_API int GetNumJoints(); 328 329 /// \brief the size in bytes of the configured number type 330 IKFAST_API int GetIkRealSize(); 331 332 /// \brief the ikfast version used to generate this file 333 IKFAST_API const char* GetIkFastVersion(); 334 335 /// \brief the ik type ID 336 IKFAST_API int GetIkType(); 337 338 /// \brief a hash of all the chain values used for double checking that the correct IK is used. 339 IKFAST_API const char* GetKinematicsHash(); 340 341 #ifdef IKFAST_NAMESPACE 342 } 343 #endif 344 345 #endif // IKFAST_HAS_LIBRARY 346