KDL
1.4.0
|
Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain. More...
#include <src/chainiksolvervel_pinv_nso.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_pinv_nso (const Chain &chain, const JntArray &opt_pos, const JntArray &weights, double eps=0.00001, int maxiter=150, double alpha=0.25) | |
Constructor of the solver. More... | |
ChainIkSolverVel_pinv_nso (const Chain &chain, double eps=0.00001, int maxiter=150, double alpha=0.25) | |
~ChainIkSolverVel_pinv_nso () | |
virtual int | CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) |
Calculate inverse velocity kinematics, from joint positions and cartesian velocity to joint velocities. More... | |
virtual int | CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out) |
not (yet) implemented. More... | |
const JntArray & | getWeights () const |
Request the joint weights for optimization criterion. More... | |
const JntArray & | getOptPos () const |
Request the optimal joint positions. More... | |
const double & | getAlpha () const |
Request null space velocity gain. More... | |
virtual int | setWeights (const JntArray &weights) |
Set joint weights for optimization criterion. More... | |
virtual int | setOptPos (const JntArray &opt_pos) |
Set optimal joint positions. More... | |
virtual int | setAlpha (const double alpha) |
Set null psace velocity gain. More... | |
int | getSVDResult () const |
Retrieve the latest return code from the SVD algorithm. More... | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. More... | |
virtual int | getError () const |
Return the latest error. More... | |
virtual const char * | strError (const int error) const |
Return a description of the latest error. 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::VectorXd | Sinv |
Eigen::MatrixXd | V |
Eigen::VectorXd | tmp |
Eigen::VectorXd | tmp2 |
double | eps |
int | maxiter |
int | svdResult |
double | alpha |
JntArray | weights |
JntArray | opt_pos |
Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain.
It uses a svd-calculation based on householders rotations.
In case of a redundant robot this solver optimizes the following criterium: g=0.5*sum(weight*(Desired_joint_positions - actual_joint_positions))^2 as described in A. Liegeois. Automatic supervisory control of the configuration and behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and Cybernetics, 7(12):868–871, 1977
|
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. |
KDL::ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso | ( | const Chain & | chain, |
const JntArray & | opt_pos, | ||
const JntArray & | weights, | ||
double | eps = 0.00001 , |
||
int | maxiter = 150 , |
||
double | alpha = 0.25 |
||
) |
Constructor of the solver.
chain | the chain to calculate the inverse velocity kinematics for |
opt_pos | the desired positions of the chain used by to resolve the redundancy |
weights | the weights applied in the joint space |
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 |
alpha | the null-space velocity gain |
|
explicit |
KDL::ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso | ( | ) |
|
virtual |
Calculate inverse velocity kinematics, from joint positions and cartesian velocity to joint velocities.
q_in | input joint positions |
v_in | input cartesian velocity |
qdot_out | output joint velocities |
Implements KDL::ChainIkSolverVel.
References alpha, chain, KDL::Jacobian::data, KDL::JntArray::data, 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(), maxiter, nj, opt_pos, KDL::JntArray::rows(), S, Sinv, svdResult, tmp, tmp2, U, V, and weights.
|
inlinevirtual |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
|
inline |
Request null space velocity gain.
References alpha, setAlpha(), setOptPos(), and setWeights().
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
inline |
Request the optimal joint positions.
References opt_pos.
|
inline |
Retrieve the latest return code from the SVD algorithm.
References svdResult, and updateInternalDataStructures().
|
inline |
Request the joint weights for optimization criterion.
References weights.
|
virtual |
Set null psace velocity gain.
alpha | NUllspace velocity cgain |
References alpha.
Referenced by getAlpha().
|
virtual |
Set optimal joint positions.
opt_pos | optimal joint positions |
References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, nj, opt_pos, and KDL::JntArray::rows().
Referenced by getAlpha().
|
virtual |
Set joint weights for optimization criterion.
weights | the joint weights |
References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, nj, KDL::JntArray::rows(), and weights.
Referenced by getAlpha().
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverPos_LMA, KDL::ChainJntToJacDotSolver, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, and KDL::ChainIkSolverPos_NR.
References KDL::SolverI::E_DEGRADED, KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED, KDL::SolverI::E_NO_CONVERGE, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_IMPLEMENTED, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_OUT_OF_RANGE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, KDL::SolverI::E_UNDEFINED, and KDL::SolverI::updateInternalDataStructures().
Referenced by KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainJntToJacDotSolver::strError(), KDL::ChainIkSolverPos_LMA::strError(), and KDL::ChainIkSolverVel_wdls::strError().
|
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::JntArray::data, KDL::Chain::getNrOfJoints(), jac, jnt2jac, nj, opt_pos, KDL::Jacobian::resize(), S, Sinv, tmp, tmp2, U, KDL::ChainJntToJacSolver::updateInternalDataStructures(), V, and weights.
Referenced by getSVDResult().
|
private |
Referenced by CartToJnt(), getAlpha(), and setAlpha().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), CartToJnt(), KDL::ChainIdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainIkSolverVel_pinv::getSVDResult(), KDL::ChainIkSolverVel_wdls::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(), setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), setWeights(), and KDL::ChainIkSolverVel_wdls::setWeightTS().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt().
|
private |
Referenced by CartToJnt(), setOptPos(), setWeights(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), getOptPos(), setOptPos(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and getSVDResult().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), getWeights(), setWeights(), and updateInternalDataStructures().