ikfast.h
Go to the documentation of this file.
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.
35 #include <vector>
36 #include <list>
37 #include <stdexcept>
38 
39 #ifndef IKFAST_HEADER_COMMON
40 #define IKFAST_HEADER_COMMON
41 
43 #define IKFAST_VERSION 61
44 
45 namespace ikfast
46 {
48 template <typename T>
50 {
51 public:
53  {
54  indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
55  }
57  signed char freeind;
58  unsigned char jointtype;
59  unsigned char maxsolutions;
60  unsigned char indices[5];
61 };
63 
69 template <typename T>
71 {
72 public:
73  virtual ~IkSolutionBase()
74  {
75  }
80  virtual void GetSolution(T* solution, const T* freevalues) const = 0;
81 
83  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
84  {
85  solution.resize(GetDOF());
86  GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
87  }
88 
92  virtual const std::vector<int>& GetFree() const = 0;
93 
95  virtual const int GetDOF() const = 0;
96 };
97 
99 template <typename T>
101 {
102 public:
104  {
105  }
106 
112  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
113 
115  virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
116 
118  virtual size_t GetNumSolutions() const = 0;
119 
122  virtual void Clear() = 0;
123 };
124 
126 template <typename T>
128 {
129 public:
131  : _ComputeIk(NULL)
132  , _ComputeFk(NULL)
133  , _GetNumFreeParameters(NULL)
134  , _GetFreeParameters(NULL)
135  , _GetNumJoints(NULL)
136  , _GetIkRealSize(NULL)
137  , _GetIkFastVersion(NULL)
138  , _GetIkType(NULL)
139  , _GetKinematicsHash(NULL)
140  {
141  }
143  {
144  }
145  typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
146  ComputeIkFn _ComputeIk;
147  typedef void (*ComputeFkFn)(const T*, T*, T*);
148  ComputeFkFn _ComputeFk;
149  typedef int (*GetNumFreeParametersFn)();
150  GetNumFreeParametersFn _GetNumFreeParameters;
151  typedef int* (*GetFreeParametersFn)();
152  GetFreeParametersFn _GetFreeParameters;
153  typedef int (*GetNumJointsFn)();
154  GetNumJointsFn _GetNumJoints;
155  typedef int (*GetIkRealSizeFn)();
156  GetIkRealSizeFn _GetIkRealSize;
157  typedef const char* (*GetIkFastVersionFn)();
158  GetIkFastVersionFn _GetIkFastVersion;
159  typedef int (*GetIkTypeFn)();
160  GetIkTypeFn _GetIkType;
161  typedef const char* (*GetKinematicsHashFn)();
162  GetKinematicsHashFn _GetKinematicsHash;
163 };
164 
165 // Implementations of the abstract classes, user doesn't need to use them
166 
168 template <typename T>
169 class IkSolution : public IkSolutionBase<T>
170 {
171 public:
172  IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
173  {
174  _vbasesol = vinfos;
175  _vfree = vfree;
176  }
177 
178  virtual void GetSolution(T* solution, const T* freevalues) const
179  {
180  for (std::size_t i = 0; i < _vbasesol.size(); ++i)
181  {
182  if (_vbasesol[i].freeind < 0)
183  solution[i] = _vbasesol[i].foffset;
184  else
185  {
186  solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset;
187  if (solution[i] > T(3.14159265358979))
188  {
189  solution[i] -= T(6.28318530717959);
190  }
191  else if (solution[i] < T(-3.14159265358979))
192  {
193  solution[i] += T(6.28318530717959);
194  }
195  }
196  }
197  }
198 
199  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
200  {
201  solution.resize(GetDOF());
202  GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
203  }
204 
205  virtual const std::vector<int>& GetFree() const
206  {
207  return _vfree;
208  }
209  virtual const int GetDOF() const
210  {
211  return static_cast<int>(_vbasesol.size());
212  }
213 
214  virtual void Validate() const
215  {
216  for (size_t i = 0; i < _vbasesol.size(); ++i)
217  {
218  if (_vbasesol[i].maxsolutions == (unsigned char)-1)
219  {
220  throw std::runtime_error("max solutions for joint not initialized");
221  }
222  if (_vbasesol[i].maxsolutions > 0)
223  {
224  if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions)
225  {
226  throw std::runtime_error("index >= max solutions for joint");
227  }
228  if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions)
229  {
230  throw std::runtime_error("2nd index >= max solutions for joint");
231  }
232  }
233  }
234  }
235 
236  virtual void GetSolutionIndices(std::vector<unsigned int>& v) const
237  {
238  v.resize(0);
239  v.push_back(0);
240  for (int i = (int)_vbasesol.size() - 1; i >= 0; --i)
241  {
242  if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1)
243  {
244  for (size_t j = 0; j < v.size(); ++j)
245  {
246  v[j] *= _vbasesol[i].maxsolutions;
247  }
248  size_t orgsize = v.size();
249  if (_vbasesol[i].indices[1] != (unsigned char)-1)
250  {
251  for (size_t j = 0; j < orgsize; ++j)
252  {
253  v.push_back(v[j] + _vbasesol[i].indices[1]);
254  }
255  }
256  if (_vbasesol[i].indices[0] != (unsigned char)-1)
257  {
258  for (size_t j = 0; j < orgsize; ++j)
259  {
260  v[j] += _vbasesol[i].indices[0];
261  }
262  }
263  }
264  }
265  }
266 
267  std::vector<IkSingleDOFSolutionBase<T> > _vbasesol;
268  std::vector<int> _vfree;
269 };
270 
272 template <typename T>
274 {
275 public:
276  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
277  {
278  size_t index = _listsolutions.size();
279  _listsolutions.push_back(IkSolution<T>(vinfos, vfree));
280  return index;
281  }
282 
283  virtual const IkSolutionBase<T>& GetSolution(size_t index) const
284  {
285  if (index >= _listsolutions.size())
286  {
287  throw std::runtime_error("GetSolution index is invalid");
288  }
289  typename std::list<IkSolution<T> >::const_iterator it = _listsolutions.begin();
290  std::advance(it, index);
291  return *it;
292  }
293 
294  virtual size_t GetNumSolutions() const
295  {
296  return _listsolutions.size();
297  }
298 
299  virtual void Clear()
300  {
301  _listsolutions.clear();
302  }
303 
304 protected:
305  std::list<IkSolution<T> > _listsolutions;
306 };
307 }
308 
309 #endif // OPENRAVE_IKFAST_HEADER
310 
311 // The following code is dependent on the C++ library linking with.
312 #ifdef IKFAST_HAS_LIBRARY
313 
314 // defined when creating a shared object/dll
315 #ifdef IKFAST_CLIBRARY
316 #ifdef _MSC_VER
317 #define IKFAST_API extern "C" __declspec(dllexport)
318 #else
319 #define IKFAST_API extern "C"
320 #endif
321 #else
322 #define IKFAST_API
323 #endif
324 
325 #ifdef IKFAST_NAMESPACE
326 namespace IKFAST_NAMESPACE
327 {
328 #endif
329 
330 #ifdef IKFAST_REAL
331 typedef IKFAST_REAL IkReal;
332 #else
333 typedef double IkReal;
334 #endif
335 
347 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
349 
351 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
352 
354 IKFAST_API int GetNumFreeParameters();
355 
357 IKFAST_API int* GetFreeParameters();
358 
360 IKFAST_API int GetNumJoints();
361 
363 IKFAST_API int GetIkRealSize();
364 
366 IKFAST_API const char* GetIkFastVersion();
367 
369 IKFAST_API int GetIkType();
370 
372 IKFAST_API const char* GetKinematicsHash();
373 
374 #ifdef IKFAST_NAMESPACE
375 }
376 #endif
377 
378 #endif // IKFAST_HAS_LIBRARY
Definition: ikfast.h:45
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:283
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
GetFreeParametersFn _GetFreeParameters
Definition: ikfast.h:152
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
std::vector< int > _vfree
Definition: ikfast.h:268
unsigned char indices[5]
Definition: ikfast.h:60
virtual void GetSolution(T *solution, const T *freevalues) const
gets a concrete solution
Definition: ikfast.h:178
virtual ~IkFastFunctions()
Definition: ikfast.h:142
holds function pointers for all the exported functions of ikfast
Definition: ikfast.h:127
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:294
unsigned char maxsolutions
max possible indices, 0 if controlled by free index or a free joint itself
Definition: ikfast.h:59
GetIkFastVersionFn _GetIkFastVersion
Definition: ikfast.h:158
holds the solution for a single dof
Definition: ikfast.h:49
signed char freeind
if >= 0, mimics another joint
Definition: ikfast.h:57
The discrete solutions are returned in this structure.
Definition: ikfast.h:70
IKFAST_API int * GetFreeParameters()
virtual void GetSolutionIndices(std::vector< unsigned int > &v) const
Definition: ikfast.h:236
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
Definition: ikfast.h:299
IKFAST_API int GetNumJoints()
virtual const int GetDOF() const
the dof of the solution
Definition: ikfast.h:209
GetKinematicsHashFn _GetKinematicsHash
Definition: ikfast.h:162
unsigned int index
ComputeFkFn _ComputeFk
Definition: ikfast.h:148
GetNumFreeParametersFn _GetNumFreeParameters
Definition: ikfast.h:150
GetNumJointsFn _GetNumJoints
Definition: ikfast.h:154
Default implementation of IkSolutionBase.
Definition: ikfast.h:169
std::list< IkSolution< T > > _listsolutions
Definition: ikfast.h:305
IKFAST_API const char * GetKinematicsHash()
IKFAST_API int GetIkType()
virtual const std::vector< int > & GetFree() const
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
Definition: ikfast.h:205
ComputeIkFn _ComputeIk
Definition: ikfast.h:146
virtual ~IkSolutionBase()
Definition: ikfast.h:73
IKFAST_API int GetNumFreeParameters()
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
Definition: ikfast.h:199
T foffset
joint value is fmul*sol[freeind]+foffset
Definition: ikfast.h:56
IKFAST_API int GetIkRealSize()
std::vector< IkSingleDOFSolutionBase< T > > _vbasesol
solution and their offsets if joints are mimiced
Definition: ikfast.h:267
Default implementation of IkSolutionListBase.
Definition: ikfast.h:273
GetIkTypeFn _GetIkType
Definition: ikfast.h:160
virtual ~IkSolutionListBase()
Definition: ikfast.h:103
GetIkRealSizeFn _GetIkRealSize
Definition: ikfast.h:156
virtual void Validate() const
Definition: ikfast.h:214
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
Definition: ikfast.h:83
IKFAST_API const char * GetIkFastVersion()
manages all the solutions
Definition: ikfast.h:100
unsigned char jointtype
joint type, 0x01 is revolute, 0x11 is slider
Definition: ikfast.h:58
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)
add one solution and return its index for later retrieval
Definition: ikfast.h:276
IkSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)
Definition: ikfast.h:172


turtlebot_arm_ikfast_plugin
Author(s):
autogenerated on Fri Feb 7 2020 03:56:27