Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include "treeiksolvervel_wdls.hpp"
00009 #include "utilities/svd_eigen_HH.hpp"
00010
00011 namespace KDL {
00012 using namespace std;
00013
00014 TreeIkSolverVel_wdls::TreeIkSolverVel_wdls(const Tree& tree_in, const std::vector<std::string>& endpoints) :
00015 tree(tree_in), jnttojacsolver(tree),
00016 J(MatrixXd::Zero(6 * endpoints.size(), tree.getNrOfJoints())),
00017 Wy(MatrixXd::Identity(J.rows(),J.rows())),
00018 Wq(MatrixXd::Identity(J.cols(),J.cols())),
00019 J_Wq(J.rows(),J.cols()),Wy_J_Wq(J.rows(),J.cols()),
00020 U(MatrixXd::Identity(J.rows(),J.cols())),
00021 V(MatrixXd::Identity(J.cols(),J.cols())),
00022 Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()),
00023 t(VectorXd::Zero(J.rows())), Wy_t(VectorXd::Zero(J.rows())),
00024 qdot(VectorXd::Zero(J.cols())),
00025 tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols())),
00026 lambda(0)
00027 {
00028
00029 for (size_t i = 0; i < endpoints.size(); ++i) {
00030 jacobians.insert(Jacobians::value_type(endpoints[i], Jacobian(tree.getNrOfJoints())));
00031 }
00032
00033 }
00034
00035 TreeIkSolverVel_wdls::~TreeIkSolverVel_wdls() {
00036 }
00037
00038 void TreeIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq) {
00039 Wq = Mq;
00040 }
00041
00042 void TreeIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx) {
00043 Wy = Mx;
00044 }
00045
00046 void TreeIkSolverVel_wdls::setLambda(const double& lambda_in) {
00047 lambda = lambda_in;
00048 }
00049
00050 double TreeIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out) {
00051
00052
00053 for (Twists::const_iterator v_it = v_in.begin(); v_it != v_in.end(); ++v_it) {
00054 if (jacobians.find(v_it->first) == jacobians.end())
00055 return -2;
00056 }
00057
00058 if (q_in.rows() != tree.getNrOfJoints())
00059 return -1;
00060
00061
00062 unsigned int k = 0;
00063 for (Jacobians::iterator jac_it = jacobians.begin(); jac_it
00064 != jacobians.end(); ++jac_it) {
00065 int ret = jnttojacsolver.JntToJac(q_in, jac_it->second, jac_it->first);
00066 if (ret < 0)
00067 return ret;
00068 else {
00069
00070 J.block(6*k,0, 6,tree.getNrOfJoints()) = jac_it->second.data;
00071 const Twist& twist=v_in.find(jac_it->first)->second;
00072 t.segment(6*k,3) = Eigen::Map<const Eigen::Vector3d>(twist.vel.data);
00073 t.segment(6*k+3,3) = Eigen::Map<const Eigen::Vector3d>(twist.rot.data);
00074 }
00075 ++k;
00076 }
00077
00078
00079
00080 J_Wq.noalias() = J * Wq;
00081 Wy_J_Wq.noalias() = Wy * J_Wq;
00082
00083
00084 int ret = svd_eigen_HH(Wy_J_Wq, U, S, V, tmp);
00085 if (ret < 0 )
00086 return E_SVD_FAILED;
00087
00088 Wy_t.noalias() = Wy * t;
00089 Wq_V.noalias() = Wq * V;
00090
00091
00092 for (unsigned int i = 0; i < J.cols(); i++) {
00093 double sum = 0.0;
00094 for (unsigned int j = 0; j < J.rows(); j++) {
00095 if (i < Wy_t.rows())
00096 sum += U(j, i) * Wy_t(j);
00097 else
00098 sum += 0.0;
00099 }
00100 tmp( i) = sum * ((S(i) / (S(i) * S(i) + lambda * lambda)));
00101 }
00102
00103
00104 qdot_out.data.noalias() = Wq_V * tmp;
00105
00106 return Wy_t.norm();
00107 }
00108 }