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

orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44