#include <chainiksolvervel_pinv_nso.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv_nso:

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 JntArray &	getOptPos () const

int	getSVDResult () const

const JntArray &	getWeights () 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 Chain &	chain

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

chain	the chain to calculate the inverse velocity kinematics for
opt_pos	the desired positions of the chain used by to resolve the redundancy
weights	the weights applied in the joint space
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
alpha	the 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_in	input joint positions
v_in	input cartesian velocity
qdot_out	output 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

alpha NUllspace 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_pos optimal 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

weights the 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.