#include <chainiksolvervel_pinv.hpp>
Public Member Functions | |
virtual int | CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) |
virtual int | CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out) |
ChainIkSolverVel_pinv (const Chain &chain, double eps=0.00001, int maxiter=150) | |
unsigned int | getNrZeroSigmas () const |
int | getSVDResult () const |
virtual const char * | strError (const int error) const |
~ChainIkSolverVel_pinv () | |
Static Public Attributes | |
static const int | E_CONVERGE_PINV_SINGULAR = +100 |
solution converged but (pseudo)inverse is singular | |
static const int | E_SVD_FAILED = -100 |
Private Attributes | |
const Chain | chain |
double | eps |
Jacobian | jac |
ChainJntToJacSolver | jnt2jac |
int | maxiter |
unsigned int | nrZeroSigmas |
JntArray | S |
SVD_HH | svd |
int | svdResult |
JntArray | tmp |
std::vector< JntArray > | U |
std::vector< JntArray > | V |
Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain. It uses a svd-calculation based on householders rotations.
Definition at line 40 of file chainiksolvervel_pinv.hpp.
KDL::ChainIkSolverVel_pinv::ChainIkSolverVel_pinv | ( | const Chain & | chain, |
double | eps = 0.00001 , |
||
int | maxiter = 150 |
||
) | [explicit] |
Constructor of the solver
chain | the chain to calculate the inverse velocity kinematics for |
eps | if a singular value is below this value, its inverse is set to zero, default: 0.00001 |
maxiter | maximum iterations for the svd calculation, default: 150 |
Definition at line 26 of file chainiksolvervel_pinv.cpp.
Definition at line 42 of file chainiksolvervel_pinv.cpp.
int KDL::ChainIkSolverVel_pinv::CartToJnt | ( | const JntArray & | q_in, |
const Twist & | v_in, | ||
JntArray & | qdot_out | ||
) | [virtual] |
Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian velocity v_in
Implements KDL::ChainIkSolverVel.
Definition at line 47 of file chainiksolvervel_pinv.cpp.
virtual int KDL::ChainIkSolverVel_pinv::CartToJnt | ( | const JntArray & | q_init, |
const FrameVel & | v_in, | ||
JntArrayVel & | q_out | ||
) | [inline, virtual] |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
Definition at line 81 of file chainiksolvervel_pinv.hpp.
unsigned int KDL::ChainIkSolverVel_pinv::getNrZeroSigmas | ( | ) | const [inline] |
Retrieve the number of singular values of the jacobian that are < eps; if the number of near zero singular values is > jac.col()-jac.row(), then the jacobian pseudoinverse is singular
Definition at line 88 of file chainiksolvervel_pinv.hpp.
int KDL::ChainIkSolverVel_pinv::getSVDResult | ( | ) | const [inline] |
Retrieve the latest return code from the SVD algorithm
Definition at line 94 of file chainiksolvervel_pinv.hpp.
const char * KDL::ChainIkSolverVel_pinv::strError | ( | const int | error | ) | const [virtual] |
Return a description of the latest error
Reimplemented from KDL::SolverI.
Definition at line 110 of file chainiksolvervel_pinv.cpp.
const Chain KDL::ChainIkSolverVel_pinv::chain [private] |
Definition at line 100 of file chainiksolvervel_pinv.hpp.
const int KDL::ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR = +100 [static] |
solution converged but (pseudo)inverse is singular
Child SVD failed
Definition at line 45 of file chainiksolvervel_pinv.hpp.
const int KDL::ChainIkSolverVel_pinv::E_SVD_FAILED = -100 [static] |
Definition at line 43 of file chainiksolvervel_pinv.hpp.
double KDL::ChainIkSolverVel_pinv::eps [private] |
Definition at line 108 of file chainiksolvervel_pinv.hpp.
Jacobian KDL::ChainIkSolverVel_pinv::jac [private] |
Definition at line 102 of file chainiksolvervel_pinv.hpp.
Definition at line 101 of file chainiksolvervel_pinv.hpp.
int KDL::ChainIkSolverVel_pinv::maxiter [private] |
Definition at line 109 of file chainiksolvervel_pinv.hpp.
unsigned int KDL::ChainIkSolverVel_pinv::nrZeroSigmas [private] |
Definition at line 110 of file chainiksolvervel_pinv.hpp.
JntArray KDL::ChainIkSolverVel_pinv::S [private] |
Definition at line 105 of file chainiksolvervel_pinv.hpp.
SVD_HH KDL::ChainIkSolverVel_pinv::svd [private] |
Definition at line 103 of file chainiksolvervel_pinv.hpp.
int KDL::ChainIkSolverVel_pinv::svdResult [private] |
Definition at line 111 of file chainiksolvervel_pinv.hpp.
JntArray KDL::ChainIkSolverVel_pinv::tmp [private] |
Definition at line 107 of file chainiksolvervel_pinv.hpp.
std::vector<JntArray> KDL::ChainIkSolverVel_pinv::U [private] |
Definition at line 104 of file chainiksolvervel_pinv.hpp.
std::vector<JntArray> KDL::ChainIkSolverVel_pinv::V [private] |
Definition at line 106 of file chainiksolvervel_pinv.hpp.