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),
71 L(_l.cast<ScalarType>()),
78 svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV),
91 nj(chain.getNrOfJoints()),
92 ns(chain.getNrOfSegments()),
100 display_information(false),
103 eps_joints(_eps_joints),
104 T_base_jointroot(nj),
109 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);
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();
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";
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;
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>();
312 q_out.
data =
q.cast<
double>();