Simox  2.3.49
VirtualRobot::PoseQualityExtendedManipulability Class Reference
Inheritance diagram for VirtualRobot::PoseQualityExtendedManipulability:
VirtualRobot::PoseQualityManipulability VirtualRobot::PoseQualityMeasurement

Data Structures

struct  extManipData
 

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW PoseQualityExtendedManipulability (VirtualRobot::RobotNodeSetPtr rns, PoseQualityManipulability::ManipulabilityIndexType i=PoseQualityManipulability::eMinMaxRatio)
 
 ~PoseQualityExtendedManipulability ()
 
virtual float getPoseQuality ()
 
float getPoseQuality (PoseQualityManipulability::ManipulabilityIndexType i, int considerFirstSV)
 
virtual float getPoseQuality (const Eigen::VectorXf &direction)
 
virtual float getManipulability (const Eigen::VectorXf &direction, int considerFirstSV=-1)
 
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)
 
virtual bool consideringJointLimits ()
 Indicates if joint limits are considered. More...
 
- Public Member Functions inherited from VirtualRobot::PoseQualityManipulability
 PoseQualityManipulability (VirtualRobot::RobotNodeSetPtr rns, ManipulabilityIndexType i=eMinMaxRatio)
 
 ~PoseQualityManipulability ()
 
virtual float getManipulability (ManipulabilityIndexType i)
 
virtual float getManipulability (ManipulabilityIndexType i, int considerFirstSV)
 
Eigen::VectorXf getSingularValues ()
 
Eigen::MatrixXf getSingularVectorCartesian ()
 
virtual void penalizeJointLimits (bool enable, float k=50.0f)
 
- 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< RobotNodePtrjoints
 
- 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

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

Constructor

Parameters
rnsThe kinematic chain for which the quality should be computed.
iThe method that should be used to compute the quality.
VirtualRobot::PoseQualityExtendedManipulability::~PoseQualityExtendedManipulability ( )

Member Function Documentation

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
svSingular vectors are stored in sv
singVectorsThe scaled sing vectors are stored here (first one: length 1, the following are scaled according to their corresponding sv ratio)
UThe U matrix is stored here
VThe V matrix is stored here
bool VirtualRobot::PoseQualityExtendedManipulability::consideringJointLimits ( )
virtual

Indicates if joint limits are considered.

Reimplemented from VirtualRobot::PoseQualityManipulability.

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)

bool VirtualRobot::PoseQualityExtendedManipulability::createCartDimPermutations ( std::vector< std::vector< float > > &  storePerm)
protected
bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( extManipData storeData,
int  considerFirstSV = 0 
)
bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( extManipData storeData,
bool(&)  dims[6],
int  considerFirstSV = 0 
)
bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( DifferentialIKPtr  jacobian,
RobotNodeSetPtr  rns,
extManipData storeData,
int  considerFirstSV = 0 
)
protected
bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( DifferentialIKPtr  jacobian,
RobotNodeSetPtr  rns,
extManipData storeData,
bool(&)  dims[6],
int  considerFirstSV = 0 
)
protected
bool VirtualRobot::PoseQualityExtendedManipulability::getDetailedAnalysis ( const Eigen::MatrixXf &  jac,
const std::vector< RobotNodePtr > &  joints,
extManipData storeData,
bool(&)  dims[6],
int  considerFirstSV = 0 
)
protected
Eigen::MatrixXf VirtualRobot::PoseQualityExtendedManipulability::getJacobianWeighted ( const Eigen::MatrixXf &  jac,
const std::vector< float > &  directionVect,
const Eigen::VectorXf &  penLo,
const Eigen::VectorXf &  penHi 
)
protected
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
float VirtualRobot::PoseQualityExtendedManipulability::getManipulability ( const Eigen::VectorXf &  direction,
int  considerFirstSV = -1 
)
virtual
void VirtualRobot::PoseQualityExtendedManipulability::getObstaclePenalizations ( RobotNodeSetPtr  rns,
const Eigen::Vector3f &  obstVect,
const Eigen::MatrixXf &  jac,
Eigen::MatrixXf &  penObstLo,
Eigen::MatrixXf &  penObstHi 
)
protected
void VirtualRobot::PoseQualityExtendedManipulability::getObstaclePenalizations ( const std::vector< RobotNodePtr > &  joints,
const Eigen::Vector3f &  obstVect,
const Eigen::MatrixXf &  jac,
Eigen::MatrixXf &  penObstLo,
Eigen::MatrixXf &  penObstHi 
)
protected
void VirtualRobot::PoseQualityExtendedManipulability::getPenalizations ( const std::vector< RobotNodePtr > &  joints,
Eigen::VectorXf &  penLo,
Eigen::VectorXf &  penHi 
)
protected

Compute weightings w and store 1/sqrt(w)

std::vector< std::vector< float > > VirtualRobot::PoseQualityExtendedManipulability::getPermutationVector ( )
float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( )
virtual

Returns the manipulability of the current configuration.

Reimplemented from VirtualRobot::PoseQualityManipulability.

float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( PoseQualityManipulability::ManipulabilityIndexType  i,
int  considerFirstSV 
)
float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( const Eigen::VectorXf &  direction)
virtual

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

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

Reimplemented from VirtualRobot::PoseQualityManipulability.

float VirtualRobot::PoseQualityExtendedManipulability::getPoseQuality ( DifferentialIKPtr  jacobian,
RobotNodeSetPtr  rns,
PoseQualityManipulability::ManipulabilityIndexType  i,
int  considerFirstSV 
)
protected
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

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

Field Documentation

std::vector< std::vector<float> > VirtualRobot::PoseQualityExtendedManipulability::cartDimPermutations
protected
extManipData VirtualRobot::PoseQualityExtendedManipulability::currentManipData
protected
std::vector<RobotNodePtr> VirtualRobot::PoseQualityExtendedManipulability::joints
protected
float VirtualRobot::PoseQualityExtendedManipulability::obstacle_alpha
protected
float VirtualRobot::PoseQualityExtendedManipulability::obstacle_beta
protected
Eigen::MatrixXf VirtualRobot::PoseQualityExtendedManipulability::tmpJac
protected