KDL
1.3.0
|
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>
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 |
ChainIkSolverVel & | iksolver |
ChainFkSolverPos & | fksolver |
JntArray | delta_q |
unsigned int | maxiter |
double | eps |
Frame | f |
Twist | delta_twist |
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.
|
inherited |
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.
chain | the chain to calculate the inverse position for |
q_min | the minimum joint positions |
q_max | the maximum 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) |
KDL::ChainIkSolverPos_NR_JL::~ChainIkSolverPos_NR_JL | ( | ) |
|
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::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, KDL::JntArray::rows(), and KDL::Twist::Zero().
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverPos_NR, and KDL::ChainJntToJacSolver.
References KDL::SolverI::E_NO_CONVERGE, KDL::SolverI::E_NOERROR, and KDL::SolverI::error.
Referenced by KDL::ChainJntToJacSolver::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), and KDL::ChainIkSolverVel_wdls::strError().
|
private |
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::SolverI::getError(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacSolver::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::SolverI::strError(), and KDL::ChainIkSolverVel_wdls::strError().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt().