orocos_kdl
treeiksolvervel_wdls.hpp
Go to the documentation of this file.
1 /*
2  * TreeIkSolverVel_wdls.hpp
3  *
4  * Created on: Nov 28, 2008
5  * Author: rubensmits
6  */
7 
8 #ifndef TREEIKSOLVERVEL_WDLS_HPP_
9 #define TREEIKSOLVERVEL_WDLS_HPP_
10 
11 #include "treeiksolver.hpp"
12 #include "treejnttojacsolver.hpp"
13 #include <Eigen/Core>
14 
15 namespace KDL {
16 
18  public:
19  static const int E_SVD_FAILED = -100;
20 
21  TreeIkSolverVel_wdls(const Tree& tree, const std::vector<std::string>& endpoints);
22  virtual ~TreeIkSolverVel_wdls();
23 
24  virtual double CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out);
25 
26  /*
27  * Set the joint space weighting matrix
28  *
29  * @param weight_js joint space weighting symmetric matrix,
30  * default : identity. M_q : This matrix being used as a
31  * weight for the norm of the joint space speed it HAS TO BE
32  * symmetric and positive definite. We can actually deal with
33  * matrices containing a symmetric and positive definite block
34  * and 0s otherwise. Taking a diagonal matrix as an example, a
35  * 0 on the diagonal means that the corresponding joints will
36  * not contribute to the motion of the system. On the other
37  * hand, the bigger the value, the most the corresponding
38  * joint will contribute to the overall motion. The obtained
39  * solution q_dot will actually minimize the weighted norm
40  * sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal
41  * with, it does not make sense to invert M_q but what is
42  * important is the physical meaning of all this : a joint
43  * that has a zero weight in M_q will not contribute to the
44  * motion of the system and this is equivalent to saying that
45  * it gets an infinite weight in the norm computation. For
46  * more detailed explanation : vincent.padois@upmc.fr
47  */
48  void setWeightJS(const Eigen::MatrixXd& Mq);
49  const Eigen::MatrixXd& getWeightJS() const {return Wq;}
50 
51  /*
52  * Set the task space weighting matrix
53  *
54  * @param weight_ts task space weighting symmetric matrix,
55  * default: identity M_x : This matrix being used as a weight
56  * for the norm of the error (in terms of task space speed) it
57  * HAS TO BE symmetric and positive definite. We can actually
58  * deal with matrices containing a symmetric and positive
59  * definite block and 0s otherwise. Taking a diagonal matrix
60  * as an example, a 0 on the diagonal means that the
61  * corresponding task coordinate will not be taken into
62  * account (ie the corresponding error can be really big). If
63  * the rank of the jacobian is equal to the number of task
64  * space coordinates which do not have a 0 weight in M_x, the
65  * weighting will actually not impact the results (ie there is
66  * an exact solution to the velocity inverse kinematics
67  * problem). In cases without an exact solution, the bigger
68  * the value, the most the corresponding task coordinate will
69  * be taken into account (ie the more the corresponding error
70  * will be reduced). The obtained solution will minimize the
71  * weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|).
72  * For more detailed explanation : vincent.padois@upmc.fr
73  */
74  void setWeightTS(const Eigen::MatrixXd& Mx);
75  const Eigen::MatrixXd& getWeightTS() const {return Wy;}
76 
77  void setLambda(const double& lambda);
78  double getLambda () const {return lambda;}
79 
80  private:
84 
85  Eigen::MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V;
86  Eigen::VectorXd t, Wy_t, qdot, tmp, S;
87  double lambda;
88  };
89 
90 }
91 
92 #endif /* TREEIKSOLVERVEL_WDLS_HPP_ */
KDL::TreeIkSolverVel_wdls::tmp
Eigen::VectorXd tmp
Definition: treeiksolvervel_wdls.hpp:86
KDL::TreeIkSolverVel_wdls::getWeightTS
const Eigen::MatrixXd & getWeightTS() const
Definition: treeiksolvervel_wdls.hpp:75
KDL::TreeIkSolverVel_wdls::Wq_V
Eigen::MatrixXd Wq_V
Definition: treeiksolvervel_wdls.hpp:85
KDL::JntArray
Definition: jntarray.hpp:69
KDL::TreeIkSolverVel_wdls::CartToJnt
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)
Definition: treeiksolvervel_wdls.cpp:48
KDL::TreeIkSolverVel_wdls::J_Wq
Eigen::MatrixXd J_Wq
Definition: treeiksolvervel_wdls.hpp:85
std::vector< std::string >
treeiksolver.hpp
KDL::TreeIkSolverVel_wdls::setLambda
void setLambda(const double &lambda)
Definition: treeiksolvervel_wdls.cpp:44
KDL::TreeIkSolverVel_wdls::J
Eigen::MatrixXd J
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::qdot
Eigen::VectorXd qdot
Definition: treeiksolvervel_wdls.hpp:86
KDL::TreeIkSolverVel
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
Definition: treeiksolver.hpp:54
KDL::TreeIkSolverVel_wdls::getWeightJS
const Eigen::MatrixXd & getWeightJS() const
Definition: treeiksolvervel_wdls.hpp:49
KDL::TreeIkSolverVel_wdls::getLambda
double getLambda() const
Definition: treeiksolvervel_wdls.hpp:78
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::TreeIkSolverVel_wdls::~TreeIkSolverVel_wdls
virtual ~TreeIkSolverVel_wdls()
Definition: treeiksolvervel_wdls.cpp:33
KDL::TreeIkSolverVel_wdls::setWeightJS
void setWeightJS(const Eigen::MatrixXd &Mq)
Definition: treeiksolvervel_wdls.cpp:36
KDL::TreeIkSolverVel_wdls::Wy_J_Wq
Eigen::MatrixXd Wy_J_Wq
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::lambda
double lambda
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::jnttojacsolver
TreeJntToJacSolver jnttojacsolver
Definition: treeiksolvervel_wdls.hpp:82
std::map< std::string, Twist >
KDL::TreeIkSolverVel_wdls::E_SVD_FAILED
static const int E_SVD_FAILED
Definition: treeiksolvervel_wdls.hpp:19
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
KDL::TreeIkSolverVel_wdls::setWeightTS
void setWeightTS(const Eigen::MatrixXd &Mx)
Definition: treeiksolvervel_wdls.cpp:40
KDL::TreeIkSolverVel_wdls::S
Eigen::VectorXd S
Definition: treeiksolvervel_wdls.hpp:86
KDL::TreeIkSolverVel_wdls::Wy
Eigen::MatrixXd Wy
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::t
Eigen::VectorXd t
Definition: treeiksolvervel_wdls.hpp:86
KDL::TreeIkSolverVel_wdls::U
Eigen::MatrixXd U
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::Wq
Eigen::MatrixXd Wq
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls
Definition: treeiksolvervel_wdls.hpp:17
KDL::TreeIkSolverVel_wdls::tree
Tree tree
Definition: treeiksolvervel_wdls.hpp:81
treejnttojacsolver.hpp
KDL::TreeIkSolverVel_wdls::TreeIkSolverVel_wdls
TreeIkSolverVel_wdls(const Tree &tree, const std::vector< std::string > &endpoints)
Child SVD failed.
Definition: treeiksolvervel_wdls.cpp:12
KDL::TreeIkSolverVel_wdls::Wy_U
Eigen::MatrixXd Wy_U
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::Wy_t
Eigen::VectorXd Wy_t
Definition: treeiksolvervel_wdls.hpp:86
KDL::TreeJntToJacSolver
Definition: treejnttojacsolver.hpp:17
KDL::TreeIkSolverVel_wdls::jacobians
Jacobians jacobians
Definition: treeiksolvervel_wdls.hpp:83
KDL::TreeIkSolverVel_wdls::V
Eigen::MatrixXd V
Definition: treeiksolvervel_wdls.hpp:85