ikfast.h

Go to the documentation of this file.

// -*- coding: utf-8 -*-
// Copyright (C) 2012 Rosen Diankov <rosen.diankov@gmail.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// LCOV_EXCL_START
#include <vector>
#include <list>
#include <stdexcept>
#include <cassert>
 
#ifndef IKFAST_HEADER_COMMON
#define IKFAST_HEADER_COMMON
 
#define IKFAST_VERSION 61
 
namespace ikfast
{
template <typename T>
class IkSingleDOFSolutionBase
{
public:
  IkSingleDOFSolutionBase() { indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; }  // NOLINT
  T fmul{ 0 };                      
  T foffset{ 0 };                   
  signed char freeind{ -1 };        
  unsigned char jointtype{ 1 };     
  unsigned char maxsolutions{ 1 };  
  unsigned char indices[5];  // NOLINT  ///< unique index of the solution used to keep track on what part it came from.
                             // sometimes a
};
 
template <typename T>
class IkSolutionBase  // NOLINT
{
public:
  virtual ~IkSolutionBase() = default;
  virtual void GetSolution(T* solution, const T* freevalues) const = 0;
 
  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
  {
    solution.resize(GetDOF());
    if (freevalues.empty())
      GetSolution(&solution.at(0), nullptr);
    else
      GetSolution(&solution.at(0), &freevalues.at(0));
  }
 
  virtual const std::vector<int>& GetFree() const = 0;
 
  virtual int GetDOF() const = 0;
};
 
template <typename T>
class IkSolutionListBase  // NOLINT
{
public:
  virtual ~IkSolutionListBase() = default;
 
  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
 
  virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
 
  virtual size_t GetNumSolutions() const = 0;
 
  virtual void Clear() = 0;
};
 
template <typename T>
class IkFastFunctions  // NOLINT
{
public:
  IkFastFunctions() = default;
  virtual ~IkFastFunctions() = default;
  using ComputeIkFn = bool (*)(const T*, const T*, const T*, IkSolutionListBase<T>&);
  ComputeIkFn _ComputeIk{ nullptr };
  using ComputeFkFn = void (*)(const T*, T*, T*);
  ComputeFkFn _ComputeFk{ nullptr };
  using GetNumFreeParametersFn = int (*)();
  GetNumFreeParametersFn _GetNumFreeParameters{ nullptr };
  using GetFreeParametersFn = int* (*)();
  GetFreeParametersFn _GetFreeParameters{ nullptr };
  using GetNumJointsFn = int (*)();
  GetNumJointsFn _GetNumJoints{ nullptr };
  using GetIkRealSizeFn = int (*)();
  GetIkRealSizeFn _GetIkRealSize{ nullptr };
  using GetIkFastVersionFn = const char* (*)();
  GetIkFastVersionFn _GetIkFastVersion{ nullptr };
  using GetIkTypeFn = int (*)();
  GetIkTypeFn _GetIkType{ nullptr };
  using GetKinematicsHashFn = const char* (*)();
  GetKinematicsHashFn _GetKinematicsHash{ nullptr };
};
 
// Implementations of the abstract classes, user doesn't need to use them
 
template <typename T>
class IkSolution : public IkSolutionBase<T>
{
public:
  IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
  {
    _vbasesol = vinfos;
    _vfree = vfree;
  }
 
  void GetSolution(T* solution, const T* freevalues) const override
  {
    for (std::size_t i = 0; i < _vbasesol.size(); ++i)
    {
      if (_vbasesol[i].freeind < 0)
        solution[i] = _vbasesol[i].foffset;
      else
      {
        assert(freevalues != nullptr);
        solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset;  // NOLINT
        if (solution[i] > T(3.14159265358979))
        {
          solution[i] -= T(6.28318530717959);
        }
        else if (solution[i] < T(-3.14159265358979))
        {
          solution[i] += T(6.28318530717959);
        }
      }
    }
  }
 
