Go to the documentation of this file. 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;
238 Eigen::LDLT<MatrixXq>
ldlt;
239 Eigen::JacobiSVD<MatrixXq>
svd;
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: chainiksolverpos_lma.cpp:340
std::vector< KDL::Frame > T_base_jointtip
Definition: chainiksolverpos_lma.hpp:273
Eigen::JacobiSVD< MatrixXq > svd
Definition: chainiksolverpos_lma.hpp:285
double data[3]
Definition: frames.hpp:163
MatrixXq jac
for internal use only.
Definition: chainiksolverpos_lma.hpp:243
static Frame Identity()
Definition: frames.inl:695
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: solveri.hpp:125
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
Definition: jntarray.hpp:69
MatrixXq A
Definition: chainiksolverpos_lma.hpp:282
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
void display_jac(const KDL::JntArray &jval)
for internal use only.
Definition: chainiksolverpos_lma.cpp:200
Vector p
origine of the Frame
Definition: frames.hpp:572
unsigned int maxiter
Definition: chainiksolverpos_lma.hpp:264
KDL::Frame T_base_head
for internal use only.
Definition: chainiksolverpos_lma.hpp:256
VectorXq diffq
Definition: chainiksolverpos_lma.hpp:286
double eps
Definition: chainiksolverpos_lma.hpp:265
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:231
Definition: articulatedbodyinertia.cpp:28
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
VectorXq tmp
Definition: chainiksolverpos_lma.hpp:283
void compute_jacobian(const VectorXq &q)
for internal use only.
Definition: chainiksolverpos_lma.cpp:181
VectorXq q_new
Definition: chainiksolverpos_lma.hpp:287
@ E_MAX_ITERATIONS_EXCEEDED
Maximum number of iterations exceeded.
Definition: solveri.hpp:101
represents both translational and rotational velocities.
Definition: frames.hpp:720
void Twist_to_Eigen(const KDL::Twist &t, Eigen::MatrixBase< Derived > &e)
Definition: chainiksolverpos_lma.cpp:63
unsigned int nj
Definition: chainiksolverpos_lma.hpp:207
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
computing inverse position kinematics using Levenberg-Marquardt.
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
bool display_information
display information on each iteration step to the console.
Definition: chainiksolverpos_lma.hpp:261
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
void compute_fwdpos(const VectorXq &q)
for internal use only.
Definition: chainiksolverpos_lma.cpp:164
static const int E_GRADIENT_JOINTS_TOO_SMALL
Definition: chainiksolverpos_lma.hpp:118
Definition: frames.hpp:570
Twist twist(const double &q, const double &qdot) const
Request the 6D-velocity of the tip of the segment, given the joint position q and the joint velocity ...
Definition: segment.cpp:62
Eigen::VectorXd data
Definition: jntarray.hpp:72
@ None
Definition: joint.hpp:47
const KDL::Chain & chain
Definition: chainiksolverpos_lma.hpp:206
Vector vel
The velocity of that point.
Definition: frames.hpp:722
Definition: segment.hpp:46
Eigen::LDLT< MatrixXq > ldlt
Definition: chainiksolverpos_lma.hpp:284
Eigen::Matrix< ScalarType, 6, 1 > L
Definition: chainiksolverpos_lma.hpp:267
VectorXq lastSV
contains the last values for the singular values of the weighted Jacobian after an execution of CartT...
Definition: chainiksolverpos_lma.hpp:236
std::vector< KDL::Frame > T_base_jointroot
Definition: chainiksolverpos_lma.hpp:272
virtual ~ChainIkSolverPos_LMA()
destructor.
Definition: chainiksolverpos_lma.cpp:162
unsigned int ns
Definition: chainiksolverpos_lma.hpp:208
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
Definition: chainiksolverpos_lma.hpp:226
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq
Definition: chainiksolverpos_lma.hpp:115
void updateInternalDataStructures()
Update the internal data structures.
Definition: chainiksolverpos_lma.cpp:145
double lastDifference
contains the last value for after an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:221
VectorXq grad
for internal use only.
Definition: chainiksolverpos_lma.hpp:250
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
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.
Definition: chainiksolverpos_lma.cpp:73
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Definition: chainiksolverpos_lma.cpp:210
Definition: chainiksolver.hpp:42
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
determines the difference of vector b with vector a.
@ E_NOERROR
No error.
Definition: solveri.hpp:91
double eps_joints
Definition: chainiksolverpos_lma.hpp:266
static const int E_INCREMENT_JOINTS_TOO_SMALL
Definition: chainiksolverpos_lma.hpp:119
Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixXq
Definition: chainiksolverpos_lma.hpp:114
double ScalarType
Definition: chainiksolverpos_lma.hpp:113
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
VectorXq original_Aii
Definition: chainiksolverpos_lma.hpp:288
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:216
VectorXq q
Definition: chainiksolverpos_lma.hpp:281