39 template <
typename Derived>
52 const Eigen::Matrix<double,6,1>& _L,
58 nj(chain.getNrOfJoints()),
59 ns(chain.getNrOfSegments()),
67 display_information(false),
70 eps_joints(_eps_joints),
78 svd(6, nj,
Eigen::ComputeThinU |
Eigen::ComputeThinV),
126 jac.conservativeResize(Eigen::NoChange,
nj);
127 grad.conservativeResize(
nj);
130 q.conservativeResize(
nj);
131 A.conservativeResize(
nj,
nj);
132 ldlt = Eigen::LDLT<MatrixXq>(
nj);
133 svd = Eigen::JacobiSVD<MatrixXq>(6,
nj,Eigen::ComputeThinU | Eigen::ComputeThinV);
134 diffq.conservativeResize(nj);
135 q_new.conservativeResize(nj);
143 unsigned int jointndx=0;
160 unsigned int jointndx=0;
166 jac(0,jointndx)=t[0];
167 jac(1,jointndx)=t[1];
168 jac(2,jointndx)=t[2];
169 jac(3,jointndx)=t[3];
170 jac(4,jointndx)=t[4];
171 jac(5,jointndx)=t[5];
183 std::cout <<
"Singular values : " <<
svd.singularValues().transpose()<<
"\n";
200 double delta_pos_norm;
201 Eigen::Matrix<ScalarType,6,1> delta_pos;
202 Eigen::Matrix<ScalarType,6,1> delta_pos_new;
208 delta_pos=
L.asDiagonal()*delta_pos;
209 delta_pos_norm = delta_pos.norm();
210 if (delta_pos_norm<
eps) {
219 q_out.
data =
q.cast<
double>();
227 for (
unsigned int i=0;i<
maxiter;++i) {
235 tmp =
svd.matrixU().transpose()*delta_pos;
238 grad =
jac.transpose()*delta_pos;
240 std::cout <<
"------- iteration " << i <<
" ----------------\n" 241 <<
" q = " <<
q.transpose() <<
"\n" 242 <<
" weighted jac = \n" <<
jac <<
"\n" 243 <<
" lambda = " << lambda <<
"\n" 244 <<
" eigenvalues = " <<
svd.singularValues().transpose() <<
"\n" 245 <<
" difference = " << delta_pos.transpose() <<
"\n" 246 <<
" difference norm= " << delta_pos_norm <<
"\n" 247 <<
" proj. on grad. = " <<
grad <<
"\n";
248 std::cout << std::endl;
250 dnorm =
diffq.lpNorm<Eigen::Infinity>();
255 q_out.
data =
q.cast<
double>();
272 q_out.
data =
q.cast<
double>();
279 delta_pos_new =
L.asDiagonal()*delta_pos_new;
280 double delta_pos_new_norm = delta_pos_new.norm();
281 rho = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm;
282 rho /= diffq.transpose()*(lambda*diffq +
grad);
285 delta_pos = delta_pos_new;
286 delta_pos_norm = delta_pos_new_norm;
287 if (delta_pos_norm<
eps) {
294 q_out.
data =
q.cast<
double>();
300 lambda = lambda*
max(1/3.0, 1-tmp*tmp*tmp);
312 q_out.
data =
q.cast<
double>();
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.
const Segment & getSegment(unsigned int nr) const
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
Eigen::JacobiSVD< MatrixXq > svd
KDL::Frame T_base_head
for internal use only.
unsigned int rows() const
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...
Vector vel
The velocity of that point.
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...
unsigned int getNrOfSegments() const
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Maximum number of iterations exceeded.
virtual const char * strError(const int error) const
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...
computing inverse position kinematics using Levenberg-Marquardt.
Input size does not match internal state.
static const int E_GRADIENT_JOINTS_TOO_SMALL
Frame pose(const double &q) const
Twist twist(const double &q, const double &qdot) const
represents both translational and rotational velocities.
MatrixXq jac
for internal use only.
Vector rot
The rotational velocity of that point.
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
const Joint & getJoint() const
Vector p
origine of the Frame
unsigned int getNrOfJoints() const
void updateInternalDataStructures()
Eigen::Matrix< ScalarType, 6, 1 > L
double lastDifference
contains the last value for after an execution of CartToJnt.
void Twist_to_Eigen(const KDL::Twist &t, Eigen::MatrixBase< Derived > &e)
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.
virtual ~ChainIkSolverPos_LMA()
destructor.
int error
Latest error, initialized to E_NOERROR in constructor.
double max(double a, double b)
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq
const JointType & getType() const