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 <chainiksolverpos_nr.hpp>
Public Member Functions | |
| virtual int | CartToJnt (const JntArray &q_init, const Frame &p_in, JntArray &q_out) |
| Calculate inverse position kinematics, from cartesian coordinates to joint coordinates. | |
| ChainIkSolverPos_NR (const Chain &chain, 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. | |
| ~ChainIkSolverPos_NR () | |
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.
Definition at line 38 of file chainiksolverpos_nr.hpp.
| KDL::ChainIkSolverPos_NR::ChainIkSolverPos_NR | ( | const Chain & | chain, | |
| 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 | |
| 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 26 of file chainiksolverpos_nr.cpp.
| KDL::ChainIkSolverPos_NR::~ChainIkSolverPos_NR | ( | ) |
Definition at line 52 of file chainiksolverpos_nr.cpp.
| int KDL::ChainIkSolverPos_NR::CartToJnt | ( | const JntArray & | q_init, | |
| const Frame & | 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::ChainIkSolverPos.
Definition at line 33 of file chainiksolverpos_nr.cpp.
References KDL::Add(), KDL::ChainIkSolverVel::CartToJnt(), KDL::diff(), KDL::Equal(), KDL::ChainFkSolverPos::JntToCart(), and KDL::Twist::Zero().
Referenced by Robot::RobotAlgos::Test().
1.6.1