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