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

orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25