Simox  2.3.74.0
VirtualRobot::Manipulability Class Reference
Inheritance diagram for VirtualRobot::Manipulability:
VirtualRobot::WorkspaceRepresentation

Data Structures

struct  ManipulabiliyGrasp
 

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW Manipulability (RobotPtr robot)
 
std::vector< ManipulabiliyGraspanalyseGrasps (GraspSetPtr grasps, ManipulationObjectPtr object)
 
ManipulabiliyGrasp analyseGrasp (GraspPtr grasp, ManipulationObjectPtr object)
 
void setManipulabilityMeasure (PoseQualityMeasurementPtr m)
 
PoseQualityMeasurementPtr getManipulabilityMeasure ()
 
float measureCurrentPose ()
 measureCurrentPose Uses internal quality measure to determine the quality value of the current tcp pose More...
 
std::string getMeasureName () const
 
bool consideringJointLimits () const
 
void setMaxManipulability (float maximalManip)
 
float getMaxManipulability ()
 
float getManipulabilityAtPose (const Eigen::Matrix4f &globalPose)
 
void initSelfDistanceCheck (RobotNodeSetPtr staticModel, RobotNodeSetPtr dynamicModel)
 
virtual bool checkForParameters (RobotNodeSetPtr nodeSet, float steps, float storeMinBounds[6], float storeMaxBounds[6], float &storeMaxManipulability, RobotNodePtr baseNode=RobotNodePtr(), RobotNodePtr tcpNode=RobotNodePtr())
 
bool smooth (unsigned int minNeighbors=1)
 
GraspSetPtr getReachableGrasps (GraspSetPtr grasps, ManipulationObjectPtr object)
 
void getSelfDistConfig (bool &storeConsiderSelfDist, RobotNodeSetPtr &storeStatic, RobotNodeSetPtr &storeDynamic)
 
WorkspaceRepresentationPtr clone () override
 
void addRandomTCPPoses (unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions=true) override
 
- Public Member Functions inherited from VirtualRobot::WorkspaceRepresentation
 WorkspaceRepresentation (RobotPtr robot)
 
virtual void reset ()
 
virtual void load (const std::string &filename)
 
virtual void save (const std::string &filename)
 
unsigned char getEntry (const Eigen::Matrix4f &globalPose) const
 
int getMaxEntry () const
 Returns the maximum entry of a voxel. More...
 
float getVoxelSize (int dim) const
 returns the extends of a voxel at corresponding dimension. More...
 
RobotNodePtr getBaseNode ()
 The base node of this workspace data. More...
 
RobotNodePtr getTCP ()
 The corresponding TCP. More...
 
RobotNodeSetPtr getNodeSet ()
 The kinematic chain that is covered by this workspace data. More...
 
void invalidateBehindRobot (bool inverted=false)
 
virtual void initialize (RobotNodeSetPtr nodeSet, float discretizeStepTranslation, float discretizeStepRotation, const float minBounds[6], const float maxBounds[6], SceneObjectSetPtr staticCollisionModel=SceneObjectSetPtr(), SceneObjectSetPtr dynamicCollisionModel=SceneObjectSetPtr(), RobotNodePtr baseNode=RobotNodePtr(), RobotNodePtr tcpNode=RobotNodePtr(), bool adjustOnOverflow=true)
 
virtual void setCurrentTCPPoseEntryIfLower (unsigned char e)
 
virtual void setCurrentTCPPoseEntry (unsigned char e)
 
virtual void clear ()
 
virtual bool setRobotNodesToRandomConfig (VirtualRobot::RobotNodeSetPtr nodeSet=VirtualRobot::RobotNodeSetPtr(), bool checkForSelfCollisions=true)
 
virtual void binarize ()
 
virtual int fillHoles (unsigned int minNeighbors=1)
 
virtual void print ()
 
Eigen::Matrix4f sampleCoveredPose ()
 returns a random pose that is covered by the workspace data More...
 
bool isCovered (const Eigen::Matrix4f &globalPose)
 
bool isCovered (unsigned int v[6])
 
virtual int getNumVoxels (int dim) const
 
virtual float getMinBound (int dim) const
 
