KDL 1.5.1
Loading...
Searching...
No Matches
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"
13#include <Eigen/Core>
14
15namespace 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_ */
Definition: jntarray.hpp:70
Definition: treeiksolvervel_wdls.hpp:17
const Eigen::MatrixXd & getWeightJS() const
Definition: treeiksolvervel_wdls.hpp:49
Eigen::VectorXd tmp
Definition: treeiksolvervel_wdls.hpp:86
virtual ~TreeIkSolverVel_wdls()
Definition: treeiksolvervel_wdls.cpp:33
Eigen::MatrixXd V
Definition: treeiksolvervel_wdls.hpp:85
Eigen::VectorXd t
Definition: treeiksolvervel_wdls.hpp:86
Eigen::MatrixXd Wy_U
Definition: treeiksolvervel_wdls.hpp:85
Eigen::MatrixXd J
Definition: treeiksolvervel_wdls.hpp:85
void setWeightJS(const Eigen::MatrixXd &Mq)
Definition: treeiksolvervel_wdls.cpp:36
Eigen::VectorXd Wy_t
Definition: treeiksolvervel_wdls.hpp:86
static const int E_SVD_FAILED
Definition: treeiksolvervel_wdls.hpp:19
TreeJntToJacSolver jnttojacsolver
Definition: treeiksolvervel_wdls.hpp:82
Eigen::MatrixXd Wy
Definition: treeiksolvervel_wdls.hpp:85
Jacobians jacobians
Definition: treeiksolvervel_wdls.hpp:83
Eigen::MatrixXd U
Definition: treeiksolvervel_wdls.hpp:85
Eigen::MatrixXd Wq
Definition: treeiksolvervel_wdls.hpp:85
double lambda
Definition: treeiksolvervel_wdls.hpp:87
void setWeightTS(const Eigen::MatrixXd &Mx)
Definition: treeiksolvervel_wdls.cpp:40
Tree tree
Definition: treeiksolvervel_wdls.hpp:81
Eigen::MatrixXd Wq_V
Definition: treeiksolvervel_wdls.hpp:85
Eigen::VectorXd S
Definition: treeiksolvervel_wdls.hpp:86
void setLambda(const double &lambda)
Definition: treeiksolvervel_wdls.cpp:44
double getLambda() const
Definition: treeiksolvervel_wdls.hpp:78
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:48
Eigen::MatrixXd J_Wq
Definition: treeiksolvervel_wdls.hpp:85
const Eigen::MatrixXd & getWeightTS() const
Definition: treeiksolvervel_wdls.hpp:75
Eigen::MatrixXd Wy_J_Wq
Definition: treeiksolvervel_wdls.hpp:85
Eigen::VectorXd qdot
Definition: treeiksolvervel_wdls.hpp:86
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
Definition: treeiksolver.hpp:54
Definition: treejnttojacsolver.hpp:17
This class encapsulates a tree kinematic interconnection structure.
Definition: tree.hpp:100
Definition: articulatedbodyinertia.cpp:26
std::map< std::string, Twist > Twists
Definition: treeiksolver.hpp:18
std::map< std::string, Jacobian > Jacobians
Definition: treeiksolver.hpp:19