orocos_kdl
Public Member Functions | Private Attributes | List of all members
KDL::TreeIkSolverPos_NR_JL Class Reference

#include <treeiksolverpos_nr_jl.hpp>

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

Public Member Functions

virtual double CartToJnt (const JntArray &q_init, const Frames &p_in, JntArray &q_out)
 
 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)
 
 ~TreeIkSolverPos_NR_JL ()
 
- Public Member Functions inherited from KDL::TreeIkSolverPos
virtual ~TreeIkSolverPos ()
 

Private Attributes

JntArray delta_q
 
Twists delta_twists
 
std::vector< std::stringendpoints
 
double eps
 
TreeFkSolverPosfksolver
 
Frames frames
 
TreeIkSolverVeliksolver
 
unsigned int maxiter
 
JntArray q_max
 
JntArray q_min
 
const Tree tree
 

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.

Definition at line 42 of file treeiksolverpos_nr_jl.hpp.

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

Definition at line 27 of file treeiksolverpos_nr_jl.cpp.

◆ ~TreeIkSolverPos_NR_JL()

KDL::TreeIkSolverPos_NR_JL::~TreeIkSolverPos_NR_JL ( )

Definition at line 79 of file treeiksolverpos_nr_jl.cpp.

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.

Definition at line 42 of file treeiksolverpos_nr_jl.cpp.

Member Data Documentation

◆ delta_q

JntArray KDL::TreeIkSolverPos_NR_JL::delta_q
private

Definition at line 73 of file treeiksolverpos_nr_jl.hpp.

◆ delta_twists

Twists KDL::TreeIkSolverPos_NR_JL::delta_twists
private

Definition at line 75 of file treeiksolverpos_nr_jl.hpp.

◆ endpoints

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

Definition at line 76 of file treeiksolverpos_nr_jl.hpp.

◆ eps

double KDL::TreeIkSolverPos_NR_JL::eps
private

Definition at line 79 of file treeiksolverpos_nr_jl.hpp.

◆ fksolver

TreeFkSolverPos& KDL::TreeIkSolverPos_NR_JL::fksolver
private

Definition at line 72 of file treeiksolverpos_nr_jl.hpp.

◆ frames

Frames KDL::TreeIkSolverPos_NR_JL::frames
private

Definition at line 74 of file treeiksolverpos_nr_jl.hpp.

◆ iksolver

TreeIkSolverVel& KDL::TreeIkSolverPos_NR_JL::iksolver
private

Definition at line 71 of file treeiksolverpos_nr_jl.hpp.

◆ maxiter

unsigned int KDL::TreeIkSolverPos_NR_JL::maxiter
private

Definition at line 78 of file treeiksolverpos_nr_jl.hpp.

◆ q_max

JntArray KDL::TreeIkSolverPos_NR_JL::q_max
private

Definition at line 70 of file treeiksolverpos_nr_jl.hpp.

◆ q_min

JntArray KDL::TreeIkSolverPos_NR_JL::q_min
private

Definition at line 69 of file treeiksolverpos_nr_jl.hpp.

◆ tree

const Tree KDL::TreeIkSolverPos_NR_JL::tree
private

Definition at line 68 of file treeiksolverpos_nr_jl.hpp.


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