#include <chainiksolvervel_pinv.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv:

Public Member Functions
virtual int	CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
virtual int	CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
	ChainIkSolverVel_pinv (const Chain &chain, double eps=0.00001, int maxiter=150)
unsigned int	getNrZeroSigmas () const
int	getSVDResult () const
virtual const char *	strError (const int error) const
virtual void	updateInternalDataStructures ()
	~ChainIkSolverVel_pinv ()
Static Public Attributes
static const int	E_CONVERGE_PINV_SINGULAR = +100
	solution converged but (pseudo)inverse is singular
Private Attributes
const Chain &	chain
double	eps
Jacobian	jac
ChainJntToJacSolver	jnt2jac
int	maxiter
unsigned int	nj
unsigned int	nrZeroSigmas
JntArray	S
SVD_HH	svd
int	svdResult
JntArray	tmp
std::vector< JntArray >	U
std::vector< JntArray >	V

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.

Definition at line 40 of file chainiksolvervel_pinv.hpp.

Constructor & Destructor Documentation

KDL::ChainIkSolverVel_pinv::ChainIkSolverVel_pinv	(	const Chain &	chain,
		double	eps = `0.00001`,
		int	maxiter = `150`
	)		`[explicit]`

Constructor of the solver

Parameters:

chain	the chain to calculate the inverse velocity kinematics for
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

Definition at line 26 of file chainiksolvervel_pinv.cpp.

KDL::ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv ( )

Definition at line 57 of file chainiksolvervel_pinv.cpp.

Member Function Documentation

int KDL::ChainIkSolverVel_pinv::CartToJnt	(	const JntArray &	q_in,
		const Twist &	v_in,
		JntArray &	qdot_out
	)		`[virtual]`

Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian velocity v_in

Returns:: E_NOERROR=solution converged to <eps in maxiter E_SVD_FAILED=SVD computation failed E_CONVERGE_PINV_SINGULAR=solution converged but (pseudo)inverse is singular

Note:: if E_CONVERGE_PINV_SINGULAR returned then converged and can continue motion, but have degraded solution; If E_SVD_FAILED returned, then getSvdResult() returns the error code from the SVD algorithm.

Implements KDL::ChainIkSolverVel.

Definition at line 62 of file chainiksolvervel_pinv.cpp.

virtual int KDL::ChainIkSolverVel_pinv::CartToJnt	(	const JntArray &	q_init,
		const FrameVel &	v_in,
		JntArrayVel &	q_out
	)		`[inline, virtual]`

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

Definition at line 80 of file chainiksolvervel_pinv.hpp.

unsigned int KDL::ChainIkSolverVel_pinv::getNrZeroSigmas ( ) const [inline]

Retrieve the number of singular values of the jacobian that are < eps; if the number of near zero singular values is > jac.col()-jac.row(), then the jacobian pseudoinverse is singular

Definition at line 87 of file chainiksolvervel_pinv.hpp.

int KDL::ChainIkSolverVel_pinv::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 93 of file chainiksolvervel_pinv.hpp.

const char * KDL::ChainIkSolverVel_pinv::strError ( const int error ) const [virtual]

Return a description of the latest error

Returns:: if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented from KDL::SolverI.

Definition at line 132 of file chainiksolvervel_pinv.cpp.

void KDL::ChainIkSolverVel_pinv::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 43 of file chainiksolvervel_pinv.cpp.