KDL  1.3.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 }
 

Public Member Functions

 ChainIkSolverVel_pinv_nso (const Chain &chain, JntArray opt_pos, 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...
 
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...
 
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
 
Jacobian jac
 
SVD_HH svd
 
std::vector< JntArray > U
 
JntArray S
 
std::vector< JntArray > V
 
JntArray tmp
 
JntArray tmp2
 
double eps
 
int maxiter
 
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 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 mechnisms. IEEE Transactions on Systems, Man, and Cybernetics, 7(12):868–871, 1977

Member Enumeration Documentation

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) )

Constructor & Destructor Documentation

KDL::ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso ( const Chain chain,
JntArray  opt_pos,
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
KDL::ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso ( const Chain chain,
double  eps = 0.00001,
int  maxiter = 150,
double  alpha = 0.25 
)
explicit
KDL::ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso ( )

Member Function Documentation

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, KDL::Jacobian::columns(), eps, jac, jnt2jac, KDL::ChainJntToJacSolver::JntToJac(), maxiter, opt_pos, KDL::Jacobian::rows(), S, svd, tmp, tmp2, U, V, and weights.

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.

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

Return the latest error.

References KDL::SolverI::error.

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

Set null psace velocity gain.

Parameters
alphaNUllspace velocity cgain

References alpha.

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

Set optimal joint positions.

Parameters
opt_posoptimal joint positions

References opt_pos.

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

Set joint weights for optimization criterion.

Parameters
weightsthe joint weights

References weights.

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

Member Data Documentation

double KDL::ChainIkSolverVel_pinv_nso::alpha
private

Referenced by CartToJnt(), and setAlpha().

const Chain KDL::ChainIkSolverVel_pinv_nso::chain
private
double KDL::ChainIkSolverVel_pinv_nso::eps
private

Referenced by CartToJnt().

int KDL::SolverI::error
protectedinherited
Jacobian KDL::ChainIkSolverVel_pinv_nso::jac
private

Referenced by CartToJnt().

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv_nso::jnt2jac
private

Referenced by CartToJnt().

int KDL::ChainIkSolverVel_pinv_nso::maxiter
private

Referenced by CartToJnt().

JntArray KDL::ChainIkSolverVel_pinv_nso::opt_pos
private

Referenced by CartToJnt(), and setOptPos().

JntArray KDL::ChainIkSolverVel_pinv_nso::S
private

Referenced by CartToJnt().

SVD_HH KDL::ChainIkSolverVel_pinv_nso::svd
private

Referenced by CartToJnt().

JntArray KDL::ChainIkSolverVel_pinv_nso::tmp
private

Referenced by CartToJnt().

JntArray KDL::ChainIkSolverVel_pinv_nso::tmp2
private

Referenced by CartToJnt().

std::vector<JntArray> KDL::ChainIkSolverVel_pinv_nso::U
private

Referenced by CartToJnt().

std::vector<JntArray> KDL::ChainIkSolverVel_pinv_nso::V
private

Referenced by CartToJnt().

JntArray KDL::ChainIkSolverVel_pinv_nso::weights
private

Referenced by CartToJnt(), and setWeights().


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