VirtualRobot::PoseQualityExtendedManipulability Class Reference

Inheritance diagram for VirtualRobot::PoseQualityExtendedManipulability:

Data Structures
struct	extManipData

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	PoseQualityExtendedManipulability (VirtualRobot::RobotNodeSetPtr rns, PoseQualityManipulability::ManipulabilityIndexType i=PoseQualityManipulability::eMinMaxRatio)
	~PoseQualityExtendedManipulability ()
float	getPoseQuality () override
float	getPoseQuality (PoseQualityManipulability::ManipulabilityIndexType i, int considerFirstSV)
float	getPoseQuality (const Eigen::VectorXf &direction) override
float	getManipulability (const Eigen::VectorXf &direction, int considerFirstSV=-1) override
bool	getDetailedAnalysis (extManipData &storeData, int considerFirstSV=0)
bool	getDetailedAnalysis (extManipData &storeData, bool(&dims)[6], int considerFirstSV=0)
std::vector< std::vector< float > >	getPermutationVector ()
void	considerObstacles (bool enable, float alpha=40.0f, float beta=1.0f)
bool	consideringJointLimits () override
	Indicates if joint limits are considered. More...
PoseQualityMeasurementPtr	clone (RobotPtr newRobot) override
void	getSelfDistParameters (float &storeAlpha, float &storeBeta)
Public Member Functions inherited from VirtualRobot::PoseQualityManipulability
	PoseQualityManipulability (VirtualRobot::RobotNodeSetPtr rns, ManipulabilityIndexType i=eMinMaxRatio)
	~PoseQualityManipulability ()
float	getPoseQuality () override
virtual float	getManipulability (ManipulabilityIndexType i)
float	getPoseQuality (const Eigen::VectorXf &direction) override
virtual float	getManipulability (ManipulabilityIndexType i, int considerFirstSV)
Eigen::VectorXf	getSingularValues ()
Eigen::MatrixXf	getSingularVectorCartesian ()
virtual void	penalizeJointLimits (bool enable, float k=50.0f)
bool	consideringJointLimits () override
	Indicates if joint limits are considered. More...
PoseQualityMeasurementPtr	clone (RobotPtr newRobot) override
Public Member Functions inherited from VirtualRobot::PoseQualityMeasurement
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	PoseQualityMeasurement (VirtualRobot::RobotNodeSetPtr rns)
	~PoseQualityMeasurement ()
void	setVerbose (bool v)
VirtualRobot::RobotNodeSetPtr	getRNS ()
std::string	getName ()
	A string that identifies the type of pose quality measure. More...
virtual void	setObstacleDistanceVector (const Eigen::Vector3f &directionSurfaceToObstance)
virtual void	disableObstacleDistance ()

Static Public Member Functions
static std::string	getTypeName ()
Static Public Member Functions inherited from VirtualRobot::PoseQualityManipulability
static std::string	getTypeName ()

Protected Member Functions
bool	getDetailedAnalysis (DifferentialIKPtr jacobian, RobotNodeSetPtr rns, extManipData &storeData, int considerFirstSV=0)
bool	getDetailedAnalysis (DifferentialIKPtr jacobian, RobotNodeSetPtr rns, extManipData &storeData, bool(&dims)[6], int considerFirstSV=0)
bool	getDetailedAnalysis (const Eigen::MatrixXf &jac, const std::vector< RobotNodePtr > &joints, extManipData &storeData, bool(&dims)[6], int considerFirstSV=0)
float	getPoseQuality (DifferentialIKPtr jacobian, RobotNodeSetPtr rns, PoseQualityManipulability::ManipulabilityIndexType i, int considerFirstSV)
bool	createCartDimPermutations (std::vector< std::vector< float > > &storePerm)
void	getQualityWeighting (float jv, float limitMin, float limitMax, float &storeWeightMin, float &storeWeightMax)
void	getPenalizations (const std::vector< RobotNodePtr > &joints, Eigen::VectorXf &penLo, Eigen::VectorXf &penHi)
	Compute weightings w and store 1/sqrt(w) More...
Eigen::MatrixXf	getJacobianWeighted (const Eigen::MatrixXf &jac, const std::vector< float > &directionVect, const Eigen::VectorXf &penLo, const Eigen::VectorXf &penHi)
bool	analyzeJacobian (const Eigen::MatrixXf &jac, Eigen::VectorXf &sv, Eigen::MatrixXf &singVectors, Eigen::MatrixXf &U, Eigen::MatrixXf &V, bool printInfo=false)
void	getObstaclePenalizations (RobotNodeSetPtr rns, const Eigen::Vector3f &obstVect, const Eigen::MatrixXf &jac, Eigen::MatrixXf &penObstLo, Eigen::MatrixXf &penObstHi)
void	getObstaclePenalizations (const std::vector< RobotNodePtr > &joints, const Eigen::Vector3f &obstVect, const Eigen::MatrixXf &jac, Eigen::MatrixXf &penObstLo, Eigen::MatrixXf &penObstHi)
Eigen::MatrixXf	getJacobianWeightedObstacles (const Eigen::MatrixXf &jac, const std::vector< float > &directionVect, const Eigen::VectorXf &penLo, const Eigen::VectorXf &penHi, const Eigen::MatrixXf &penObstLo, const Eigen::MatrixXf &penObstHi)
Protected Member Functions inherited from VirtualRobot::PoseQualityManipulability
float	getJointLimitPenalizationFactor ()

