lrmate200id_kinematics/include/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>
47 class IkSingleDOFSolutionBase
48 {
49 public:
51  indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
52  }
53  T fmul, foffset;
54  signed char freeind;
55  unsigned char jointtype;
56  unsigned char maxsolutions;
57  unsigned char indices[5];
58 };
59 
64 template <typename T>
65 class IkSolutionBase
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>
117 class IkFastFunctions
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>
229 class IkSolutionList : public IkSolutionListBase<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
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
unsigned char indices[5]
unique index of the solution used to keep track on what part it came from. sometimes a solution can b...
virtual void GetSolution(T *solution, const T *freevalues) const
gets a concrete solution
holds function pointers for all the exported functions of ikfast
virtual size_t GetNumSolutions() const
returns the number of solutions stored
unsigned char maxsolutions
max possible indices, 0 if controlled by free index or a free joint itself
signed char freeind
if >= 0, mimics another joint
The discrete solutions are returned in this structure.
virtual void GetSolutionIndices(std::vector< unsigned int > &v) const
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
virtual const int GetDOF() const
the dof of the solution
unsigned int index
Default implementation of IkSolutionBase.
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
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...
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
T foffset
joint value is fmul*sol[freeind]+foffset
Default implementation of IkSolutionListBase.
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
unsigned char jointtype
joint type, 0x01 is revolute, 0x11 is slider
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
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
IkSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)


fanuc_lrmate200id_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Tue Mar 23 2021 02:10:01