ChainIkSolverPos_ST.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __CHAIN_IK_SOLVER_POS_ST_HPP__
#define __CHAIN_IK_SOLVER_POS_ST_HPP__
 
#include <kdl/chainiksolver.hpp>
 
#include "ScrewTheoryIkProblem.hpp"
#include "ConfigurationSelector.hpp"
 
namespace roboticslab
{
 
class ChainIkSolverPos_ST : public KDL::ChainIkSolverPos
{
public:
    virtual ~ChainIkSolverPos_ST();
 
    int CartToJnt(const KDL::JntArray & q_init, const KDL::Frame & p_in, KDL::JntArray & q_out) override;
 
    void updateInternalDataStructures() override;
 
    const char * strError(const int error) const override;
 
    static KDL::ChainIkSolverPos * create(const KDL::Chain & chain, const ConfigurationSelectorFactory & configFactory);
 
    static const int E_SOLUTION_NOT_FOUND = -100;
 
    static const int E_OUT_OF_LIMITS = -101;
 
    static const int E_NOT_REACHABLE = 100;
 
private:
    ChainIkSolverPos_ST(const KDL::Chain & chain, ScrewTheoryIkProblem * problem, ConfigurationSelector * config);
 
    const KDL::Chain & chain;
 
    ScrewTheoryIkProblem * problem;
 
    ConfigurationSelector * config;
};
 
} // namespace roboticslab
 
#endif // __CHAIN_IK_SOLVER_POS_ST_HPP__