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