treeiksolvervel_wdls.cpp
Go to the documentation of this file.
00001 /*
00002  * TreeIkSolverVel_wdls.cpp
00003  *
00004  *  Created on: Nov 28, 2008
00005  *      Author: rubensmits
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         //First check if we are configured for this Twists:
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         //Check if q_in has the right size
00058         if (q_in.rows() != tree.getNrOfJoints())
00059             return -1;
00060         
00061         //Lets get all the jacobians we need:
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                 //lets put the jacobian in the big matrix and put the twist in the big t:
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         //Lets use the wdls algorithm to find the qdot:
00079         // Create the Weighted jacobian
00080         J_Wq.noalias() = J * Wq;
00081         Wy_J_Wq.noalias() = Wy * J_Wq;
00082         
00083         // Compute the SVD of the weighted jacobian
00084         int ret = svd_eigen_HH(Wy_J_Wq, U, S, V, tmp);
00085         if (ret < 0 )
00086             return E_SVD_FAILED;
00087         //Pre-multiply U and V by the task space and joint space weighting matrix respectively
00088         Wy_t.noalias() = Wy * t;
00089         Wq_V.noalias() = Wq * V;
00090         
00091         // tmp = (Si*Wy*U'*y),
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         // x = Lx^-1*V*tmp + x
00104         qdot_out.data.noalias() = Wq_V * tmp;
00105         
00106         return Wy_t.norm();
00107     }
00108 }


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14