ChainIkSolverPos_ID.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __CHAIN_IK_SOLVER_POS_ID_HPP__
#define __CHAIN_IK_SOLVER_POS_ID_HPP__
 
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/jntarray.hpp>
 
namespace roboticslab
{
 
class ChainIkSolverPos_ID : public KDL::ChainIkSolverPos
{
public:
    ChainIkSolverPos_ID(const KDL::Chain & chain, const KDL::JntArray & q_min, const KDL::JntArray & q_max, KDL::ChainFkSolverPos & fksolver);
 
    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 const int E_FKSOLVERPOS_FAILED = -100;
 
    static const int E_JACSOLVER_FAILED = -101;
 
private:
    KDL::JntArray computeDiffInvKin(const KDL::Twist & delta_twist);
 
    const KDL::Chain & chain;
    unsigned int nj;
 
    KDL::JntArray qMin;
    KDL::JntArray qMax;
 
    KDL::ChainFkSolverPos & fkSolverPos;
    KDL::ChainJntToJacSolver jacSolver;
 
    KDL::Jacobian jacobian;
};
 
} // namespace roboticslab
 
#endif // __CHAIN_IK_SOLVER_POS_ID_HPP__