KDL 1.5.1
Loading...
Searching...
No Matches
Public Types | Public Member Functions | Protected Attributes | Private Attributes | List of all members
KDL::ChainIkSolverVel_pinv_givens Class Reference

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_givens.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv_givens:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIkSolverVel_pinv_givens:
Collaboration graph
[legend]

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_givens (const Chain &chain)
 Constructor of the solver. More...
 
 ~ChainIkSolverVel_pinv_givens ()
 
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 &, const FrameVel &, JntArrayVel &)
 not (yet) implemented. 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 Chainchain
 
unsigned int nj
 
ChainJntToJacSolver jnt2jac
 
Jacobian jac
 
bool transpose
 
bool toggle
 
unsigned int m
 
unsigned int n
 
Eigen::MatrixXd jac_eigen
 
Eigen::MatrixXd U
 
Eigen::MatrixXd V
 
Eigen::MatrixXd B
 
Eigen::VectorXd S
 
Eigen::VectorXd tempi
 
Eigen::VectorXd UY
 
Eigen::VectorXd SUY
 
Eigen::VectorXd qdot_eigen
 
Eigen::VectorXd v_in_eigen
 

Detailed Description

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.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
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.

Constructor & Destructor Documentation

◆ ChainIkSolverVel_pinv_givens()

KDL::ChainIkSolverVel_pinv_givens::ChainIkSolverVel_pinv_givens ( const Chain chain)
explicit

Constructor of the solver.

Parameters
chainthe chain to calculate the inverse velocity kinematics for

◆ ~ChainIkSolverVel_pinv_givens()

KDL::ChainIkSolverVel_pinv_givens::~ChainIkSolverVel_pinv_givens ( )

Member Function Documentation

◆ CartToJnt() [1/2]

virtual int KDL::ChainIkSolverVel_pinv_givens::CartToJnt ( const JntArray ,
const FrameVel ,
JntArrayVel  
)
inlinevirtual

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

References KDL::SolverI::E_NOT_IMPLEMENTED, and KDL::SolverI::error.

◆ CartToJnt() [2/2]

int KDL::ChainIkSolverVel_pinv_givens::CartToJnt ( const JntArray q_in,
const Twist v_in,
JntArray qdot_out 
)
virtual

Calculate inverse velocity kinematics, from joint positions and cartesian velocity to joint velocities.

Parameters
q_ininput joint positions
v_ininput cartesian velocity
qdot_outoutput joint velocities
Returns
if < 0 something went wrong

Implements KDL::ChainIkSolverVel.

References B, chain, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), jac, jac_eigen, jnt2jac, KDL::ChainJntToJacSolver::JntToJac(), m, n, nj, qdot_eigen, KDL::JntArray::rows(), S, SUY, KDL::svd_eigen_Macie(), tempi, toggle, transpose, U, UY, V, and v_in_eigen.

◆ getError()

virtual int KDL::SolverI::getError ( ) const
inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

◆ strError()

virtual const char * KDL::SolverI::strError ( const int  error) const
inlinevirtualinherited

◆ updateInternalDataStructures()

void KDL::ChainIkSolverVel_pinv_givens::updateInternalDataStructures ( )
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 B, chain, KDL::Chain::getNrOfJoints(), jac, jac_eigen, jnt2jac, m, n, nj, qdot_eigen, KDL::Jacobian::resize(), S, SUY, tempi, transpose, U, KDL::ChainJntToJacSolver::updateInternalDataStructures(), and V.

Member Data Documentation

◆ B

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_givens::B
private

◆ chain

const Chain& KDL::ChainIkSolverVel_pinv_givens::chain
private

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainExternalWrenchEstimator::JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainExternalWrenchEstimator::setInitialMomentum(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), KDL::ChainIkSolverVel_wdls::setWeightTS(), KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), KDL::ChainJntToJacDotSolver::strError(), and KDL::SolverI::strError().

◆ jac

Jacobian KDL::ChainIkSolverVel_pinv_givens::jac
private

◆ jac_eigen

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_givens::jac_eigen
private

◆ jnt2jac

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv_givens::jnt2jac
private

◆ m

unsigned int KDL::ChainIkSolverVel_pinv_givens::m
private

◆ n

unsigned int KDL::ChainIkSolverVel_pinv_givens::n
private

◆ nj

unsigned int KDL::ChainIkSolverVel_pinv_givens::nj
private

◆ qdot_eigen

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_givens::qdot_eigen
private

◆ S

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_givens::S
private

◆ SUY

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_givens::SUY
private

◆ tempi

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_givens::tempi
private

◆ toggle

bool KDL::ChainIkSolverVel_pinv_givens::toggle
private

Referenced by CartToJnt().

◆ transpose

bool KDL::ChainIkSolverVel_pinv_givens::transpose
private

◆ U

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_givens::U
private

◆ UY

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_givens::UY
private

Referenced by CartToJnt().

◆ V

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_givens::V
private

◆ v_in_eigen

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_givens::v_in_eigen
private

Referenced by CartToJnt().


The documentation for this class was generated from the following files: