31 nj(chain.getNrOfJoints()),
33 U(Eigen::MatrixXd::Zero(6,nj)),
34 S(Eigen::VectorXd::Zero(nj)),
35 V(Eigen::MatrixXd::Zero(nj,nj)),
38 tmp(Eigen::VectorXd::Zero(nj)),
39 tmp_jac(Eigen::MatrixXd::Zero(6,nj)),
40 tmp_jac_weight1(Eigen::MatrixXd::Zero(6,nj)),
41 tmp_jac_weight2(Eigen::MatrixXd::Zero(6,nj)),
42 tmp_ts(Eigen::MatrixXd::Zero(6,6)),
43 tmp_js(Eigen::MatrixXd::Zero(nj,nj)),
44 weight_ts(Eigen::MatrixXd::Identity(6,6)),
45 weight_js(Eigen::MatrixXd::Identity(nj,nj)),
58 Eigen::MatrixXd z6nj = Eigen::MatrixXd::Zero(6,
nj);
59 Eigen::VectorXd znj = Eigen::VectorXd::Zero(
nj);
60 Eigen::MatrixXd znjnj = Eigen::MatrixXd::Zero(
nj,
nj);
61 U.conservativeResizeLike(z6nj);
62 S.conservativeResizeLike(znj);
63 V.conservativeResizeLike(znjnj);
64 tmp.conservativeResizeLike(znj);
65 tmp_jac.conservativeResizeLike(z6nj);
68 tmp_js.conservativeResizeLike(znjnj);
69 weight_js.conservativeResizeLike(Eigen::MatrixXd::Identity(
nj,
nj));
110 if (Sout.size() !=
S.size())
149 qdot_out.
data.setZero() ;
170 sum+=
tmp_ts(j,i)*v_in(j);