virtual float getMaxBound (int dim) const
 
virtual unsigned char getVoxelEntry (unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f) const
 
virtual int sumAngleReachabilities (int x0, int x1, int x2) const
 
virtual int avgAngleReachabilities (int x0, int x1, int x2) const
 
virtual int getMaxEntry (int x0, int x1, int x2) const
 
virtual int getMaxEntry (const Eigen::Vector3f &position_global) const
 
virtual bool getVoxelFromPose (const Eigen::Matrix4f &globalPose, unsigned int v[6]) const
 
virtual bool getVoxelFromPosition (const Eigen::Matrix4f &globalPose, unsigned int v[3]) const
 
Eigen::Matrix4f getPoseFromVoxel (unsigned int v[6], bool transformToGlobalPose=true)
 
Eigen::Matrix4f getPoseFromVoxel (float v[6], bool transformToGlobalPose=true)
 
virtual int getMaxSummedAngleReachablity ()
 
virtual bool hasEntry (unsigned int x, unsigned int y, unsigned int z)
 
virtual bool checkForParameters (RobotNodeSetPtr nodeSet, float steps, float storeMinBounds[6], float storeMaxBounds[6], RobotNodePtr baseNode=RobotNodePtr(), RobotNodePtr tcpNode=RobotNodePtr())
 
WorkspaceCut2DPtr createCut (const Eigen::Matrix4f &referencePose, float cellSize, bool sumAngles) const
 
WorkspaceCut2DPtr createCut (float heightPercent, float cellSize, bool sumAngles) const
 createCut Create a cut at a specific height (assuming z is upwards). More...
 
std::vector< WorkspaceCut2DTransformationPtrcreateCutTransformations (WorkspaceCut2DPtr cutXY, RobotNodePtr referenceNode=RobotNodePtr(), float maxAngle=M_PIf32)
 
bool getWorkspaceExtends (Eigen::Vector3f &storeMinBBox, Eigen::Vector3f &storeMaxBBox) const
 
MathTools::OOBB getOOBB (bool achievedValues=false) const
 
float getDiscretizeParameterTranslation ()
 
float getDiscretizeParameterRotation ()
 
RobotPtr getRobot ()
 
SceneObjectSetPtr getCollisionModelStatic ()
 
SceneObjectSetPtr getCollisionModelDynamic ()
 
RobotNodePtr getTcp ()
 
bool getAdjustOnOverflow ()
 
virtual void addCurrentTCPPose ()
 
void addRandomTCPPoses (unsigned int loops, bool checkForSelfCollisions=true)
 
void setVoxelEntry (unsigned int v[6], unsigned char e)
 
void setEntry (const Eigen::Matrix4f &poseGlobal, unsigned char e)
 
void setEntryCheckNeighbors (const Eigen::Matrix4f &poseGlobal, unsigned char e, unsigned int neighborVoxels)
 
virtual void toLocal (Eigen::Matrix4f &p) const
 
virtual void toGlobal (Eigen::Matrix4f &p) const
 
void toLocalVec (Eigen::Vector3f &positionGlobal) const
 
void toGlobalVec (Eigen::Vector3f &positionLocal) const
 
void matrix2Vector (const Eigen::Matrix4f &m, float x[6]) const
 Convert a 4x4 matrix to a pos + ori vector. More...
 
void vector2Matrix (const float x[6], Eigen::Matrix4f &m) const
 
void vector2Matrix (const Eigen::Vector3f &pos, const Eigen::Vector3f &rot, Eigen::Matrix4f &m) const
 
virtual bool getVoxelFromPose (float x[6], unsigned int v[6]) const
 
virtual bool getVoxelFromPosition (float x[3], unsigned int v[3]) const
 
void setOrientationType (eOrientationType t)
 
WorkspaceDataPtr getData ()
 
bool getPoseFromVoxel (unsigned int x[], float v[]) const
 
virtual VolumeInfo computeVolumeInformation ()
 

Protected Member Functions

bool customLoad (std::ifstream &file) override
 
bool customSave (std::ofstream &file) override
 
void customPrint () override
 
void customInitialize () override
 
