42#define IKFAST_VERSION 61
48class 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;
150 typedef int* (*GetFreeParametersFn)();
156 typedef const char* (*GetIkFastVersionFn)();
160 typedef const char* (*GetKinematicsHashFn)();
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 ==
static_cast<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)
241 if (
_vbasesol[i].maxsolutions !=
static_cast<unsigned char>(-1) &&
_vbasesol[i].maxsolutions > 1)
243 for (
size_t j = 0; j < v.size(); ++j)
247 size_t orgsize = v.size();
248 if (
_vbasesol[i].indices[1] !=
static_cast<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] !=
static_cast<unsigned char>(-1))
257 for (
size_t j = 0; j < orgsize; ++j)
266 std::vector<IkSingleDOFSolutionBase<T> >
_vbasesol;
272class 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
323namespace IKFAST_NAMESPACE
328typedef IKFAST_REAL IkReal;
330typedef double IkReal;
344IKFAST_API
bool ComputeIk(
const IkReal* eetrans,
const IkReal* eerot,
const IkReal* pfree,
348IKFAST_API
void ComputeFk(
const IkReal* joints, IkReal* eetrans, IkReal* eerot);
371#ifdef IKFAST_NAMESPACE
holds function pointers for all the exported functions of ikfast
const char *(* GetIkFastVersionFn)()
const char *(* GetKinematicsHashFn)()
GetIkFastVersionFn _GetIkFastVersion
GetFreeParametersFn _GetFreeParameters
int(* GetNumFreeParametersFn)()
void(* ComputeFkFn)(const T *, T *, T *)
GetIkRealSizeFn _GetIkRealSize
int *(* GetFreeParametersFn)()
virtual ~IkFastFunctions()
GetKinematicsHashFn _GetKinematicsHash
GetNumJointsFn _GetNumJoints
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 size_t GetNumSolutions() const =0
returns the number of solutions stored
virtual const IkSolutionBase< T > & GetSolution(size_t index) const =0
returns the solution pointer
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
std::list< IkSolution< T > > _listsolutions
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
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 mimicked
virtual void GetSolution(T *solution, const T *freevalues) const
gets a concrete solution
IKFAST_API int * GetFreeParameters()
IKFAST_API int GetNumJoints()
IKFAST_API const char * GetIkFastVersion()
IKFAST_API int GetIkRealSize()
IKFAST_API const char * GetKinematicsHash()
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 int GetNumFreeParameters()