13 tree(tree_in), jnttojacsolver(tree),
14 J(Eigen::MatrixXd::Zero(6 * endpoints.size(), tree.getNrOfJoints())),
15 Wy(Eigen::MatrixXd::Identity(J.rows(),J.rows())),
16 Wq(Eigen::MatrixXd::Identity(J.cols(),J.cols())),
17 J_Wq(J.rows(),J.cols()),Wy_J_Wq(J.rows(),J.cols()),
18 U(Eigen::MatrixXd::Identity(J.rows(),J.cols())),
19 V(Eigen::MatrixXd::Identity(J.cols(),J.cols())),
20 Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()),
21 t(Eigen::VectorXd::Zero(J.rows())), Wy_t(Eigen::VectorXd::Zero(J.rows())),
22 qdot(Eigen::VectorXd::Zero(J.cols())),
23 tmp(Eigen::VectorXd::Zero(J.cols())),S(Eigen::VectorXd::Zero(J.cols())),
27 for (
size_t i = 0; i < endpoints.
size(); ++i) {
51 for (Twists::const_iterator v_it = v_in.
begin(); v_it != v_in.
end(); ++v_it) {
69 const Twist& twist=v_in.
find(jac_it->first)->second;
70 t.segment(6*k,3) = Eigen::Map<const Eigen::Vector3d>(twist.
vel.
data);
71 t.segment(6*k+3,3) = Eigen::Map<const Eigen::Vector3d>(twist.
rot.
data);
90 for (
unsigned int i = 0; i <
J.cols(); i++) {
92 for (
unsigned int j = 0; j <
J.rows(); j++) {
94 sum +=
U(j, i) *
Wy_t(j);