42 #define IKFAST_VERSION 61
48 class IkSingleDOFSolutionBase
79 virtual void GetSolution(T* solution,
const T* freevalues)
const = 0;
82 virtual void GetSolution(std::vector<T>& solution,
const std::vector<T>& freevalues)
const
85 GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) :
nullptr);
91 virtual const std::vector<int>&
GetFree()
const = 0;
125 template <
typename T>
150 typedef int* (*GetFreeParametersFn)();
156 typedef const char* (*GetIkFastVersionFn)();
160 typedef const char* (*GetKinematicsHashFn)();
167 template <
typename T>
179 for (std::size_t i = 0; i <
_vbasesol.size(); ++i)
186 if (solution[i] > T(3.14159265358979))
188 solution[i] -= T(6.28318530717959);
190 else if (solution[i] < T(-3.14159265358979))
192 solution[i] += T(6.28318530717959);
198 virtual void GetSolution(std::vector<T>& solution,
const std::vector<T>& freevalues)
const
200 solution.resize(
GetDOF());
201 GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) :
nullptr);
204 virtual const std::vector<int>&
GetFree()
const
210 return static_cast<int>(
_vbasesol.size());
215 for (
size_t i = 0; i <
_vbasesol.size(); ++i)
217 if (
_vbasesol[i].maxsolutions == (
unsigned char)-1)
219 throw std::runtime_error(
"max solutions for joint not initialized");
225 throw std::runtime_error(
"index >= max solutions for joint");
229 throw std::runtime_error(
"2nd index >= max solutions for joint");
239 for (
int i =
static_cast<int>(
_vbasesol.size()) - 1; i >= 0; --i)
243 for (
size_t j = 0; j < v.size(); ++j)
247 size_t orgsize = v.size();
248 if (
_vbasesol[i].indices[1] != (
unsigned char)-1)
250 for (
size_t j = 0; j < orgsize; ++j)
252 v.push_back(v[j] +
_vbasesol[i].indices[1]);
255 if (
_vbasesol[i].indices[0] != (
unsigned char)-1)
257 for (
size_t j = 0; j < orgsize; ++j)
266 std::vector<IkSingleDOFSolutionBase<T> >
_vbasesol;
271 template <
typename T>
272 class IkSolutionList :
public IkSolutionListBase<T>
286 throw std::runtime_error(
"GetSolution index is invalid");
288 typename std::list<IkSolution<T> >::const_iterator it =
_listsolutions.begin();
289 std::advance(it, index);
309 #ifdef IKFAST_HAS_LIBRARY
312 #ifdef IKFAST_CLIBRARY
314 #define IKFAST_API extern "C" __declspec(dllexport)
316 #define IKFAST_API extern "C"
322 #ifdef IKFAST_NAMESPACE
323 namespace IKFAST_NAMESPACE
328 typedef IKFAST_REAL IkReal;
330 typedef double IkReal;
344 IKFAST_API
bool ComputeIk(
const IkReal* eetrans,
const IkReal* eerot,
const IkReal* pfree,
348 IKFAST_API
void ComputeFk(
const IkReal* joints, IkReal* eetrans, IkReal* eerot);
371 #ifdef IKFAST_NAMESPACE
holds function pointers for all the exported functions of ikfast
GetIkFastVersionFn _GetIkFastVersion
GetFreeParametersFn _GetFreeParameters
const char *(* GetKinematicsHashFn)()
const char *(* GetIkFastVersionFn)()
int(* GetNumFreeParametersFn)()
void(* ComputeFkFn)(const T *, T *, T *)
GetIkRealSizeFn _GetIkRealSize
virtual ~IkFastFunctions()
GetKinematicsHashFn _GetKinematicsHash
GetNumJointsFn _GetNumJoints
int *(* GetFreeParametersFn)()
GetNumFreeParametersFn _GetNumFreeParameters
bool(* ComputeIkFn)(const T *, const T *, const T *, IkSolutionListBase< T > &)
holds the solution for a single dof
T foffset
joint value is fmul*sol[freeind]+foffset
unsigned char jointtype
joint type, 0x01 is revolute, 0x11 is slider
unsigned char maxsolutions
max possible indices, 0 if controlled by free index or a free joint itself
IkSingleDOFSolutionBase()
signed char freeind
if >= 0, mimics another joint
The discrete solutions are returned in this structure.
virtual const std::vector< int > & GetFree() const =0
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
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
virtual int GetDOF() const =0
the dof of the solution
virtual ~IkSolutionBase()
manages all the solutions
virtual ~IkSolutionListBase()
virtual const IkSolutionBase< T > & GetSolution(size_t index) const =0
returns the solution pointer
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
std::list< IkSolution< T > > _listsolutions
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
virtual size_t GetNumSolutions() const
returns the number of solutions stored
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
Default implementation of IkSolutionBase.
virtual void Validate() const
virtual int GetDOF() const
the dof of the solution
virtual void GetSolutionIndices(std::vector< unsigned int > &v) const
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...
IkSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)
std::vector< int > _vfree
virtual void GetSolution(std::vector< T > &solution, const std::vector< T > &freevalues) const
std::vector version of GetSolution
std::vector< IkSingleDOFSolutionBase< T > > _vbasesol
solution and their offsets if joints are mimiced
virtual void GetSolution(T *solution, const T *freevalues) const
gets a concrete solution
IKFAST_API int * GetFreeParameters()
IKFAST_API int GetNumJoints()
IKFAST_API int GetIkRealSize()
IKFAST_API int GetIkType()
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
IKFAST_API const char * GetIkFastVersion()
IKFAST_API int GetNumFreeParameters()
IKFAST_API const char * GetKinematicsHash()