chainiksolverpos_lma.hpp

#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
#define KDL_CHAINIKSOLVERPOS_GN_HPP
 
/**************************************************************************
    begin                : May 2012
    copyright            : (C) 2012 Erwin Aertbelien
    email                : firstname.lastname@mech.kuleuven.ac.be
 
 History (only major changes)( AUTHOR-Description ) :
 
 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/
 
 
#include "chainiksolver.hpp"
#include "chain.hpp"
#include <Eigen/Dense>
 
namespace KDL
{

class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
{
private:
    typedef double ScalarType;
    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
public:
 
    static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
    static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;

    ChainIkSolverPos_LMA(
            const KDL::Chain& _chain,
        const Eigen::Matrix<double,6,1>& _l,
            double _eps=1E-5,
            int _maxiter=500,
            double _eps_joints=1E-15
    );

    ChainIkSolverPos_LMA(
            const KDL::Chain& _chain,
            double _eps=1E-5,
            int _maxiter=500,
            double _eps_joints=1E-15
    );

    virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);

    virtual ~ChainIkSolverPos_LMA();

    void compute_fwdpos(const VectorXq& q);

    void compute_jacobian(const VectorXq& q);

    void display_jac(const KDL::JntArray& jval);

    void updateInternalDataStructures();

    virtual const char* strError(const int error) const;
 
private:
    const KDL::Chain& chain;
    unsigned int nj;
    unsigned int ns;
 
public:
 

    int lastNrOfIter;

    double lastDifference;

    double lastTransDiff;

    double lastRotDiff;

    VectorXq lastSV;

    MatrixXq jac;

    VectorXq grad;
    KDL::Frame T_base_head;

    bool display_information;
private:
    // additional specification of the inverse position kinematics problem:
    unsigned int maxiter;
    double eps;
    double eps_joints;
    Eigen::Matrix<ScalarType,6,1> L;
 
 
 
    // state of compute_fwdpos and compute_jacobian:
    std::vector<KDL::Frame> T_base_jointroot;
    std::vector<KDL::Frame> T_base_jointtip;
                    // need 2 vectors because of the somewhat strange definition of segment.hpp
                    // you could also recompute jointtip out of jointroot,
                    // but then you'll need more expensive cos/sin functions.
 
 
    // the following are state of CartToJnt that is pre-allocated:
 
    VectorXq q;
    MatrixXq A;
    VectorXq tmp;
    Eigen::LDLT<MatrixXq> ldlt;
    Eigen::JacobiSVD<MatrixXq> svd;
    VectorXq diffq;
    VectorXq q_new;
    VectorXq original_Aii;
};
 
 
 
 
 
} // namespace KDL
 
 
 
 
 
 
#endif