  void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const override
  {
    solution.resize(GetDOF());
    if (freevalues.empty())
      GetSolution(&solution.at(0), nullptr);
    else
      GetSolution(&solution.at(0), &freevalues.at(0));
  }
 
  const std::vector<int>& GetFree() const override { return _vfree; }
  int GetDOF() const override { return static_cast<int>(_vbasesol.size()); }
 
  virtual void Validate() const
  {
    for (size_t i = 0; i < _vbasesol.size(); ++i)
    {
      if (_vbasesol[i].maxsolutions == (unsigned char)-1)
      {
        throw std::runtime_error("max solutions for joint not initialized");
      }
      if (_vbasesol[i].maxsolutions > 0)
      {
        if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions)
        {
          throw std::runtime_error("index >= max solutions for joint");
        }
        if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions)
        {
          throw std::runtime_error("2nd index >= max solutions for joint");
        }
      }
    }
  }
 
  virtual void GetSolutionIndices(std::vector<unsigned int>& v) const
  {
    v.resize(0);
    v.push_back(0);
    for (int i = (int)_vbasesol.size() - 1; i >= 0; --i)
    {
      if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1)
      {
        for (size_t j = 0; j < v.size(); ++j)  // NOLINT
        {
          v[j] *= _vbasesol[i].maxsolutions;
        }
        size_t orgsize = v.size();
        if (_vbasesol[i].indices[1] != (unsigned char)-1)
        {
          for (size_t j = 0; j < orgsize; ++j)
          {
            v.push_back(v[j] + _vbasesol[i].indices[1]);
          }
        }
        if (_vbasesol[i].indices[0] != (unsigned char)-1)
        {
          for (size_t j = 0; j < orgsize; ++j)
          {
            v[j] += _vbasesol[i].indices[0];
          }
        }
      }
    }
  }
 
  std::vector<IkSingleDOFSolutionBase<T> > _vbasesol;  
  std::vector<int> _vfree;
};
 
template <typename T>
class IkSolutionList : public IkSolutionListBase<T>
{
public:
  size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) override
  {
    size_t index = _listsolutions.size();
    _listsolutions.push_back(IkSolution<T>(vinfos, vfree));
    return index;
  }
 
  const IkSolutionBase<T>& GetSolution(size_t index) const override
  {
    if (index >= _listsolutions.size())
    {
      throw std::runtime_error("GetSolution index is invalid");
    }
    auto it = _listsolutions.begin();
    std::advance(it, index);
    return *it;
  }
 
  size_t GetNumSolutions() const override { return _listsolutions.size(); }
 
  void Clear() override { _listsolutions.clear(); }
 
protected:
  std::list<IkSolution<T> > _listsolutions;
};
}  // namespace ikfast
 
#endif  // OPENRAVE_IKFAST_HEADER
 
// The following code is dependent on the C++ library linking with.
#ifdef IKFAST_HAS_LIBRARY
 
// defined when creating a shared object/dll
#ifdef IKFAST_CLIBRARY
#ifdef _MSC_VER
#define IKFAST_API extern "C" __declspec(dllexport)
#else
#define IKFAST_API extern "C" __attribute__((visibility("default")))
#endif
#else
#define IKFAST_API
#endif
 
#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE
{
#endif
 
#ifdef IKFAST_REAL
typedef IKFAST_REAL IkReal;
#else
using IkReal = double;
#endif
 
IKFAST_API bool ComputeIk(const IkReal* eetrans,
                          const IkReal* eerot,
                          const IkReal* pfree,
                          ikfast::IkSolutionListBase<IkReal>& solutions);
 
IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
 
IKFAST_API int GetNumFreeParameters();
 
IKFAST_API int* GetFreeParameters();
 
IKFAST_API int GetNumJoints();
 
IKFAST_API int GetIkRealSize();
 
IKFAST_API const char* GetIkFastVersion();
 
IKFAST_API int GetIkType();
 
IKFAST_API const char* GetKinematicsHash();
 
#ifdef IKFAST_NAMESPACE
}
#endif
 
#endif  // IKFAST_HAS_LIBRARY
// LCOV_EXCL_STOP