Protected Attributes
std::vector< std::vector< float > >	cartDimPermutations
float	obstacle_alpha
float	obstacle_beta
extManipData	currentManipData
Eigen::MatrixXf	tmpJac
std::vector< RobotNodePtr >	joints
Protected Attributes inherited from VirtualRobot::PoseQualityManipulability
VirtualRobot::DifferentialIKPtr	jacobian
ManipulabilityIndexType	manipulabilityType
bool	penJointLimits
float	penJointLimits_k
float	penalizeRotationFactor
bool	convertMMtoM
	if set, the Jacobian is computed in [m], while assuming the kinematic definitions to be in [mm] More...
Protected Attributes inherited from VirtualRobot::PoseQualityMeasurement
std::string	name
VirtualRobot::RobotNodeSetPtr	rns
bool	considerObstacle
Eigen::Vector3f	obstacleDir
bool	verbose

Additional Inherited Members
Public Types inherited from VirtualRobot::PoseQualityManipulability
enum	ManipulabilityIndexType { eMultiplySV, eMinMaxRatio }

Detailed Description

This pose quality measure is an extended approach based on Yoshikawa's manipulability measure. It considers joint limits and hence a detailed analysis of the constrained manipulability of a kinematic chain can be performed. Additionally self-distance and distances to obstacles can be considered. For details see: {Vahrenkamp2012, author = {Nikolaus Vahrenkamp and Tamim Asfour and Giorgio Metta and Giulio Sandini and R"udiger Dillmann}, title = {Manipulability Analysis}, booktitle = {Proceedings of IEEE-RAS International Conference on Humanoid Robots (Humanoids)}, year = {2012} }

For performance reasons this class is not thread safe and cannot be used from different threads in parallel.

Constructor & Destructor Documentation

PoseQualityExtendedManipulability()

VirtualRobot::PoseQualityExtendedManipulability::PoseQualityExtendedManipulability	(	VirtualRobot::RobotNodeSetPtr	rns,
		PoseQualityManipulability::ManipulabilityIndexType	i = PoseQualityManipulability::eMinMaxRatio
	)

Constructor

Parameters

rns	The kinematic chain for which the quality should be computed.
i	The method that should be used to compute the quality.

~PoseQualityExtendedManipulability()

VirtualRobot::PoseQualityExtendedManipulability::~PoseQualityExtendedManipulability ( )

default

Member Function Documentation

analyzeJacobian()

bool VirtualRobot::PoseQualityExtendedManipulability::analyzeJacobian ( const Eigen::MatrixXf &  jac, Eigen::VectorXf &  sv, Eigen::MatrixXf &  singVectors, Eigen::MatrixXf &  U, Eigen::MatrixXf &  V, bool  printInfo = false  )

protected

Perform SVD on jac.

Parameters

sv	Singular vectors are stored in sv
singVectors	The scaled sing vectors are stored here (first one: length 1, the following are scaled according to their corresponding sv ratio)
U	The U matrix is stored here
V	The V matrix is stored here

clone()

PoseQualityMeasurementPtr VirtualRobot::PoseQualityExtendedManipulability::clone ( RobotPtr  newRobot )

overridevirtual

Reimplemented from VirtualRobot::PoseQualityMeasurement.

consideringJointLimits()

bool VirtualRobot::PoseQualityExtendedManipulability::consideringJointLimits ( )

overridevirtual

Indicates if joint limits are considered.

Reimplemented from VirtualRobot::PoseQualityMeasurement.

considerObstacles()

void VirtualRobot::PoseQualityExtendedManipulability::considerObstacles	(	bool	enable,
		float	alpha = 40.0f,
		float	beta = 1.0f
	)

If enabled (standard = disabled), you must take care of setting the obstacle vector before computing the manipulability. (see PoseQualityMeasurement::setObstacleDistanceVector)

createCartDimPermutations()

bool VirtualRobot::PoseQualityExtendedManipulability::createCartDimPermutations ( std::vector< std::vector< float > > &  storePerm )

protected

getDetailedAnalysis() [1/5]

bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis	(	extManipData &	storeData,
		int	considerFirstSV = 0
	)

getDetailedAnalysis() [2/5]

bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis	(	extManipData &	storeData,
		bool(&)	dims[6],
		int	considerFirstSV = 0
	)

getDetailedAnalysis() [3/5]

bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( DifferentialIKPtr  jacobian, RobotNodeSetPtr  rns, extManipData &  storeData, int  considerFirstSV = 0  )

protected

getDetailedAnalysis() [4/5]

bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( DifferentialIKPtr  jacobian, RobotNodeSetPtr  rns, extManipData &  storeData, bool(&)  dims[6], int  considerFirstSV = 0  )

protected

getDetailedAnalysis() [5/5]

bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( const Eigen::MatrixXf &  jac, const std::vector< RobotNodePtr > &  joints, extManipData &  storeData, bool(&)  dims[6], int  considerFirstSV = 0  )

protected

getJacobianWeighted()

Eigen::MatrixXf VirtualRobot::PoseQualityExtendedManipulability::getJacobianWeighted ( const Eigen::MatrixXf &  jac, const std::vector< float > &  directionVect, const Eigen::VectorXf &  penLo, const Eigen::VectorXf &  penHi  )

protected

getJacobianWeightedObstacles()

Eigen::MatrixXf VirtualRobot::PoseQualityExtendedManipulability::getJacobianWeightedObstacles ( const Eigen::MatrixXf &  jac, const std::vector< float > &  directionVect, const Eigen::VectorXf &  penLo, const Eigen::VectorXf &  penHi, const Eigen::MatrixXf &  penObstLo, const Eigen::MatrixXf &  penObstHi  )

protected

getManipulability()

float VirtualRobot::PoseQualityExtendedManipulability::getManipulability ( const Eigen::VectorXf &  direction, int  considerFirstSV = -1  )

overridevirtual

Reimplemented from VirtualRobot::PoseQualityManipulability.

getObstaclePenalizations() [1/2]

void VirtualRobot::PoseQualityExtendedManipulability::getObstaclePenalizations ( RobotNodeSetPtr  rns, const Eigen::Vector3f &  obstVect, const Eigen::MatrixXf &  jac, Eigen::MatrixXf &  penObstLo, Eigen::MatrixXf &  penObstHi  )

protected

getObstaclePenalizations() [2/2]

void VirtualRobot::PoseQualityExtendedManipulability::getObstaclePenalizations ( const std::vector< RobotNodePtr > &  joints, const Eigen::Vector3f &  obstVect, const Eigen::MatrixXf &  jac, Eigen::MatrixXf &  penObstLo, Eigen::MatrixXf &  penObstHi  )

protected

getPenalizations()

void VirtualRobot::PoseQualityExtendedManipulability::getPenalizations ( const std::vector< RobotNodePtr > &  joints, Eigen::VectorXf &  penLo, Eigen::VectorXf &  penHi  )

protected

Compute weightings w and store 1/sqrt(w)

getPermutationVector()

std::vector< std::vector< float > > VirtualRobot::PoseQualityExtendedManipulability::getPermutationVector

(

)

getPoseQuality() [1/4]

float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( )

overridevirtual

The main method for determining the pose quality. The current configuration of the corresponding RNS is analyzed and the quality is returned. See derived classes for details.

Reimplemented from VirtualRobot::PoseQualityMeasurement.

getPoseQuality() [2/4]

float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality	(	PoseQualityManipulability::ManipulabilityIndexType	i,
		int	considerFirstSV
	)

getPoseQuality() [3/4]

float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( const Eigen::VectorXf &  direction )

overridevirtual

The quality is determined for a given Cartesian direction. The current configuration of the corresponding RNS is analyzed and the quality is returned.

Parameters

direction

A 3d or 6d vector with the Cartesian direction to investigate.

Reimplemented from VirtualRobot::PoseQualityMeasurement.

getPoseQuality() [4/4]

float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( DifferentialIKPtr  jacobian, RobotNodeSetPtr  rns, PoseQualityManipulability::ManipulabilityIndexType  i, int  considerFirstSV  )

protected

getQualityWeighting()

void VirtualRobot::PoseQualityExtendedManipulability::getQualityWeighting ( float  jv, float  limitMin, float  limitMax, float &  storeWeightMin, float &  storeWeightMax  )

protected

Get the weighting for jv with limits. The weighting is computed as follows: w = ( (limitMax - limitMin)^2 (2jv - limitMax - limitMin) ) / ( 4(limitMax - jv)^2 (jv - limitMin)^2 ) In case jv is on the lower half of the joint range -> storeWeightMin = 1+w; storeWeightMax = 1 In case jv is on the upper half of the joint range -> storeWeightMin = 1; storeWeightMax = 1+w

getSelfDistParameters()

void VirtualRobot::PoseQualityExtendedManipulability::getSelfDistParameters	(	float &	storeAlpha,
		float &	storeBeta
	)

getTypeName()

std::string VirtualRobot::PoseQualityExtendedManipulability::getTypeName ( )

static

Field Documentation

cartDimPermutations

std::vector< std::vector<float> > VirtualRobot::PoseQualityExtendedManipulability::cartDimPermutations

protected

currentManipData

extManipData VirtualRobot::PoseQualityExtendedManipulability::currentManipData

protected

joints

std::vector<RobotNodePtr> VirtualRobot::PoseQualityExtendedManipulability::joints

protected

obstacle_alpha

float VirtualRobot::PoseQualityExtendedManipulability::obstacle_alpha

protected

obstacle_beta

float VirtualRobot::PoseQualityExtendedManipulability::obstacle_beta

protected

tmpJac

Eigen::MatrixXf VirtualRobot::PoseQualityExtendedManipulability::tmpJac

protected