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         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_ */


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:17