openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 62
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 
86  virtual const std::vector<int>& GetFree() const = 0;
87 
89  virtual const int GetDOF() const = 0;
90 };
91 
93 template <typename T>
95 {
96 public:
97  virtual ~IkSolutionListBase() {
98  }
99 
104  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
105 
107  virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
108 
110  virtual size_t GetNumSolutions() const = 0;
111 
113  virtual void Clear() = 0;
114 };
115 
117 template <typename T>
119 {
120 public:
122  }
123  virtual ~IkFastFunctions() {
124  }
125  typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
127  typedef void (*ComputeFkFn)(const T*, T*, T*);
129  typedef int (*GetNumFreeParametersFn)();
131  typedef int* (*GetFreeParametersFn)();
133  typedef int (*GetNumJointsFn)();
135  typedef int (*GetIkRealSizeFn)();
137  typedef const char* (*GetIkFastVersionFn)();
139  typedef int (*GetIkTypeFn)();
141  typedef const char* (*GetKinematicsHashFn)();
143 };
144 
145 // Implementations of the abstract classes, user doesn't need to use them
146 
148 template <typename T>
149 class IkSolution : public IkSolutionBase<T>
150 {
151 public:
152  IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) {
153  _vbasesol = vinfos;
154  _vfree = vfree;
155  }
156 
157  virtual void GetSolution(T* solution, const T* freevalues) const {
158  for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
159  if( _vbasesol[i].freeind < 0 )
160  solution[i] = _vbasesol[i].foffset;
161  else {
162  solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset;
163  if( solution[i] > T(3.14159265358979) ) {
164  solution[i] -= T(6.28318530717959);
165  }
166  else if( solution[i] < T(-3.14159265358979) ) {
167  solution[i] += T(6.28318530717959);
168  }
169  }
170  }
171  }
172 
173  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
174  solution.resize(GetDOF());
175  GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
176  }
177 
178  virtual const std::vector<int>& GetFree() const {
179  return _vfree;
180  }
181  virtual const int GetDOF() const {
182  return static_cast<int>(_vbasesol.size());
183  }
184 
185  virtual void Validate() const {
186  for(size_t i = 0; i < _vbasesol.size(); ++i) {
187  if( _vbasesol[i].maxsolutions == (unsigned char)-1) {
188  throw std::runtime_error("max solutions for joint not initialized");
189  }
190  if( _vbasesol[i].maxsolutions > 0 ) {
191  if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) {
192  throw std::runtime_error("index >= max solutions for joint");
193  }
194  if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) {
195  throw std::runtime_error("2nd index >= max solutions for joint");
196  }
197  }
198  }
199  }
200 
201  virtual void GetSolutionIndices(std::vector<unsigned int>& v) const {
202  v.resize(0);
203  v.push_back(0);
204  for(int i = (int)_vbasesol.size()-1; i >= 0; --i) {
205  if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) {
206  for(size_t j = 0; j < v.size(); ++j) {
207  v[j] *= _vbasesol[i].maxsolutions;
208  }
209  size_t orgsize=v.size();
210  if( _vbasesol[i].indices[1] != (unsigned char)-1 ) {
211  for(size_t j = 0; j < orgsize; ++j) {
212  v.push_back(v[j]+_vbasesol[i].indices[1]);
213  }
214  }
215  if( _vbasesol[i].indices[0] != (unsigned char)-1 ) {
216  for(size_t j = 0; j < orgsize; ++j) {
217  v[j] += _vbasesol[i].indices[0];
218  }
219  }
220  }
221  }
222  }
223 
224  std::vector< IkSingleDOFSolutionBase<T> > _vbasesol;
225  std::vector<int> _vfree;
226 };
227 
229 template <typename T>
231 {
232 public:
233  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
234  {
235  size_t index = _listsolutions.size();
236  _listsolutions.push_back(IkSolution<T>(vinfos,vfree));
237  return index;
238  }
239 
240  virtual const IkSolutionBase<T>& GetSolution(size_t index) const
241  {
242  if( index >= _listsolutions.size() ) {
243  throw std::runtime_error("GetSolution index is invalid");
244  }
245  typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin();
246  std::advance(it,index);
247  return *it;
248  }
249 
250  virtual size_t GetNumSolutions() const {
251  return _listsolutions.size();
252  }
253 
254  virtual void Clear() {
255  _listsolutions.clear();
256  }
257 
258 protected:
259  std::list< IkSolution<T> > _listsolutions;
260 };
261 
262 }
263 
264 #endif // OPENRAVE_IKFAST_HEADER
265 
266 // The following code is dependent on the C++ library linking with.
267 #ifdef IKFAST_HAS_LIBRARY
268 
269 // defined when creating a shared object/dll
270 #ifdef IKFAST_CLIBRARY
271 #ifdef _MSC_VER
272 #define IKFAST_API extern "C" __declspec(dllexport)
273 #else
274 #define IKFAST_API extern "C"
275 #endif
276 #else
277 #define IKFAST_API
278 #endif
279 
280 #ifdef IKFAST_NAMESPACE
281 namespace IKFAST_NAMESPACE {
282 #endif
283 
284 #ifdef IKFAST_REAL
285 typedef IKFAST_REAL IkReal;
286 #else
287 typedef double IkReal;
288 #endif
289 
299 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
300 
302 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
303 
305 IKFAST_API int GetNumFreeParameters();
306 
308 IKFAST_API int* GetFreeParameters();
309 
311 IKFAST_API int GetNumJoints();
312 
314 IKFAST_API int GetIkRealSize();
315 
317 IKFAST_API const char* GetIkFastVersion();
318 
320 IKFAST_API int GetIkType();
321 
323 IKFAST_API const char* GetKinematicsHash();
324 
325 #ifdef IKFAST_NAMESPACE
326 }
327 #endif
328 
329 #endif // IKFAST_HAS_LIBRARY