KDL  1.4.0
Public Types | Public Member Functions | Protected Attributes | Private Attributes | List of all members
KDL::ChainIkSolverVel_pinv_nso 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_nso.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv_nso:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIkSolverVel_pinv_nso:
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_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 JntArraygetWeights () const
 Request the joint weights for optimization criterion. More...
 
const JntArraygetOptPos () 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 Chainchain
 
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
 

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.

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

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_nso() [1/2]

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.

Parameters
chainthe chain to calculate the inverse velocity kinematics for
opt_posthe desired positions of the chain used by to resolve the redundancy
weightsthe weights applied in the joint space
epsif a singular value is below this value, its inverse is set to zero, default: 0.00001
maxitermaximum iterations for the svd calculation, default: 150
alphathe null-space velocity gain

§ ChainIkSolverVel_pinv_nso() [2/2]

KDL::ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso ( const Chain chain,
double  eps = 0.00001,
int  maxiter = 150,
double  alpha = 0.25 
)
explicit

§ ~ChainIkSolverVel_pinv_nso()

KDL::ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso ( )

Member Function Documentation

§ CartToJnt() [1/2]

int KDL::ChainIkSolverVel_pinv_nso::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 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.

§ CartToJnt() [2/2]

virtual int KDL::ChainIkSolverVel_pinv_nso::CartToJnt ( const JntArray q_init,
const FrameVel v_in,
JntArrayVel q_out 
)
inlinevirtual

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

§ getAlpha()

const double& KDL::ChainIkSolverVel_pinv_nso::getAlpha ( ) const
inline

Request null space velocity gain.

Returns
const reference to the null space velocity gain

References alpha, setAlpha(), setOptPos(), and setWeights().

§ getError()

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

Return the latest error.

References KDL::SolverI::error.

§ getOptPos()

const JntArray& KDL::ChainIkSolverVel_pinv_nso::getOptPos ( ) const
inline

Request the optimal joint positions.

Returns
const reference to the optimal joint positions

References opt_pos.

§ getSVDResult()

int KDL::ChainIkSolverVel_pinv_nso::getSVDResult ( ) const
inline

Retrieve the latest return code from the SVD algorithm.

Returns
0 if CartToJnt() not yet called, otherwise latest SVD result code.

References svdResult, and updateInternalDataStructures().

§ getWeights()

const JntArray& KDL::ChainIkSolverVel_pinv_nso::getWeights ( ) const
inline

Request the joint weights for optimization criterion.

Returns
const reference to the joint weights

References weights.

§ setAlpha()

int KDL::ChainIkSolverVel_pinv_nso::setAlpha ( const double  alpha)
virtual

Set null psace velocity gain.

Parameters
alphaNUllspace velocity cgain

References alpha.

Referenced by getAlpha().

§ setOptPos()

int KDL::ChainIkSolverVel_pinv_nso::setOptPos ( const JntArray opt_pos)
virtual

Set optimal joint positions.

Parameters
opt_posoptimal 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().

§ setWeights()

int KDL::ChainIkSolverVel_pinv_nso::setWeights ( const JntArray weights)
virtual

Set joint weights for optimization criterion.

Parameters
weightsthe joint weights

References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, nj, KDL::JntArray::rows(), and weights.

Referenced by getAlpha().

§ strError()

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

§ updateInternalDataStructures()

void KDL::ChainIkSolverVel_pinv_nso::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 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().

Member Data Documentation

§ alpha

double KDL::ChainIkSolverVel_pinv_nso::alpha
private

Referenced by CartToJnt(), getAlpha(), and setAlpha().

§ chain

const Chain& KDL::ChainIkSolverVel_pinv_nso::chain
private

§ eps

double KDL::ChainIkSolverVel_pinv_nso::eps
private

Referenced by CartToJnt().

§ error

int KDL::SolverI::error
protectedinherited

§ jac

Jacobian KDL::ChainIkSolverVel_pinv_nso::jac
private

§ jnt2jac

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv_nso::jnt2jac
private

§ maxiter

int KDL::ChainIkSolverVel_pinv_nso::maxiter
private

Referenced by CartToJnt().

§ nj

unsigned int KDL::ChainIkSolverVel_pinv_nso::nj
private

§ opt_pos

JntArray KDL::ChainIkSolverVel_pinv_nso::opt_pos
private

§ S

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_nso::S
private

§ Sinv

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_nso::Sinv
private

§ svdResult

int KDL::ChainIkSolverVel_pinv_nso::svdResult
private

Referenced by CartToJnt(), and getSVDResult().

§ tmp

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_nso::tmp
private

§ tmp2

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_nso::tmp2
private

§ U

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_nso::U
private

§ V

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_nso::V
private

§ weights

JntArray KDL::ChainIkSolverVel_pinv_nso::weights
private

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