bool customStringRead (std::ifstream &file, std::string &res)
 
float getCurrentManipulability (PoseQualityMeasurementPtr qualMeasure, RobotNodeSetPtr selfDistSt=RobotNodeSetPtr(), RobotNodeSetPtr selfDistDyn=RobotNodeSetPtr())
 
void addPose (const Eigen::Matrix4f &p) override
 
void addPose (const Eigen::Matrix4f &p, PoseQualityMeasurementPtr qualMeasure) override
 addPose Adds pose and rate it with the given quality measure. Ignored in this implementation, but used in derived classes ( More...
 
void addPose (const Eigen::Matrix4f &p, PoseQualityMeasurementPtr qualMeasure, RobotNodeSetPtr selfDistSt, RobotNodeSetPtr selfDistDyn)
 
- Protected Member Functions inherited from VirtualRobot::WorkspaceRepresentation
void uncompressData (const unsigned char *source, int size, unsigned char *dest)
 Uncompress the data. More...
 
unsigned char * compressData (const unsigned char *source, int size, int &compressedSize)
 Compress the data. More...
 
virtual Eigen::Matrix4f getToLocalTransformation () const
 
virtual Eigen::Matrix4f getToGlobalTransformation () const
 

Protected Attributes

PoseQualityMeasurementPtr measure
 
float maxManip
 
std::string measureName
 
bool considerJL
 
bool considerSelfDist
 
RobotNodeSetPtr selfDistStatic
 
RobotNodeSetPtr selfDistDynamic
 
float selfDistAlpha
 
float selfDistBeta
 
- Protected Attributes inherited from VirtualRobot::WorkspaceRepresentation
RobotPtr robot
 
RobotNodePtr baseNode
 
RobotNodePtr tcpNode
 
RobotNodeSetPtr nodeSet
 
SceneObjectSetPtr staticCollisionModel
 
SceneObjectSetPtr dynamicCollisionModel
 
int buildUpLoops
 
int collisionConfigs
 
float discretizeStepTranslation
 
float discretizeStepRotation
 
float minBounds [6]
 
float maxBounds [6]
 
int numVoxels [6]
 
float achievedMinValues [6]
 
float achievedMaxValues [6]
 
float spaceSize [6]
 
WorkspaceDataPtr data
 
bool adjustOnOverflow
 
std::string type
 
int versionMajor
 
int versionMinor
 
eOrientationType orientationType
 Specifies how the rotation part (x[3],x[4],x[5]) of an 6D voxel entry is encoded. More...
 

Additional Inherited Members

- Public Types inherited from VirtualRobot::WorkspaceRepresentation
enum  eOrientationType { RPY, EulerXYZ, EulerXYZExtrinsic, Hopf }
 
typedef int32_t ioIntTypeRead
 
typedef std::shared_ptr< WorkspaceCut2DWorkspaceCut2DPtr
 
typedef std::shared_ptr< WorkspaceCut2DTransformationWorkspaceCut2DTransformationPtr
 
- Data Fields inherited from VirtualRobot::WorkspaceRepresentation
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef int32_t ioIntTypeWrite
 

Detailed Description

This class represents an approximation of the distribution of a kinematic chain's manipulability. Therefore, the Cartesian space (6D) is voxelized. Each voxel holds a quality value, approximatively representing the maximum manipulability that can be achieved at the given pose. The discretized manipulability data can be written to and loaded from binary files.

The manipulability is linked to a base coordinate system which is defined by a robot joint. This base system is used to align the data when the robot is moving. I.e. think of an arm of a humanoid where the manipulability data is linked to the shoulder. When the torso moves, the manipulability also changes it's position according to the position of the shoulder.

For buildup different manipulability measures can be incorporated (e.g. Yoshikawa's manipulability measure or extensions to this approach).

See also
PoseQualityMeasurement
PoseQualityManipulability
PoseQualityExtendedManipulability

Constructor & Destructor Documentation

◆ Manipulability()

VirtualRobot::Manipulability::Manipulability ( RobotPtr  robot)

Member Function Documentation

◆ addPose() [1/3]

void VirtualRobot::Manipulability::addPose ( const Eigen::Matrix4f &  p)
overrideprotectedvirtual

◆ addPose() [2/3]

void VirtualRobot::Manipulability::addPose ( const Eigen::Matrix4f &  p,
PoseQualityMeasurementPtr  qualMeasure 
)
overrideprotectedvirtual

addPose Adds pose and rate it with the given quality measure. Ignored in this implementation, but used in derived classes (

See also
Manipulability)
Parameters
p
qualMeasure

Reimplemented from VirtualRobot::WorkspaceRepresentation.

◆ addPose() [3/3]

void VirtualRobot::Manipulability::addPose ( const Eigen::Matrix4f &  p,
PoseQualityMeasurementPtr  qualMeasure,
RobotNodeSetPtr  selfDistSt,
RobotNodeSetPtr  selfDistDyn 
)
protected

◆ addRandomTCPPoses()

void VirtualRobot::Manipulability::addRandomTCPPoses ( unsigned int  loops,
unsigned int  numThreads,
bool  checkForSelfCollisions = true 
)
overridevirtual

Appends a number of random TCP poses to workspace Data (multithreaded). This method is blocking, i.e. it returns as soon as all threads are done.

Parameters
loopsNumber of poses that should be appended
numThreadsnumber of worker threads used behind the scenes to append random TCP poses to workspace data.
checkForSelfCollisionsBuild a collision-free configuration. If true, random configs are generated until one is collision-free.

Reimplemented from VirtualRobot::WorkspaceRepresentation.

◆ analyseGrasp()

Manipulability::ManipulabiliyGrasp VirtualRobot::Manipulability::analyseGrasp ( GraspPtr  grasp,
ManipulationObjectPtr  object 
)

◆ analyseGrasps()

std::vector< Manipulability::ManipulabiliyGrasp > VirtualRobot::Manipulability::analyseGrasps ( GraspSetPtr  grasps,
ManipulationObjectPtr  object 
)

Performs a manipulability analysis of grasps at the current position of the object.

Parameters
graspsThe grasps that should be analyzed.
objectThe grasps are supposed to to be applied to the object at its current global pose.
Returns
A vector of all grasps with manipulability>0 in sorted order (starting with the grasp with highest manipulability)

◆ checkForParameters()

bool VirtualRobot::Manipulability::checkForParameters ( RobotNodeSetPtr  nodeSet,
float  steps,
float  storeMinBounds[6],
float  storeMaxBounds[6],
float &  storeMaxManipulability,
RobotNodePtr  baseNode = RobotNodePtr(),
RobotNodePtr  tcpNode = RobotNodePtr() 
)
virtual

Do a test run in order to get an idea of the covered workspace extends and the maximum manipulability. The resulting values can be used to initialize this object. Values are slightly scaled.

Parameters
nodeSetThese nodes should be considered.
stepsHow many steps should be performed (use a large value e.g. 1000)
storeMinBoundsThe achieved minimum values are stored here.
storeMaxBoundsThe achieved maximum values are stored here.
storeMaxManipulabilityThe maximal achieved manipulability is stored here.
baseNodeThe base node.
tcpNodeThe tcp.
See also
WorkspaceRepresentation::initialize()

◆ clone()

VirtualRobot::WorkspaceRepresentationPtr VirtualRobot::Manipulability::clone ( )
overridevirtual

Creates a deep copy of this data structure. A ManipulabilityPtr is returned.

Reimplemented from VirtualRobot::WorkspaceRepresentation.

◆ consideringJointLimits()

bool VirtualRobot::Manipulability::consideringJointLimits ( ) const

Returns true, if manipulability measure considers joint limits.

◆ customInitialize()

void VirtualRobot::Manipulability::customInitialize ( )
overrideprotectedvirtual

◆ customLoad()

bool VirtualRobot::Manipulability::customLoad ( std::ifstream &  )
overrideprotectedvirtual

Derived classes may implement some custom data access.

Reimplemented from VirtualRobot::WorkspaceRepresentation.

◆ customPrint()

void VirtualRobot::Manipulability::customPrint ( )
overrideprotectedvirtual

◆ customSave()

bool VirtualRobot::Manipulability::customSave ( std::ofstream &  )
overrideprotectedvirtual

Derived classes may implement some custom data access.

Reimplemented from VirtualRobot::WorkspaceRepresentation.

◆ customStringRead()

bool VirtualRobot::Manipulability::customStringRead ( std::ifstream &  file,
std::string &  res 
)
protected

◆ getCurrentManipulability()

float VirtualRobot::Manipulability::getCurrentManipulability ( PoseQualityMeasurementPtr  qualMeasure,
RobotNodeSetPtr  selfDistSt = RobotNodeSetPtr(),
RobotNodeSetPtr  selfDistDyn = RobotNodeSetPtr() 
)
protected

◆ getManipulabilityAtPose()

float VirtualRobot::Manipulability::getManipulabilityAtPose ( const Eigen::Matrix4f &  globalPose)

Returns the maximal manipulability that can approximatively be achieved at globalPose. Therefore, the entry of the discretized manipulability data is used and projected to [0,maxManip]

◆ getManipulabilityMeasure()

PoseQualityMeasurementPtr VirtualRobot::Manipulability::getManipulabilityMeasure ( )

◆ getMaxManipulability()

float VirtualRobot::Manipulability::getMaxManipulability ( )

◆ getMeasureName()

std::string VirtualRobot::Manipulability::getMeasureName ( ) const

Returns the name of the manipulability measure.

◆ getReachableGrasps()

GraspSetPtr VirtualRobot::Manipulability::getReachableGrasps ( GraspSetPtr  grasps,
ManipulationObjectPtr  object 
)

Returns all reachable grasps that can be applied at the current position of object.

◆ getSelfDistConfig()

void VirtualRobot::Manipulability::getSelfDistConfig ( bool &  storeConsiderSelfDist,
RobotNodeSetPtr storeStatic,
RobotNodeSetPtr storeDynamic 
)

Access self distance configuration.

◆ initSelfDistanceCheck()

void VirtualRobot::Manipulability::initSelfDistanceCheck ( RobotNodeSetPtr  staticModel,
RobotNodeSetPtr  dynamicModel 
)

Initializes the consideration of self distances (off by default). If enabled, the distance vector between static and dynamic rns is computed for every random pose and the quality measure is penalized according to that vector.

Parameters
staticModelThe static (not moving) part of the robot (e.g. torso and head)
staticModelThe dynamic (moving) part of the robot. Recommending to use the end effector instead of the complete arm, since the upperarm usually has a low distance to the rest of the body.

◆ measureCurrentPose()

float VirtualRobot::Manipulability::measureCurrentPose ( )

measureCurrentPose Uses internal quality measure to determine the quality value of the current tcp pose

Returns

◆ setManipulabilityMeasure()

void VirtualRobot::Manipulability::setManipulabilityMeasure ( PoseQualityMeasurementPtr  m)

The manipulability measure can be defined here

◆ setMaxManipulability()

void VirtualRobot::Manipulability::setMaxManipulability ( float  maximalManip)

All entries are scaled (during construction) according to this value. Higher manipulability values will result in an entry of 255

◆ smooth()

bool VirtualRobot::Manipulability::smooth ( unsigned int  minNeighbors = 1)

smooth the data

Field Documentation

◆ considerJL

bool VirtualRobot::Manipulability::considerJL
protected

◆ considerSelfDist

bool VirtualRobot::Manipulability::considerSelfDist
protected

◆ maxManip

float VirtualRobot::Manipulability::maxManip
protected

◆ measure

PoseQualityMeasurementPtr VirtualRobot::Manipulability::measure
protected

◆ measureName

std::string VirtualRobot::Manipulability::measureName
protected

◆ selfDistAlpha

float VirtualRobot::Manipulability::selfDistAlpha
protected

◆ selfDistBeta

float VirtualRobot::Manipulability::selfDistBeta
protected

◆ selfDistDynamic

RobotNodeSetPtr VirtualRobot::Manipulability::selfDistDynamic
protected

◆ selfDistStatic

RobotNodeSetPtr VirtualRobot::Manipulability::selfDistStatic
protected