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 <treeiksolverpos_nr_jl.hpp>
 
  
 | Public Member Functions | |
| virtual double | CartToJnt (const JntArray &q_init, const Frames &p_in, JntArray &q_out) | 
| Calculate inverse position kinematics, from cartesian coordinates to joint coordinates. | |
| 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. | |
| ~TreeIkSolverPos_NR_JL () | |
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.
| 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.
| tree | the tree to calculate the inverse position for | |
| endpoints | the list of endpoints you are interested in. | |
| q_max | the maximum joint positions | |
| q_min | the minimum joint positions | |
| fksolver | a forward position kinematics solver | |
| iksolver | an inverse velocity kinematics solver | |
| maxiter | the maximum Newton-Raphson iterations, default: 100 | |
| eps | the precision for the position, used to end the iterations, default: epsilon (defined in kdl.hpp) | 
Definition at line 27 of file treeiksolverpos_nr_jl.cpp.
References KDL::Frame::Identity(), and KDL::Twist::Zero().
| KDL::TreeIkSolverPos_NR_JL::~TreeIkSolverPos_NR_JL | ( | ) | 
Definition at line 82 of file treeiksolverpos_nr_jl.cpp.
| 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.
| q_init | initial guess of the joint coordinates | |
| p_in | input cartesian coordinates | |
| q_out | output joint coordinates | 
Implements KDL::TreeIkSolverPos.
Definition at line 45 of file treeiksolverpos_nr_jl.cpp.
References KDL::Add(), KDL::TreeIkSolverVel::CartToJnt(), KDL::diff(), KDL::TreeFkSolverPos::JntToCart(), k, and KDL::JntArray::rows().
 1.6.1
 1.6.1