30 nj(chain.getNrOfJoints()),
32 U(Eigen::MatrixXd::Zero(6,nj)),
33 S(Eigen::VectorXd::Zero(nj)),
34 Sinv(Eigen::VectorXd::Zero(nj)),
35 V(Eigen::MatrixXd::Zero(nj,nj)),
36 tmp(Eigen::VectorXd::Zero(nj)),
37 tmp2(Eigen::VectorXd::Zero(nj)),
50 nj(chain.getNrOfJoints()),
52 U(Eigen::MatrixXd::Zero(6,nj)),
53 S(Eigen::VectorXd::Zero(nj)),
54 Sinv(Eigen::VectorXd::Zero(nj)),
55 V(Eigen::MatrixXd::Zero(nj,nj)),
56 tmp(Eigen::VectorXd::Zero(nj)),
57 tmp2(Eigen::VectorXd::Zero(nj)),
69 U.conservativeResizeLike(Eigen::MatrixXd::Zero(6,
nj));
70 S.conservativeResizeLike(Eigen::VectorXd::Zero(
nj));
71 Sinv.conservativeResizeLike(Eigen::VectorXd::Zero(
nj));
72 V.conservativeResizeLike(Eigen::MatrixXd::Zero(
nj,
nj));
73 tmp.conservativeResizeLike(Eigen::VectorXd::Zero(
nj));
74 tmp2.conservativeResizeLike(Eigen::VectorXd::Zero(
nj));
75 opt_pos.
data.conservativeResizeLike(Eigen::VectorXd::Zero(
nj));
76 weights.
data.conservativeResizeLike(Eigen::VectorXd::Ones(
nj));
102 qdot_out.
data.setZero() ;
113 for (i = 0; i <
nj; ++i) {
114 Sinv(i) = fabs(
S(i))<
eps ? 0.0 : 1.0/
S(i);
116 for (i = 0; i < 6; ++i) {
120 qdot_out.
data =
V *
Sinv.asDiagonal() *
U.transpose() *
tmp.head(6);
142 for (i = 0; i <
nj; ++i) {
143 double qd = q_in(i) -
opt_pos(i);
150 for (i = 0; i <
nj; ++i) {
155 tmp2 =
V *
Sinv.asDiagonal() *
U.transpose() *
U *
S.asDiagonal() *
V.transpose() *
tmp;
157 for (i = 0; i <
nj; ++i) {
168 if (
nj != _weights.
rows())
176 if (
nj != _opt_pos.
rows())