KDL  1.4.0
Public Member Functions | Private Attributes | List of all members
KDL::TreeIkSolverPos_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::Tree. More...

#include <src/treeiksolverpos_nr_jl.hpp>

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

Public Member Functions

 TreeIkSolverPos_NR_JL (const Tree &tree, const std::vector< std::string > &endpoints, const JntArray &q_min, const JntArray &q_max, TreeFkSolverPos &fksolver, TreeIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
 Constructor of the solver, it needs the tree, a forward position kinematics solver and an inverse velocity kinematics solver for that tree, and a list of the segments you are interested in. More...
 
 ~TreeIkSolverPos_NR_JL ()
 
virtual double CartToJnt (const JntArray &q_init, const Frames &p_in, JntArray &q_out)
 Calculate inverse position kinematics, from cartesian coordinates to joint coordinates. More...
 

Private Attributes

const Tree tree
 
JntArray q_min
 
JntArray q_max
 
TreeIkSolverVeliksolver
 
TreeFkSolverPosfksolver
 
JntArray delta_q
 
Frames frames
 
Twists delta_twists
 
std::vector< std::string > endpoints
 
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::Tree.

Takes joint limits into account.

Constructor & Destructor Documentation

§ TreeIkSolverPos_NR_JL()

KDL::TreeIkSolverPos_NR_JL::TreeIkSolverPos_NR_JL ( const Tree tree,
const std::vector< std::string > &  endpoints,
const JntArray q_min,
const JntArray q_max,
TreeFkSolverPos fksolver,
TreeIkSolverVel iksolver,
unsigned int  maxiter = 100,
double  eps = 1e-6 
)

Constructor of the solver, it needs the tree, a forward position kinematics solver and an inverse velocity kinematics solver for that tree, and a list of the segments you are interested in.

Parameters
treethe tree to calculate the inverse position for
endpointsthe list of endpoints you are interested in.
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

References delta_twists, endpoints, frames, KDL::Frame::Identity(), and KDL::Twist::Zero().

§ ~TreeIkSolverPos_NR_JL()

KDL::TreeIkSolverPos_NR_JL::~TreeIkSolverPos_NR_JL ( )

Member Function Documentation

§ CartToJnt()

double KDL::TreeIkSolverPos_NR_JL::CartToJnt ( const JntArray q_init,
const Frames 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 otherwise (>=0) remaining (weighted) distance to target

Implements KDL::TreeIkSolverPos.

References KDL::Add(), KDL::TreeIkSolverVel::CartToJnt(), delta_q, delta_twists, KDL::diff(), eps, fksolver, frames, iksolver, KDL::TreeFkSolverPos::JntToCart(), maxiter, q_max, q_min, and KDL::JntArray::rows().

Member Data Documentation

§ delta_q

JntArray KDL::TreeIkSolverPos_NR_JL::delta_q
private

Referenced by CartToJnt().

§ delta_twists

Twists KDL::TreeIkSolverPos_NR_JL::delta_twists
private

Referenced by CartToJnt(), and TreeIkSolverPos_NR_JL().

§ endpoints

std::vector<std::string> KDL::TreeIkSolverPos_NR_JL::endpoints
private

Referenced by TreeIkSolverPos_NR_JL().

§ eps

double KDL::TreeIkSolverPos_NR_JL::eps
private

Referenced by CartToJnt().

§ fksolver

TreeFkSolverPos& KDL::TreeIkSolverPos_NR_JL::fksolver
private

Referenced by CartToJnt().

§ frames

Frames KDL::TreeIkSolverPos_NR_JL::frames
private

Referenced by CartToJnt(), and TreeIkSolverPos_NR_JL().

§ iksolver

TreeIkSolverVel& KDL::TreeIkSolverPos_NR_JL::iksolver
private

Referenced by CartToJnt().

§ maxiter

unsigned int KDL::TreeIkSolverPos_NR_JL::maxiter
private

Referenced by CartToJnt().

§ q_max

JntArray KDL::TreeIkSolverPos_NR_JL::q_max
private

Referenced by CartToJnt().

§ q_min

JntArray KDL::TreeIkSolverPos_NR_JL::q_min
private

Referenced by CartToJnt().

§ tree

const Tree KDL::TreeIkSolverPos_NR_JL::tree
private

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