treeiksolvervel_wdls.hpp
Go to the documentation of this file.00001 /* 00002 * TreeIkSolverVel_wdls.hpp 00003 * 00004 * Created on: Nov 28, 2008 00005 * Author: rubensmits 00006 */ 00007 00008 #ifndef TREEIKSOLVERVEL_WDLS_HPP_ 00009 #define TREEIKSOLVERVEL_WDLS_HPP_ 00010 00011 #include "treeiksolver.hpp" 00012 #include "treejnttojacsolver.hpp" 00013 #include <Eigen/Core> 00014 00015 namespace KDL { 00016 00017 using namespace Eigen; 00018 00019 class TreeIkSolverVel_wdls: public TreeIkSolverVel { 00020 public: 00021 TreeIkSolverVel_wdls(const Tree& tree, const std::vector<std::string>& endpoints); 00022 virtual ~TreeIkSolverVel_wdls(); 00023 00024 virtual double CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out); 00025 00026 /* 00027 * Set the joint space weighting matrix 00028 * 00029 * @param weight_js joint space weighting symetric matrix, 00030 * default : identity. M_q : This matrix being used as a 00031 * weight for the norm of the joint space speed it HAS TO BE 00032 * symmetric and positive definite. We can actually deal with 00033 * matrices containing a symmetric and positive definite block 00034 * and 0s otherwise. Taking a diagonal matrix as an example, a 00035 * 0 on the diagonal means that the corresponding joints will 00036 * not contribute to the motion of the system. On the other 00037 * hand, the bigger the value, the most the corresponding 00038 * joint will contribute to the overall motion. The obtained 00039 * solution q_dot will actually minimize the weighted norm 00040 * sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal 00041 * with, it does not make sense to invert M_q but what is 00042 * important is the physical meaning of all this : a joint 00043 * that has a zero weight in M_q will not contribute to the 00044 * motion of the system and this is equivalent to saying that 00045 * it gets an infinite weight in the norm computation. For 00046 * more detailed explanation : vincent.padois@upmc.fr 00047 */ 00048 void setWeightJS(const MatrixXd& Mq); 00049 const MatrixXd& getWeightJS() const {return Wq;} 00050 00051 /* 00052 * Set the task space weighting matrix 00053 * 00054 * @param weight_ts task space weighting symetric matrix, 00055 * default: identity M_x : This matrix being used as a weight 00056 * for the norm of the error (in terms of task space speed) it 00057 * HAS TO BE symmetric and positive definite. We can actually 00058 * deal with matrices containing a symmetric and positive 00059 * definite block and 0s otherwise. Taking a diagonal matrix 00060 * as an example, a 0 on the diagonal means that the 00061 * corresponding task coordinate will not be taken into 00062 * account (ie the corresponding error can be really big). If 00063 * the rank of the jacobian is equal to the number of task 00064 * space coordinates which do not have a 0 weight in M_x, the 00065 * weighting will actually not impact the results (ie there is 00066 * an exact solution to the velocity inverse kinematics 00067 * problem). In cases without an exact solution, the bigger 00068 * the value, the most the corresponding task coordinate will 00069 * be taken into account (ie the more the corresponding error 00070 * will be reduced). The obtained solution will minimize the 00071 * weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|). 00072 * For more detailed explanation : vincent.padois@upmc.fr 00073 */ 00074 void setWeightTS(const MatrixXd& Mx); 00075 const MatrixXd& getWeightTS() const {return Wy;} 00076 00077 void setLambda(const double& lambda); 00078 double getLambda () const {return lambda;} 00079 00080 private: 00081 Tree tree; 00082 TreeJntToJacSolver jnttojacsolver; 00083 Jacobians jacobians; 00084 00085 MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V; 00086 VectorXd t, Wy_t, qdot, tmp, S; 00087 double lambda; 00088 }; 00089 00090 } 00091 00092 #endif /* TREEIKSOLVERVEL_WDLS_HPP_ */