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);
374 #ifdef IKFAST_NAMESPACE 378 #endif // IKFAST_HAS_LIBRARY
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
IKFAST_API const char * GetIkFastVersion()
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
IKFAST_API int GetIkRealSize()
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
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
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
IKFAST_API int GetNumJoints()
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
IKFAST_API int * GetFreeParameters()
GetNumFreeParametersFn _GetNumFreeParameters
IKFAST_API int GetIkType()
GetNumJointsFn _GetNumJoints
Default implementation of IkSolutionBase.
std::list< IkSolution< T > > _listsolutions
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...
IKFAST_API const char * GetKinematicsHash()
virtual ~IkSolutionBase()
IKFAST_API int GetNumFreeParameters()
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)