KDL
1.4.0
|
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. More...
#include <src/chainiksolvervel_wdls.hpp>
Public Types | |
enum | { E_DEGRADED = +1, E_NOERROR = 0, E_NO_CONVERGE = -1, E_UNDEFINED = -2, E_NOT_UP_TO_DATE = -3, E_SIZE_MISMATCH = -4, E_MAX_ITERATIONS_EXCEEDED = -5, E_OUT_OF_RANGE = -6, E_NOT_IMPLEMENTED = -7, E_SVD_FAILED = -8 } |
Public Member Functions | |
ChainIkSolverVel_wdls (const Chain &chain, double eps=0.00001, int maxiter=150) | |
Constructor of the solver. More... | |
~ChainIkSolverVel_wdls () | |
virtual int | CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) |
Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian velocity v_in. More... | |
virtual int | CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out) |
not (yet) implemented. More... | |
int | setWeightJS (const Eigen::MatrixXd &Mq) |
Set the joint space weighting matrix. More... | |
int | setWeightTS (const Eigen::MatrixXd &Mx) |
Set the task space weighting matrix. More... | |
void | setLambda (const double lambda) |
Set lambda. More... | |
void | setEps (const double eps_in) |
Set eps. More... | |
void | setMaxIter (const int maxiter_in) |
Set maxIter. More... | |
unsigned int | getNrZeroSigmas () const |
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. More... | |
double | getSigmaMin () const |
Request the minimum of the first six singular values. More... | |
int | getSigma (Eigen::VectorXd &Sout) |
Request the six singular values of the Jacobian. More... | |
double | getEps () const |
Request the value of eps. More... | |
double | getLambda () const |
Request the value of lambda for the minimum. More... | |
double | getLambdaScaled () const |
Request the scaled value of lambda for the minimum singular value 1-6. More... | |
int | getSVDResult () const |
Retrieve the latest return code from the SVD algorithm. More... | |
virtual const char * | strError (const int error) const |
Return a description of the latest error. More... | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. More... | |
virtual int | getError () const |
Return the latest error. More... | |
Static Public Attributes | |
static const int | E_CONVERGE_PINV_SINGULAR = +100 |
solution converged but (pseudo)inverse is singular More... | |
Protected Attributes | |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
Private Attributes | |
const Chain & | chain |
ChainJntToJacSolver | jnt2jac |
unsigned int | nj |
Jacobian | jac |
Eigen::MatrixXd | U |
Eigen::VectorXd | S |
Eigen::MatrixXd | V |
double | eps |
int | maxiter |
Eigen::VectorXd | tmp |
Eigen::MatrixXd | tmp_jac |
Eigen::MatrixXd | tmp_jac_weight1 |
Eigen::MatrixXd | tmp_jac_weight2 |
Eigen::MatrixXd | tmp_ts |
Eigen::MatrixXd | tmp_js |
Eigen::MatrixXd | weight_ts |
Eigen::MatrixXd | weight_js |
double | lambda |
double | lambda_scaled |
unsigned int | nrZeroSigmas |
int | svdResult |
double | sigmaMin |
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.
|
inherited |
Enumerator | |
---|---|
E_DEGRADED | Converged but degraded solution (e.g. WDLS with psuedo-inverse singular) |
E_NOERROR | No error. |
E_NO_CONVERGE | Failed to converge. |
E_UNDEFINED | Undefined value (e.g. computed a NAN, or tan(90 degrees) ) |
E_NOT_UP_TO_DATE | Chain size changed. |
E_SIZE_MISMATCH | Input size does not match internal state. |
E_MAX_ITERATIONS_EXCEEDED | Maximum number of iterations exceeded. |
E_OUT_OF_RANGE | Requested index out of range. |
E_NOT_IMPLEMENTED | Not yet implemented. |
E_SVD_FAILED | Internal svd calculation failed. |
|
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 |
KDL::ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls | ( | ) |
|
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.
References chain, KDL::Jacobian::columns(), KDL::Jacobian::data, KDL::JntArray::data, E_CONVERGE_PINV_SINGULAR, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, eps, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), jac, jnt2jac, KDL::ChainJntToJacSolver::JntToJac(), lambda, lambda_scaled, maxiter, nj, nrZeroSigmas, KDL::Jacobian::rows(), KDL::JntArray::rows(), S, sigmaMin, svdResult, tmp, tmp_jac_weight1, tmp_jac_weight2, tmp_js, tmp_ts, U, V, weight_js, and weight_ts.
|
inlinevirtual |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
References lambda, setEps(), setLambda(), setMaxIter(), setWeightJS(), and setWeightTS().
|
inline |
Request the value of eps.
References eps.
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
inline |
Request the value of lambda for the minimum.
References lambda.
|
inline |
Request the scaled value of lambda for the minimum singular value 1-6.
References lambda_scaled.
|
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.
References nrZeroSigmas.
int KDL::ChainIkSolverVel_wdls::getSigma | ( | Eigen::VectorXd & | Sout | ) |
Request the six singular values of the Jacobian.
References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, and S.
Referenced by getSigmaMin().
|
inline |
Request the minimum of the first six singular values.
References getSigma(), and sigmaMin.
|
inline |
Retrieve the latest return code from the SVD algorithm.
References KDL::SolverI::error, strError(), svdResult, and updateInternalDataStructures().
void KDL::ChainIkSolverVel_wdls::setEps | ( | const double | eps_in | ) |
void KDL::ChainIkSolverVel_wdls::setLambda | ( | const double | lambda | ) |
void KDL::ChainIkSolverVel_wdls::setMaxIter | ( | const int | maxiter_in | ) |
int KDL::ChainIkSolverVel_wdls::setWeightJS | ( | const Eigen::MatrixXd & | Mq | ) |
Set the joint space weighting matrix.
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 : vince nt.p adois @upm c.fr |
References chain, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), nj, and weight_js.
Referenced by CartToJnt().
int KDL::ChainIkSolverVel_wdls::setWeightTS | ( | const Eigen::MatrixXd & | Mx | ) |
Set the task space weighting matrix.
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 : vince nt.p adois @upm c.fr |
References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, and weight_ts.
Referenced by CartToJnt().
|
virtual |
Return a description of the latest error.
Reimplemented from KDL::SolverI.
References E_CONVERGE_PINV_SINGULAR, and KDL::SolverI::strError().
Referenced by getSVDResult().
|
virtual |
Update the internal data structures.
This is required if the number of segments or number of joints of a chain/tree have changed. This provides a single point of contact for solver memory allocations.
Implements KDL::ChainIkSolverVel.
References chain, KDL::Chain::getNrOfJoints(), jac, jnt2jac, nj, KDL::Jacobian::resize(), S, tmp, tmp_jac, tmp_jac_weight1, tmp_jac_weight2, tmp_js, U, KDL::ChainJntToJacSolver::updateInternalDataStructures(), V, and weight_js.
Referenced by getSVDResult().
|
private |
Referenced by CartToJnt(), setWeightJS(), and updateInternalDataStructures().
|
static |
solution converged but (pseudo)inverse is singular
Referenced by CartToJnt(), and strError().
|
private |
Referenced by CartToJnt(), getEps(), and setEps().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), getSigma(), KDL::ChainIkSolverVel_pinv::getSVDResult(), getSVDResult(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainJntToJacDotSolver::setInternialRepresentation(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), and setWeightTS().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), getLambda(), and setLambda().
|
private |
Referenced by CartToJnt(), and getLambdaScaled().
|
private |
Referenced by CartToJnt(), and setMaxIter().
|
private |
Referenced by CartToJnt(), setWeightJS(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and getNrZeroSigmas().
|
private |
Referenced by CartToJnt(), getSigma(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and getSigmaMin().
|
private |
Referenced by CartToJnt(), and getSVDResult().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), setWeightJS(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and setWeightTS().