KDL  1.4.0
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_ */
KDL::Vector::data
double data[3]
Definition: frames.hpp:163
KDL::TreeIkSolverVel_wdls::U
MatrixXd U
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::getWeightTS
const MatrixXd & getWeightTS() const
Definition: treeiksolvervel_wdls.hpp:77
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the tree.
Definition: tree.hpp:159
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
KDL::JntArray
Definition: jntarray.hpp:70
KDL::TreeIkSolverVel_wdls::CartToJnt
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)
Calculate inverse velocity kinematics, from joint positions and cartesian velocities to joint velocit...
Definition: treeiksolvervel_wdls.cpp:50
KDL::Jacobians
std::map< std::string, Jacobian > Jacobians
Definition: treeiksolver.hpp:19
KDL::TreeIkSolverVel_wdls::V
MatrixXd V
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::Wy_J_Wq
MatrixXd Wy_J_Wq
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeJntToJacSolver::JntToJac
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
Definition: treejnttojacsolver.cpp:21
treeiksolver.hpp
KDL::TreeIkSolverVel_wdls::Wq_V
MatrixXd Wq_V
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::setLambda
void setLambda(const double &lambda)
Definition: treeiksolvervel_wdls.cpp:46
KDL::TreeIkSolverVel_wdls::setWeightTS
void setWeightTS(const MatrixXd &Mx)
Definition: treeiksolvervel_wdls.cpp:42
KDL::TreeIkSolverVel
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
Definition: treeiksolver.hpp:54
KDL::TreeIkSolverVel_wdls::setWeightJS
void setWeightJS(const MatrixXd &Mq)
Definition: treeiksolvervel_wdls.cpp:38
KDL::TreeIkSolverVel_wdls::J_Wq
MatrixXd J_Wq
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::getLambda
double getLambda() const
Definition: treeiksolvervel_wdls.hpp:80
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::TreeIkSolverVel_wdls::~TreeIkSolverVel_wdls
virtual ~TreeIkSolverVel_wdls()
Definition: treeiksolvervel_wdls.cpp:35
KDL::TreeIkSolverVel_wdls::Wy_t
VectorXd Wy_t
Definition: treeiksolvervel_wdls.hpp:88
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::Twists
std::map< std::string, Twist > Twists
Definition: treeiksolver.hpp:18
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
treeiksolvervel_wdls.hpp
KDL::TreeIkSolverVel_wdls::t
VectorXd t
Definition: treeiksolvervel_wdls.hpp:88
KDL::TreeIkSolverVel_wdls::lambda
double lambda
Definition: treeiksolvervel_wdls.hpp:89
KDL::TreeIkSolverVel_wdls::Wq
MatrixXd Wq
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::jnttojacsolver
TreeJntToJacSolver jnttojacsolver
Definition: treeiksolvervel_wdls.hpp:84
KDL::TreeIkSolverVel_wdls::tmp
VectorXd tmp
Definition: treeiksolvervel_wdls.hpp:88
KDL::TreeIkSolverVel_wdls::J
MatrixXd J
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::Wy
MatrixXd Wy
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::E_SVD_FAILED
static const int E_SVD_FAILED
Definition: treeiksolvervel_wdls.hpp:21
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::Tree
This class encapsulates a tree kinematic interconnection structure.
Definition: tree.hpp:100
KDL::TreeIkSolverVel_wdls::getWeightJS
const MatrixXd & getWeightJS() const
Definition: treeiksolvervel_wdls.hpp:51
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:722
KDL::TreeIkSolverVel_wdls::S
VectorXd S
Definition: treeiksolvervel_wdls.hpp:88
KDL::TreeIkSolverVel_wdls::Wy_U
MatrixXd Wy_U
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls
Definition: treeiksolvervel_wdls.hpp:19
KDL::TreeIkSolverVel_wdls::tree
Tree tree
Definition: treeiksolvervel_wdls.hpp:83
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:14
KDL::TreeJntToJacSolver
Definition: treejnttojacsolver.hpp:17
KDL::TreeIkSolverVel_wdls::jacobians
Jacobians jacobians
Definition: treeiksolvervel_wdls.hpp:85
KDL::Jacobian
Definition: jacobian.hpp:37