1 #ifndef KDL_CHAINIKSOLVERPOS_GN_HPP 2 #define KDL_CHAINIKSOLVERPOS_GN_HPP 36 #include <Eigen/Dense> 68 typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic>
MatrixXq;
69 typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1>
VectorXq;
97 const Eigen::Matrix<double,6,1>& _L,
100 double _eps_joints=1E-15
112 double _eps_joints=1E-15
221 Eigen::Matrix<ScalarType,6,1>
L;
239 Eigen::JacobiSVD<MatrixXq>
svd;
ChainIkSolverPos_LMA(const KDL::Chain &_chain, const Eigen::Matrix< double, 6, 1 > &_L, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15)
constructs an ChainIkSolverPos_LMA solver.
This abstract class encapsulates the inverse position solver for a KDL::Chain.
Eigen::JacobiSVD< MatrixXq > svd
KDL::Frame T_base_head
for internal use only.
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt...
std::vector< KDL::Frame > T_base_jointtip
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
bool display_information
display information on each iteration step to the console.
void compute_jacobian(const VectorXq &q)
for internal use only. Only exposed for test and diagnostic purposes. compute_fwdpos(q) should always...
This class represents an fixed size array containing joint values of a KDL::Chain.
void display_jac(const KDL::JntArray &jval)
for internal use only. Only exposed for test and diagnostic purposes.
VectorXq lastSV
contains the last values for the singular values of the weighted Jacobian after an execution of CartT...
static const int E_GRADIENT_JOINTS_TOO_SMALL
MatrixXq jac
for internal use only.
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
void updateInternalDataStructures()
Eigen::Matrix< ScalarType, 6, 1 > L
double lastDifference
contains the last value for after an execution of CartToJnt.
VectorXq grad
for internal use only.
void compute_fwdpos(const VectorXq &q)
for internal use only.
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
Eigen::LDLT< MatrixXq > ldlt
std::vector< KDL::Frame > T_base_jointroot
virtual const char * strError(const int error) const
represents a frame transformation in 3D space (rotation + translation)
static const int E_INCREMENT_JOINTS_TOO_SMALL
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixXq
virtual ~ChainIkSolverPos_LMA()
destructor.
int error
Latest error, initialized to E_NOERROR in constructor.
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq