KDL  1.3.0
Public Types | Public Member Functions | Static Public Attributes | Protected Attributes | Private Attributes | List of all members
KDL::ChainIkSolverPos_NR Class Reference

Implementation of a general inverse position kinematics algorithm based on Newton-Raphson iterations to calculate the position transformation from Cartesian to joint space of a general KDL::Chain. More...

#include <src/chainiksolverpos_nr.hpp>

Inheritance diagram for KDL::ChainIkSolverPos_NR:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIkSolverPos_NR:
Collaboration graph
[legend]

Public Types

enum  { E_DEGRADED = +1, E_NOERROR = 0, E_NO_CONVERGE = -1, E_UNDEFINED = -2 }
 

Public Member Functions

 ChainIkSolverPos_NR (const Chain &chain, ChainFkSolverPos &fksolver, ChainIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
 Child IK solver failed. More...
 
 ~ChainIkSolverPos_NR ()
 
virtual int CartToJnt (const JntArray &q_init, const Frame &p_in, JntArray &q_out)
 Find an output joint pose q_out, given a starting joint pose q_init and a desired cartesian pose p_in. More...
 
virtual const char * strError (const int error) const
 Return a description of the latest error. More...
 
virtual int getError () const
 Return the latest error. More...
 

Static Public Attributes

static const int E_IKSOLVER_FAILED = -100
 

Protected Attributes

int error
 Latest error, initialized to E_NOERROR in constructor. More...
 

Private Attributes

const Chain chain
 
ChainIkSolverVeliksolver
 
ChainFkSolverPosfksolver
 
JntArray delta_q
 
Frame f
 
Twist delta_twist
 
unsigned int maxiter
 
double eps
 

Detailed Description

Implementation of a general inverse position kinematics algorithm based on Newton-Raphson iterations to calculate the position transformation from Cartesian to joint space of a general KDL::Chain.

Member Enumeration Documentation

anonymous enum
inherited
Enumerator
E_DEGRADED 

Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)

E_NOERROR 

No error.

E_NO_CONVERGE 

Failed to converge.

E_UNDEFINED 

Undefined value (e.g. computed a NAN, or tan(90 degrees) )

Constructor & Destructor Documentation

KDL::ChainIkSolverPos_NR::ChainIkSolverPos_NR ( const Chain chain,
ChainFkSolverPos fksolver,
ChainIkSolverVel iksolver,
unsigned int  maxiter = 100,
double  eps = 1e-6 
)

Child IK solver failed.

Constructor of the solver, it needs the chain, a forward position kinematics solver and an inverse velocity kinematics solver for that chain.

Parameters
chainthe chain to calculate the inverse position for
fksolvera forward position kinematics solver
iksolveran inverse velocity kinematics solver
maxiterthe maximum Newton-Raphson iterations, default: 100
epsthe precision for the position, used to end the iterations, default: epsilon (defined in kdl.hpp)
Returns
KDL::ChainIkSolverPos_NR::~ChainIkSolverPos_NR ( )

Member Function Documentation

int KDL::ChainIkSolverPos_NR::CartToJnt ( const JntArray &  q_init,
const Frame p_in,
JntArray &  q_out 
)
virtual

Find an output joint pose q_out, given a starting joint pose q_init and a desired cartesian pose p_in.

Returns
: E_NOERROR=solution converged to <eps in maxiter E_DEGRADED=solution converged to <eps in maxiter, but solution is degraded in quality (e.g. pseudo-inverse in iksolver is singular) E_IKSOLVER_FAILED=velocity solver failed E_NO_CONVERGE=solution did not converge (e.g. large displacement, low iterations)

Implements KDL::ChainIkSolverPos.

References KDL::Add(), KDL::ChainIkSolverVel::CartToJnt(), delta_q, delta_twist, KDL::diff(), KDL::SolverI::E_DEGRADED, E_IKSOLVER_FAILED, KDL::SolverI::E_NO_CONVERGE, KDL::SolverI::E_NOERROR, eps, KDL::Equal(), KDL::SolverI::error, f, fksolver, iksolver, KDL::ChainFkSolverPos::JntToCart(), maxiter, and KDL::Twist::Zero().

virtual int KDL::SolverI::getError ( ) const
inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

const char * KDL::ChainIkSolverPos_NR::strError ( const int  error) const
virtual

Return a description of the latest error.

Returns
if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented from KDL::SolverI.

References E_IKSOLVER_FAILED, and KDL::SolverI::strError().

Member Data Documentation

const Chain KDL::ChainIkSolverPos_NR::chain
private
JntArray KDL::ChainIkSolverPos_NR::delta_q
private

Referenced by CartToJnt().

Twist KDL::ChainIkSolverPos_NR::delta_twist
private

Referenced by CartToJnt().

const int KDL::ChainIkSolverPos_NR::E_IKSOLVER_FAILED = -100
static

Referenced by CartToJnt(), and strError().

double KDL::ChainIkSolverPos_NR::eps
private

Referenced by CartToJnt().

int KDL::SolverI::error
protectedinherited
Frame KDL::ChainIkSolverPos_NR::f
private

Referenced by CartToJnt().

ChainFkSolverPos& KDL::ChainIkSolverPos_NR::fksolver
private

Referenced by CartToJnt().

ChainIkSolverVel& KDL::ChainIkSolverPos_NR::iksolver
private

Referenced by CartToJnt().

unsigned int KDL::ChainIkSolverPos_NR::maxiter
private

Referenced by CartToJnt().


The documentation for this class was generated from the following files: