chainiksolvervel_pinv_givens.hpp

// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
 
#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
 
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
 
#include <Eigen/Core>
 
namespace KDL
{
    class ChainIkSolverVel_pinv_givens : public ChainIkSolverVel
    {
    public:

        explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
        ~ChainIkSolverVel_pinv_givens();
 
        virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
        virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};

        virtual void updateInternalDataStructures();
 
    private:
        const Chain& chain;
        unsigned int nj;
        ChainJntToJacSolver jnt2jac;
        Jacobian jac;
        bool transpose,toggle;
        unsigned int m,n;
        Eigen::MatrixXd jac_eigen,U,V,B;
        Eigen::VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen;
    };
}
#endif