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_ */
void setWeightTS(const Eigen::MatrixXd &Mx)
const Eigen::MatrixXd & getWeightJS() const
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
const Eigen::MatrixXd & getWeightTS() const
std::map< std::string, Jacobian > Jacobians
void setLambda(const double &lambda)
TreeIkSolverVel_wdls(const Tree &tree, const std::vector< std::string > &endpoints)
Child SVD failed.
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
void setWeightJS(const Eigen::MatrixXd &Mq)
std::map< std::string, Twist > Twists
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Definition: tree.hpp:99


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14