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.
33 #include <vector>
34 #include <list>
35 #include <stdexcept>
36 
37 #ifndef IKFAST_HEADER_COMMON
38 #define IKFAST_HEADER_COMMON
39 
41 #define IKFAST_VERSION 61
42 
43 namespace ikfast {
44 
46 template <typename T>
48 {
49 public:
51  indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
52  }
54  signed char freeind;
55  unsigned char jointtype;
56  unsigned char maxsolutions;
57  unsigned char indices[5];
58 };
59 
64 template <typename T>
66 {
67 public:
68  virtual ~IkSolutionBase() {
69  }
74  virtual void GetSolution(T* solution, const T* freevalues) const = 0;
75 
77  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
78  solution.resize(GetDOF());
79  GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
80  }
81 
85  virtual const std::vector<int>& GetFree() const = 0;
86 
88  virtual const int GetDOF() const = 0;
89 };
90 
92 template <typename T>
94 {
95 public:
96  virtual ~IkSolutionListBase() {
97  }
98 
103  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
104 
106  virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
107 
109  virtual size_t GetNumSolutions() const = 0;
110 
112  virtual void Clear() = 0;
113 };
114 
116 template <typename T>
118 {
119 public:
120  IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) {
121  }
122  virtual ~IkFastFunctions() {
123  }
124  typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
125  ComputeIkFn _ComputeIk;
126  typedef void (*ComputeFkFn)(const T*, T*, T*);
127  ComputeFkFn _ComputeFk;
128  typedef int (*GetNumFreeParametersFn)();
129  GetNumFreeParametersFn _GetNumFreeParameters;
130  typedef int* (*GetFreeParametersFn)();
131  GetFreeParametersFn _GetFreeParameters;
132  typedef int (*GetNumJointsFn)();
133  GetNumJointsFn _GetNumJoints;
134  typedef int (*GetIkRealSizeFn)();
135  GetIkRealSizeFn _GetIkRealSize;
136  typedef const char* (*GetIkFastVersionFn)();
137  GetIkFastVersionFn _GetIkFastVersion;
138  typedef int (*GetIkTypeFn)();
139  GetIkTypeFn _GetIkType;
140  typedef const char* (*GetKinematicsHashFn)();
141  GetKinematicsHashFn _GetKinematicsHash;
142 };
143 
144 // Implementations of the abstract classes, user doesn't need to use them
145 
147 template <typename T>
148 class IkSolution : public IkSolutionBase<T>
149 {
150 public:
151  IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) {
152  _vbasesol = vinfos;
153  _vfree = vfree;
154  }
155 
156  virtual void GetSolution(T* solution, const T* freevalues) const {
157  for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
158  if( _vbasesol[i].freeind < 0 )
159  solution[i] = _vbasesol[i].foffset;
160  else {
161  solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset;
162  if( solution[i] > T(3.14159265358979) ) {
163  solution[i] -= T(6.28318530717959);
164  }
165  else if( solution[i] < T(-3.14159265358979) ) {
166  solution[i] += T(6.28318530717959);
167  }
168  }
169  }
170  }
171 
172  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
173  solution.resize(GetDOF());
174  GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
175  }
176 
177  virtual const std::vector<int>& GetFree() const {
178  return _vfree;
179  }
180  virtual const int GetDOF() const {
181  return static_cast<int>(_vbasesol.size());
182  }
183 
184  virtual void Validate() const {
185  for(size_t i = 0; i < _vbasesol.size(); ++i) {
186  if( _vbasesol[i].maxsolutions == (unsigned char)-1) {
187  throw std::runtime_error("max solutions for joint not initialized");
188  }
189  if( _vbasesol[i].maxsolutions > 0 ) {
190  if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) {
191  throw std::runtime_error("index >= max solutions for joint");
192  }
193  if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) {
194  throw std::runtime_error("2nd index >= max solutions for joint");
195  }
196  }
197  }
198  }
199 
200  virtual void GetSolutionIndices(std::vector<unsigned int>& v) const {
201  v.resize(0);
202  v.push_back(0);
203  for(int i = (int)_vbasesol.size()-1; i >= 0; --i) {
204  if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) {
205  for(size_t j = 0; j < v.size(); ++j) {
206  v[j] *= _vbasesol[i].maxsolutions;
207  }
208  size_t orgsize=v.size();
209  if( _vbasesol[i].indices[1] != (unsigned char)-1 ) {
210  for(size_t j = 0; j < orgsize; ++j) {
211  v.push_back(v[j]+_vbasesol[i].indices[1]);
212  }
213  }
214  if( _vbasesol[i].indices[0] != (unsigned char)-1 ) {
215  for(size_t j = 0; j < orgsize; ++j) {
216  v[j] += _vbasesol[i].indices[0];
217  }
218  }
219  }
220  }
221  }
222 
223  std::vector< IkSingleDOFSolutionBase<T> > _vbasesol;
224  std::vector<int> _vfree;
225 };
226 
228 template <typename T>
230 {
231 public:
232  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
233  {
234  size_t index = _listsolutions.size();
235  _listsolutions.push_back(IkSolution<T>(vinfos,vfree));
236  return index;
237  }
238 
239  virtual const IkSolutionBase<T>& GetSolution(size_t index) const
240  {
241  if( index >= _listsolutions.size() ) {
242  throw std::runtime_error("GetSolution index is invalid");
243  }
244  typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin();
245  std::advance(it,index);
246  return *it;
247  }
248 
249  virtual size_t GetNumSolutions() const {
250  return _listsolutions.size();
251  }
252 
253  virtual void Clear() {
254  _listsolutions.clear();
255  }
256 
257 protected:
258  std::list< IkSolution<T> > _listsolutions;
259 };
260 
261 }
262 
263 #endif // OPENRAVE_IKFAST_HEADER
264 
265 // The following code is dependent on the C++ library linking with.
266 #ifdef IKFAST_HAS_LIBRARY
267 
268 // defined when creating a shared object/dll
269 #ifdef IKFAST_CLIBRARY
270 #ifdef _MSC_VER
271 #define IKFAST_API extern "C" __declspec(dllexport)
272 #else
273 #define IKFAST_API extern "C"
274 #endif
275 #else
276 #define IKFAST_API
277 #endif
278 
279 #ifdef IKFAST_NAMESPACE
280 namespace IKFAST_NAMESPACE {
281 #endif
282 
283 #ifdef IKFAST_REAL
284 typedef IKFAST_REAL IkReal;
285 #else
286 typedef double IkReal;
287 #endif
288 
298 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
299 
301 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
302 
304 IKFAST_API int GetNumFreeParameters();
305 
307 IKFAST_API int* GetFreeParameters();
308 
310 IKFAST_API int GetNumJoints();
311 
313 IKFAST_API int GetIkRealSize();
314 
316 IKFAST_API const char* GetIkFastVersion();
317 
319 IKFAST_API int GetIkType();
320 
322 IKFAST_API const char* GetKinematicsHash();
323 
324 #ifdef IKFAST_NAMESPACE
325 }
326 #endif
327 
328 #endif // IKFAST_HAS_LIBRARY
Definition: ikfast.h:43
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:239
IKFAST_API int GetIkRealSize()
GetFreeParametersFn _GetFreeParameters
Definition: ikfast.h:131
IKFAST_API int GetIkType()
std::vector< int > _vfree
Definition: ikfast.h:224
unsigned char indices[5]
unique index of the solution used to keep track on what part it came from. sometimes a solution can b...
Definition: ikfast.h:57
virtual void GetSolution(T *solution, const T *freevalues) const
gets a concrete solution
Definition: ikfast.h:156
virtual ~IkFastFunctions()
Definition: ikfast.h:122
holds function pointers for all the exported functions of ikfast
Definition: ikfast.h:117
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:249
unsigned char maxsolutions
max possible indices, 0 if controlled by free index or a free joint itself
Definition: ikfast.h:56
GetIkFastVersionFn _GetIkFastVersion
Definition: ikfast.h:137
holds the solution for a single dof
Definition: ikfast.h:47
signed char freeind
if >= 0, mimics another joint
Definition: ikfast.h:54
The discrete solutions are returned in this structure.
Definition: ikfast.h:65
virtual void GetSolutionIndices(std::vector< unsigned int > &v) const
Definition: ikfast.h:200
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
Definition: ikfast.h:253
std::vector< IkSingleDOFSolutionBase< T > > _vbasesol
solution and their offsets if joints are mimiced
Definition: ikfast.h:223
virtual const int GetDOF() const
the dof of the solution
Definition: ikfast.h:180
GetKinematicsHashFn _GetKinematicsHash
Definition: ikfast.h:141
unsigned int index
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
ComputeFkFn _ComputeFk
Definition: ikfast.h:127
std::list< IkSolution< T > > _listsolutions
Definition: ikfast.h:258
GetNumFreeParametersFn _GetNumFreeParameters
Definition: ikfast.h:129
IKFAST_API int * GetFreeParameters()
GetNumJointsFn _GetNumJoints
Definition: ikfast.h:133
Default implementation of IkSolutionBase.
Definition: ikfast.h:148
IKFAST_API int GetNumFreeParameters()
IKFAST_API int GetNumJoints()
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:177
ComputeIkFn _ComputeIk
Definition: ikfast.h:125
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
virtual ~IkSolutionBase()
Definition: ikfast.h:68
IKFAST_API const char * GetKinematicsHash()
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
Definition: ikfast.h:172
T foffset
joint value is fmul*sol[freeind]+foffset
Definition: ikfast.h:53
Default implementation of IkSolutionListBase.
Definition: ikfast.h:229
GetIkTypeFn _GetIkType
Definition: ikfast.h:139
virtual ~IkSolutionListBase()
Definition: ikfast.h:96
GetIkRealSizeFn _GetIkRealSize
Definition: ikfast.h:135
virtual void Validate() const
Definition: ikfast.h:184
IKFAST_API const char * GetIkFastVersion()
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
Definition: ikfast.h:77
manages all the solutions
Definition: ikfast.h:93
unsigned char jointtype
joint type, 0x01 is revolute, 0x11 is slider
Definition: ikfast.h:55
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:232
IkSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)
Definition: ikfast.h:151


moveit_opw_kinematics_plugin
Author(s): Jeroen De Maeyer, Simon Schmeisser (isys vision)
autogenerated on Wed Jun 3 2020 03:17:14