orocos_kdl
Public Member Functions | Private Attributes | List of all members
KDL::ChainIkSolverVel_pinv_nso Class Reference

#include <chainiksolvervel_pinv_nso.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv_nso:
Inheritance graph
[legend]

Public Member Functions

virtual int CartToJnt (const JntArray &, const FrameVel &, JntArrayVel &)
 
virtual int CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
 
 ChainIkSolverVel_pinv_nso (const Chain &chain, const JntArray &opt_pos, const JntArray &weights, double eps=0.00001, int maxiter=150, double alpha=0.25)
 
 ChainIkSolverVel_pinv_nso (const Chain &chain, double eps=0.00001, int maxiter=150, double alpha=0.25)
 
const double & getAlpha () const
 
const JntArraygetOptPos () const
 
int getSVDResult () const
 
const JntArraygetWeights () const
 
virtual int setAlpha (const double alpha)
 
virtual int setOptPos (const JntArray &opt_pos)
 
virtual int setWeights (const JntArray &weights)
 
virtual void updateInternalDataStructures ()
 
 ~ChainIkSolverVel_pinv_nso ()
 
- Public Member Functions inherited from KDL::ChainIkSolverVel
virtual ~ChainIkSolverVel ()
 
- Public Member Functions inherited from KDL::SolverI
virtual int getError () const
 Return the latest error. More...
 
 SolverI ()
 Initialize latest error to E_NOERROR. More...
 
virtual const char * strError (const int error) const
 
virtual ~SolverI ()
 

Private Attributes

double alpha
 
const Chainchain
 
double eps
 
Jacobian jac
 
ChainJntToJacSolver jnt2jac
 
int maxiter
 
unsigned int nj
 
JntArray opt_pos
 
Eigen::VectorXd S
 
Eigen::VectorXd Sinv
 
int svdResult
 
Eigen::VectorXd tmp
 
Eigen::VectorXd tmp2
 
Eigen::MatrixXd U
 
Eigen::MatrixXd V
 
JntArray weights
 

Additional Inherited Members

- Public Types inherited from KDL::SolverI
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
}
 
- Protected Attributes inherited from KDL::SolverI
int error
 Latest error, initialized to E_NOERROR in constructor. More...
 

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

Definition at line 46 of file chainiksolvervel_pinv_nso.hpp.

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

Definition at line 27 of file chainiksolvervel_pinv_nso.cpp.

◆ 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

Definition at line 47 of file chainiksolvervel_pinv_nso.cpp.

◆ ~ChainIkSolverVel_pinv_nso()

KDL::ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso ( )

Definition at line 79 of file chainiksolvervel_pinv_nso.cpp.

Member Function Documentation

◆ CartToJnt() [1/2]

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

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

Definition at line 72 of file chainiksolvervel_pinv_nso.hpp.

◆ CartToJnt() [2/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.

Definition at line 84 of file chainiksolvervel_pinv_nso.cpp.

◆ getAlpha()

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

Request null space velocity gain

Returns
const reference to the null space velocity gain

Definition at line 102 of file chainiksolvervel_pinv_nso.hpp.

◆ getOptPos()

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

Request the optimal joint positions

Returns
const reference to the optimal joint positions

Definition at line 91 of file chainiksolvervel_pinv_nso.hpp.

◆ 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.

Definition at line 135 of file chainiksolvervel_pinv_nso.hpp.

◆ getWeights()

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

Request the joint weights for optimization criterion

Returns
const reference to the joint weights

Definition at line 80 of file chainiksolvervel_pinv_nso.hpp.

◆ setAlpha()

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

Set null space velocity gain

Parameters
alphaNUllspace velocity cgain

Definition at line 182 of file chainiksolvervel_pinv_nso.cpp.

◆ setOptPos()

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

Set optimal joint positions

Parameters
opt_posoptimal joint positions

Definition at line 174 of file chainiksolvervel_pinv_nso.cpp.

◆ setWeights()

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

Set joint weights for optimization criterion

Parameters
weightsthe joint weights

Definition at line 166 of file chainiksolvervel_pinv_nso.cpp.

◆ 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.

Definition at line 65 of file chainiksolvervel_pinv_nso.cpp.

Member Data Documentation

◆ alpha

double KDL::ChainIkSolverVel_pinv_nso::alpha
private

Definition at line 154 of file chainiksolvervel_pinv_nso.hpp.

◆ chain

const Chain& KDL::ChainIkSolverVel_pinv_nso::chain
private

Definition at line 141 of file chainiksolvervel_pinv_nso.hpp.

◆ eps

double KDL::ChainIkSolverVel_pinv_nso::eps
private

Definition at line 151 of file chainiksolvervel_pinv_nso.hpp.

◆ jac

Jacobian KDL::ChainIkSolverVel_pinv_nso::jac
private

Definition at line 144 of file chainiksolvervel_pinv_nso.hpp.

◆ jnt2jac

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv_nso::jnt2jac
private

Definition at line 142 of file chainiksolvervel_pinv_nso.hpp.

◆ maxiter

int KDL::ChainIkSolverVel_pinv_nso::maxiter
private

Definition at line 152 of file chainiksolvervel_pinv_nso.hpp.

◆ nj

unsigned int KDL::ChainIkSolverVel_pinv_nso::nj
private

Definition at line 143 of file chainiksolvervel_pinv_nso.hpp.

◆ opt_pos

JntArray KDL::ChainIkSolverVel_pinv_nso::opt_pos
private

Definition at line 156 of file chainiksolvervel_pinv_nso.hpp.

◆ S

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

Definition at line 146 of file chainiksolvervel_pinv_nso.hpp.

◆ Sinv

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

Definition at line 147 of file chainiksolvervel_pinv_nso.hpp.

◆ svdResult

int KDL::ChainIkSolverVel_pinv_nso::svdResult
private

Definition at line 153 of file chainiksolvervel_pinv_nso.hpp.

◆ tmp

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

Definition at line 149 of file chainiksolvervel_pinv_nso.hpp.

◆ tmp2

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

Definition at line 150 of file chainiksolvervel_pinv_nso.hpp.

◆ U

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

Definition at line 145 of file chainiksolvervel_pinv_nso.hpp.

◆ V

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

Definition at line 148 of file chainiksolvervel_pinv_nso.hpp.

◆ weights

JntArray KDL::ChainIkSolverVel_pinv_nso::weights
private

Definition at line 155 of file chainiksolvervel_pinv_nso.hpp.


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