93 lines
3.9 KiB
C++
93 lines
3.9 KiB
C++
/*
|
|
* 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 {
|
|
|
|
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 symmetric 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 Eigen::MatrixXd& Mq);
|
|
const Eigen::MatrixXd& getWeightJS() const {return Wq;}
|
|
|
|
/*
|
|
* Set the task space weighting matrix
|
|
*
|
|
* @param weight_ts task space weighting symmetric 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 Eigen::MatrixXd& Mx);
|
|
const Eigen::MatrixXd& getWeightTS() const {return Wy;}
|
|
|
|
void setLambda(const double& lambda);
|
|
double getLambda () const {return lambda;}
|
|
|
|
private:
|
|
Tree tree;
|
|
TreeJntToJacSolver jnttojacsolver;
|
|
Jacobians jacobians;
|
|
|
|
Eigen::MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V;
|
|
Eigen::VectorXd t, Wy_t, qdot, tmp, S;
|
|
double lambda;
|
|
};
|
|
|
|
}
|
|
|
|
#endif /* TREEIKSOLVERVEL_WDLS_HPP_ */
|