#include <chainiksolvervel_wdls.hpp>
Public Member Functions | |
virtual int | CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) |
virtual int | CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out) |
ChainIkSolverVel_wdls (const Chain &chain, double eps=0.00001, int maxiter=150) | |
double | getLambda () const |
double | getLambdaScaled () const |
unsigned int | getNrZeroSigmas () const |
double | getSigmaMin () const |
int | getSVDResult () const |
void | setEps (const double eps_in) |
void | setLambda (const double lambda) |
void | setMaxIter (const int maxiter_in) |
void | setWeightJS (const Eigen::MatrixXd &Mq) |
void | setWeightTS (const Eigen::MatrixXd &Mx) |
virtual const char * | strError (const int error) const |
~ChainIkSolverVel_wdls () | |
Static Public Attributes | |
static const int | E_CONVERGE_PINV_SINGULAR = +100 |
solution converged but (pseudo)inverse is singular | |
static const int | E_SVD_FAILED = -100 |
Private Attributes | |
const Chain | chain |
double | eps |
Jacobian | jac |
ChainJntToJacSolver | jnt2jac |
double | lambda |
double | lambda_scaled |
int | maxiter |
unsigned int | nrZeroSigmas |
Eigen::VectorXd | S |
double | sigmaMin |
int | svdResult |
Eigen::VectorXd | tmp |
Eigen::MatrixXd | tmp_jac |
Eigen::MatrixXd | tmp_jac_weight1 |
Eigen::MatrixXd | tmp_jac_weight2 |
Eigen::MatrixXd | tmp_js |
Eigen::MatrixXd | tmp_ts |
Eigen::MatrixXd | U |
Eigen::MatrixXd | V |
Eigen::MatrixXd | weight_js |
Eigen::MatrixXd | weight_ts |
Implementation of a inverse velocity kinematics algorithm based on the weighted pseudo inverse with damped least-square to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain. It uses a svd-calculation based on householders rotations.
J# = M_q*Vb*pinv_dls(Db)*Ub'*M_x
where B = Mx*J*Mq
and B = Ub*Db*Vb' is the SVD decomposition of B
Mq and Mx represent, respectively, the joint-space and task-space weighting matrices. Please refer to the documentation of setWeightJS(const Eigen::MatrixXd& Mq) and setWeightTS(const Eigen::MatrixXd& Mx) for details on the effects of these matrices.
For more details on Weighted Pseudo Inverse, see : 1) [Ben Israel 03] A. Ben Israel & T.N.E. Greville. Generalized Inverses : Theory and Applications, second edition. Springer, 2003. ISBN 0-387-00293-6.
2) [Doty 93] K. L. Doty, C. Melchiorri & C. Boniveto. A theory of generalized inverses applied to Robotics. The International Journal of Robotics Research, vol. 12, no. 1, pages 1-19, february 1993.
Definition at line 63 of file chainiksolvervel_wdls.hpp.
KDL::ChainIkSolverVel_wdls::ChainIkSolverVel_wdls | ( | const Chain & | chain, |
double | eps = 0.00001 , |
||
int | maxiter = 150 |
||
) | [explicit] |
Constructor of the solver
chain | the chain to calculate the inverse velocity kinematics for |
eps | if a singular value is below this value, its inverse is set to zero, default: 0.00001 |
maxiter | maximum iterations for the svd calculation, default: 150 |
Definition at line 28 of file chainiksolvervel_wdls.cpp.
Definition at line 53 of file chainiksolvervel_wdls.cpp.
int KDL::ChainIkSolverVel_wdls::CartToJnt | ( | const JntArray & | q_in, |
const Twist & | v_in, | ||
JntArray & | qdot_out | ||
) | [virtual] |
Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian velocity v_in
Implements KDL::ChainIkSolverVel.
Definition at line 80 of file chainiksolvervel_wdls.cpp.
virtual int KDL::ChainIkSolverVel_wdls::CartToJnt | ( | const JntArray & | q_init, |
const FrameVel & | v_in, | ||
JntArrayVel & | q_out | ||
) | [inline, virtual] |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
Definition at line 106 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::getLambda | ( | ) | const [inline] |
Request the value of lambda for the minimum
Definition at line 185 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::getLambdaScaled | ( | ) | const [inline] |
Request the scaled value of lambda for the minimum singular value 1-6
Definition at line 191 of file chainiksolvervel_wdls.hpp.
unsigned int KDL::ChainIkSolverVel_wdls::getNrZeroSigmas | ( | ) | const [inline] |
Request the number of singular values of the jacobian that are < eps; if the number of near zero singular values is > jac.col()-jac.row(), then the jacobian pseudoinverse is singular
Definition at line 175 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::getSigmaMin | ( | ) | const [inline] |
Request the minimum of the first six singular values
Definition at line 180 of file chainiksolvervel_wdls.hpp.
int KDL::ChainIkSolverVel_wdls::getSVDResult | ( | ) | const [inline] |
Retrieve the latest return code from the SVD algorithm
Definition at line 197 of file chainiksolvervel_wdls.hpp.
void KDL::ChainIkSolverVel_wdls::setEps | ( | const double | eps_in | ) |
Set eps
Definition at line 70 of file chainiksolvervel_wdls.cpp.
void KDL::ChainIkSolverVel_wdls::setLambda | ( | const double | lambda | ) |
Set lambda
Definition at line 65 of file chainiksolvervel_wdls.cpp.
void KDL::ChainIkSolverVel_wdls::setMaxIter | ( | const int | maxiter_in | ) |
Set maxIter
Definition at line 75 of file chainiksolvervel_wdls.cpp.
void KDL::ChainIkSolverVel_wdls::setWeightJS | ( | const Eigen::MatrixXd & | Mq | ) |
Set the joint space weighting matrix
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 |
Definition at line 57 of file chainiksolvervel_wdls.cpp.
void KDL::ChainIkSolverVel_wdls::setWeightTS | ( | const Eigen::MatrixXd & | Mx | ) |
Set the task space weighting matrix
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 |
Definition at line 61 of file chainiksolvervel_wdls.cpp.
const char * KDL::ChainIkSolverVel_wdls::strError | ( | const int | error | ) | const [virtual] |
Return a description of the latest error
Reimplemented from KDL::SolverI.
Definition at line 176 of file chainiksolvervel_wdls.cpp.
const Chain KDL::ChainIkSolverVel_wdls::chain [private] |
Definition at line 203 of file chainiksolvervel_wdls.hpp.
const int KDL::ChainIkSolverVel_wdls::E_CONVERGE_PINV_SINGULAR = +100 [static] |
solution converged but (pseudo)inverse is singular
SVD solver failed
Definition at line 68 of file chainiksolvervel_wdls.hpp.
const int KDL::ChainIkSolverVel_wdls::E_SVD_FAILED = -100 [static] |
Definition at line 66 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::eps [private] |
Definition at line 209 of file chainiksolvervel_wdls.hpp.
Jacobian KDL::ChainIkSolverVel_wdls::jac [private] |
Definition at line 205 of file chainiksolvervel_wdls.hpp.
Definition at line 204 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::lambda [private] |
Definition at line 219 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::lambda_scaled [private] |
Definition at line 220 of file chainiksolvervel_wdls.hpp.
int KDL::ChainIkSolverVel_wdls::maxiter [private] |
Definition at line 210 of file chainiksolvervel_wdls.hpp.
unsigned int KDL::ChainIkSolverVel_wdls::nrZeroSigmas [private] |
Definition at line 221 of file chainiksolvervel_wdls.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_wdls::S [private] |
Definition at line 207 of file chainiksolvervel_wdls.hpp.
double KDL::ChainIkSolverVel_wdls::sigmaMin [private] |
Definition at line 223 of file chainiksolvervel_wdls.hpp.
int KDL::ChainIkSolverVel_wdls::svdResult [private] |
Definition at line 222 of file chainiksolvervel_wdls.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_wdls::tmp [private] |
Definition at line 211 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac [private] |
Definition at line 212 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac_weight1 [private] |
Definition at line 213 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac_weight2 [private] |
Definition at line 214 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_js [private] |
Definition at line 216 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_ts [private] |
Definition at line 215 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::U [private] |
Definition at line 206 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::V [private] |
Definition at line 208 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::weight_js [private] |
Definition at line 218 of file chainiksolvervel_wdls.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::weight_ts [private] |
Definition at line 217 of file chainiksolvervel_wdls.hpp.