KDL  1.3.0
Public Types | Public Member Functions | Protected Attributes | Private Attributes | List of all members
KDL::ChainIkSolverPos_NR_JL 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_jl.hpp>

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

Public Types

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

Public Member Functions

 ChainIkSolverPos_NR_JL (const Chain &chain, const JntArray &q_min, const JntArray &q_max, ChainFkSolverPos &fksolver, ChainIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
 Constructor of the solver, it needs the chain, a forward position kinematics solver and an inverse velocity kinematics solver for that chain. More...
 
 ~ChainIkSolverPos_NR_JL ()
 
virtual int CartToJnt (const JntArray &q_init, const Frame &p_in, JntArray &q_out)
 Calculate inverse position kinematics, from cartesian coordinates to joint coordinates. More...
 
virtual int getError () const
 Return the latest error. More...
 
virtual const char * strError (const int error) const
 Return a description of the latest error. More...
 

Protected Attributes

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

Private Attributes

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

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.

Takes joint limits into account.

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_JL::ChainIkSolverPos_NR_JL ( const Chain chain,
const JntArray &  q_min,
const JntArray &  q_max,
ChainFkSolverPos fksolver,
ChainIkSolverVel iksolver,
unsigned int  maxiter = 100,
double  eps = 1e-6 
)

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
q_maxthe maximum joint positions
q_minthe minimum joint positions
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_JL::~ChainIkSolverPos_NR_JL ( )

Member Function Documentation

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

Calculate inverse position kinematics, from cartesian coordinates to joint coordinates.

Parameters
q_initinitial guess of the joint coordinates
p_ininput cartesian coordinates
q_outoutput joint coordinates
Returns
if < 0 something went wrong

Implements KDL::ChainIkSolverPos.

References KDL::Add(), KDL::ChainIkSolverVel::CartToJnt(), delta_q, delta_twist, KDL::diff(), eps, KDL::Equal(), f, fksolver, iksolver, KDL::ChainFkSolverPos::JntToCart(), maxiter, q_max, q_min, and KDL::Twist::Zero().

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

Return the latest error.

References KDL::SolverI::error.

virtual const char* KDL::SolverI::strError ( const int  error) const
inlinevirtualinherited

Member Data Documentation

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

Referenced by CartToJnt().

Twist KDL::ChainIkSolverPos_NR_JL::delta_twist
private

Referenced by CartToJnt().

double KDL::ChainIkSolverPos_NR_JL::eps
private

Referenced by CartToJnt().

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

Referenced by CartToJnt().

ChainFkSolverPos& KDL::ChainIkSolverPos_NR_JL::fksolver
private

Referenced by CartToJnt().

ChainIkSolverVel& KDL::ChainIkSolverPos_NR_JL::iksolver
private

Referenced by CartToJnt().

unsigned int KDL::ChainIkSolverPos_NR_JL::maxiter
private

Referenced by CartToJnt().

JntArray KDL::ChainIkSolverPos_NR_JL::q_max
private

Referenced by CartToJnt().

JntArray KDL::ChainIkSolverPos_NR_JL::q_min
private

Referenced by CartToJnt().


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