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