39 #ifndef IKFAST_HEADER_COMMON 40 #define IKFAST_HEADER_COMMON 43 #define IKFAST_VERSION 61 80 virtual void GetSolution(T* solution,
const T* freevalues)
const = 0;
83 virtual void GetSolution(std::vector<T>& solution,
const std::vector<T>& freevalues)
const 85 solution.resize(GetDOF());
86 GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
92 virtual const std::vector<int>& GetFree()
const = 0;
95 virtual const int GetDOF()
const = 0;
118 virtual size_t GetNumSolutions()
const = 0;
122 virtual void Clear() = 0;
126 template <
typename T>
133 , _GetNumFreeParameters(NULL)
134 , _GetFreeParameters(NULL)
135 , _GetNumJoints(NULL)
136 , _GetIkRealSize(NULL)
137 , _GetIkFastVersion(NULL)
139 , _GetKinematicsHash(NULL)
147 typedef void (*ComputeFkFn)(
const T*, T*, T*);
149 typedef int (*GetNumFreeParametersFn)();
151 typedef int* (*GetFreeParametersFn)();
153 typedef int (*GetNumJointsFn)();
155 typedef int (*GetIkRealSizeFn)();
157 typedef const char* (*GetIkFastVersionFn)();
159 typedef int (*GetIkTypeFn)();
161 typedef const char* (*GetKinematicsHashFn)();
168 template <
typename T>
180 for (std::size_t i = 0; i < _vbasesol.size(); ++i)
183 solution[i] = _vbasesol[i].foffset;
186 solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset;
187 if (solution[i] > T(3.14159265358979))
189 solution[i] -= T(6.28318530717959);
191 else if (solution[i] < T(-3.14159265358979))
193 solution[i] += T(6.28318530717959);
199 virtual void GetSolution(std::vector<T>& solution,
const std::vector<T>& freevalues)
const 201 solution.resize(GetDOF());
202 GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
205 virtual const std::vector<int>&
GetFree()
const 211 return static_cast<int>(_vbasesol.size());
216 for (
size_t i = 0; i < _vbasesol.size(); ++i)
220 throw std::runtime_error(
"max solutions for joint not initialized");
226 throw std::runtime_error(
"index >= max solutions for joint");
228 if (_vbasesol[i].
indices[1] != (
unsigned char)-1 && _vbasesol[i].
indices[1] >= _vbasesol[i].maxsolutions)
230 throw std::runtime_error(
"2nd index >= max solutions for joint");
240 for (
int i = (
int)_vbasesol.size() - 1; i >= 0; --i)
242 if (_vbasesol[i].
maxsolutions != (
unsigned char)-1 && _vbasesol[i].maxsolutions > 1)
244 for (
size_t j = 0; j < v.size(); ++j)
246 v[j] *= _vbasesol[i].maxsolutions;
248 size_t orgsize = v.size();
249 if (_vbasesol[i].
indices[1] != (
unsigned char)-1)
251 for (
size_t j = 0; j < orgsize; ++j)
253 v.push_back(v[j] + _vbasesol[i].
indices[1]);
256 if (_vbasesol[i].
indices[0] != (
unsigned char)-1)
258 for (
size_t j = 0; j < orgsize; ++j)
260 v[j] += _vbasesol[i].indices[0];
272 template <
typename T>
278 size_t index = _listsolutions.size();
285 if (index >= _listsolutions.size())
287 throw std::runtime_error(
"GetSolution index is invalid");
289 typename std::list<IkSolution<T> >::const_iterator it = _listsolutions.begin();
290 std::advance(it, index);
296 return _listsolutions.size();
301 _listsolutions.clear();
309 #endif // OPENRAVE_IKFAST_HEADER 312 #ifdef IKFAST_HAS_LIBRARY 315 #ifdef IKFAST_CLIBRARY 317 #define IKFAST_API extern "C" __declspec(dllexport) 319 #define IKFAST_API extern "C" 325 #ifdef IKFAST_NAMESPACE 326 namespace IKFAST_NAMESPACE
331 typedef IKFAST_REAL IkReal;
333 typedef double IkReal;
347 IKFAST_API
bool ComputeIk(
const IkReal* eetrans,
const IkReal* eerot,
const IkReal* pfree,
351 IKFAST_API
void ComputeFk(
const IkReal* joints, IkReal* eetrans, IkReal* eerot);
354 IKFAST_API
int GetNumFreeParameters();
357 IKFAST_API
int* GetFreeParameters();
360 IKFAST_API
int GetNumJoints();
363 IKFAST_API
int GetIkRealSize();
366 IKFAST_API
const char* GetIkFastVersion();
369 IKFAST_API
int GetIkType();
372 IKFAST_API
const char* GetKinematicsHash();
374 #ifdef IKFAST_NAMESPACE 378 #endif // IKFAST_HAS_LIBRARY
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
GetFreeParametersFn _GetFreeParameters
std::vector< int > _vfree
virtual void GetSolution(T *solution, const T *freevalues) const
gets a concrete solution
virtual ~IkFastFunctions()
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
GetIkFastVersionFn _GetIkFastVersion
holds the solution for a single dof
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
GetKinematicsHashFn _GetKinematicsHash
GetNumFreeParametersFn _GetNumFreeParameters
GetNumJointsFn _GetNumJoints
Default implementation of IkSolutionBase.
std::list< IkSolution< T > > _listsolutions
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 ~IkSolutionBase()
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
std::vector< IkSingleDOFSolutionBase< T > > _vbasesol
solution and their offsets if joints are mimiced
Default implementation of IkSolutionListBase.
virtual ~IkSolutionListBase()
GetIkRealSizeFn _GetIkRealSize
virtual void Validate() const
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
IkSingleDOFSolutionBase()
manages all the solutions
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
IkSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)