15 tree(tree_in), jnttojacsolver(tree),
16 J(MatrixXd::Zero(6 * endpoints.size(), tree.getNrOfJoints())),
17 Wy(MatrixXd::Identity(J.rows(),J.rows())),
18 Wq(MatrixXd::Identity(J.cols(),J.cols())),
19 J_Wq(J.rows(),J.cols()),Wy_J_Wq(J.rows(),J.cols()),
20 U(MatrixXd::Identity(J.rows(),J.cols())),
21 V(MatrixXd::Identity(J.cols(),J.cols())),
22 Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()),
23 t(VectorXd::Zero(J.rows())), Wy_t(VectorXd::Zero(J.rows())),
24 qdot(VectorXd::Zero(J.cols())),
25 tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols())),
29 for (
size_t i = 0; i < endpoints.size(); ++i) {
53 for (Twists::const_iterator v_it = v_in.begin(); v_it != v_in.end(); ++v_it) {
63 for (Jacobians::iterator jac_it =
jacobians.begin(); jac_it
71 const Twist& twist=v_in.find(jac_it->first)->second;
72 t.segment(6*k,3) = Eigen::Map<const Eigen::Vector3d>(twist.vel.data);
73 t.segment(6*k+3,3) = Eigen::Map<const Eigen::Vector3d>(twist.rot.data);
89 Wq_V.noalias() = Wq *
V;
92 for (
unsigned int i = 0; i <
J.cols(); i++) {
94 for (
unsigned int j = 0; j <
J.rows(); j++) {
96 sum +=
U(j, i) *
Wy_t(j);
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
unsigned int rows() const
TreeJntToJacSolver jnttojacsolver
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)
void setWeightJS(const MatrixXd &Mq)
This class represents an fixed size array containing joint values of a KDL::Chain.
virtual ~TreeIkSolverVel_wdls()
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
represents both translational and rotational velocities.
void setLambda(const double &lambda)
TreeIkSolverVel_wdls(const Tree &tree, const std::vector< std::string > &endpoints)
Child SVD failed.
void setWeightTS(const MatrixXd &Mx)
std::map< std::string, Twist > Twists
unsigned int getNrOfJoints() const
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
static const int E_SVD_FAILED