Simox  2.3.50
VirtualRobot::WorkspaceGrid Class Reference

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW WorkspaceGrid (float minX, float maxX, float minY, float maxY, float discretizeSize)
 
 ~WorkspaceGrid ()
 
int getEntry (float x, float y)
 returns entry of position in x/y plane (in world coords) More...
 
bool getEntry (float x, float y, int &storeEntry, GraspPtr &storeGrasp)
 
bool getEntry (float x, float y, int &nStoreEntry, std::vector< GraspPtr > &storeGrasps)
 
int getCellEntry (int cellX, int cellY)
 returns entry of discretized square (x/y) More...
 
bool getCellEntry (int cellX, int cellY, int &nStoreEntry, GraspPtr &storeGrasp)
 
bool getCellEntry (int cellX, int cellY, int &nStoreEntry, std::vector< GraspPtr > &storeGrasps)
 
void setEntry (float x, float y, int value, GraspPtr grasp)
 
void setCellEntry (int cellX, int cellY, int value, GraspPtr pGrasp)
 
int getMaxEntry ()
 
void setEntryCheckNeighbors (float x, float y, int value, GraspPtr grasp)
 
bool getRandomPos (int minEntry, float &storeXGlobal, float &storeYGlobal, GraspPtr &storeGrasp, int maxLoops=50)
 tries to find a random position with a entry >= minEntry More...
 
bool getRandomPos (int minEntry, float &storeXGlobal, float &storeYGlobal, std::vector< GraspPtr > &storeGrasps, int maxLoops=50)
 
void reset ()
 
bool fillGridData (WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode)
 
void setGridPosition (float x, float y)
 
void getExtends (float &storeMinX, float &storeMaxX, float &storeMinY, float &storeMaxY)
 
void getCells (int &storeCellsX, int &storeCellsY)
 

Protected Member Functions

void setEntries (std::vector< WorkspaceRepresentation::WorkspaceCut2DTransformationPtr > &wsData, Eigen::Matrix4f &graspGlobal, GraspPtr grasp)
 
int getDataPos (int x, int y)
 

Protected Attributes

float minX
 
float maxX
 
float minY
 
float maxY
 
float discretizeSize
 
int gridSizeX
 
int gridSizeY
 
float gridExtendX
 
float gridExtendY
 
int * data
 
std::vector< GraspPtr > * graspLink
 

Detailed Description

A 2D grid which represents a quality distribution (e.g. the reachability) at 2D positions w.r.t. one or multiple grasp(s). Internally the inverse workspace data (

See also
WorkspaceRepresentation), which encodes the transformation between robot's base and grasping position, is used. This data is useful to quickly sample positions from where the probability that a grasp is reachable is high (see getRandomPos).

Constructor & Destructor Documentation

VirtualRobot::WorkspaceGrid::WorkspaceGrid ( float  minX,
float  maxX,
float  minY,
float  maxY,
float  discretizeSize 
)

Setup the 2D grid with given extends and discretization parameter.

VirtualRobot::WorkspaceGrid::~WorkspaceGrid ( )

Member Function Documentation

bool VirtualRobot::WorkspaceGrid::fillGridData ( WorkspaceRepresentationPtr  ws,
ManipulationObjectPtr  o,
GraspPtr  g,
RobotNodePtr  baseRobotNode 
)

Fill the grid with inverse reachability data generated from grasp g and object o.

int VirtualRobot::WorkspaceGrid::getCellEntry ( int  cellX,
int  cellY 
)

returns entry of discretized square (x/y)

bool VirtualRobot::WorkspaceGrid::getCellEntry ( int  cellX,
int  cellY,
int &  nStoreEntry,
GraspPtr storeGrasp 
)
bool VirtualRobot::WorkspaceGrid::getCellEntry ( int  cellX,
int  cellY,
int &  nStoreEntry,
std::vector< GraspPtr > &  storeGrasps 
)
void VirtualRobot::WorkspaceGrid::getCells ( int &  storeCellsX,
int &  storeCellsY 
)

Number of cells in x and y

int VirtualRobot::WorkspaceGrid::getDataPos ( int  x,
int  y 
)
inlineprotected
int VirtualRobot::WorkspaceGrid::getEntry ( float  x,
float  y 
)

returns entry of position in x/y plane (in world coords)

bool VirtualRobot::WorkspaceGrid::getEntry ( float  x,
float  y,
int &  storeEntry,
GraspPtr storeGrasp 
)

Gets the corresponding entry of a world / global 2d position.

Parameters
xThe x coordinate (in global coordinate system)
yThe y coordinate (in global coordinate system)
storeEntryThe corresponding entry is stored.
storeGraspA random grasp of corresponding cell is stored.
Returns
True if x/y is inside this grid and an entry>0 was found, false otherwise.
bool VirtualRobot::WorkspaceGrid::getEntry ( float  x,
float  y,
int &  nStoreEntry,
std::vector< GraspPtr > &  storeGrasps 
)
void VirtualRobot::WorkspaceGrid::getExtends ( float &  storeMinX,
float &  storeMaxX,
float &  storeMinY,
float &  storeMaxY 
)

Get extends in global coord system.

int VirtualRobot::WorkspaceGrid::getMaxEntry ( )
bool VirtualRobot::WorkspaceGrid::getRandomPos ( int  minEntry,
float &  storeXGlobal,
float &  storeYGlobal,
GraspPtr storeGrasp,
int  maxLoops = 50 
)

tries to find a random position with a entry >= minEntry

bool VirtualRobot::WorkspaceGrid::getRandomPos ( int  minEntry,
float &  storeXGlobal,
float &  storeYGlobal,
std::vector< GraspPtr > &  storeGrasps,
int  maxLoops = 50 
)
void VirtualRobot::WorkspaceGrid::reset ( )

Clear all entries.

void VirtualRobot::WorkspaceGrid::setCellEntry ( int  cellX,
int  cellY,
int  value,
GraspPtr  pGrasp 
)
void VirtualRobot::WorkspaceGrid::setEntries ( std::vector< WorkspaceRepresentation::WorkspaceCut2DTransformationPtr > &  wsData,
Eigen::Matrix4f &  graspGlobal,
GraspPtr  grasp 
)
protected

Adds data stored in reachability transformations. This data defines transformations from robot base system to grasping pose, so when defining a grasping pose, the inverse reachability can be represented by this grid Therefor the "world coordinates" of the inverse reachability distributions are computed by T_grasp * ReachTransformation^-1

void VirtualRobot::WorkspaceGrid::setEntry ( float  x,
float  y,
int  value,
GraspPtr  grasp 
)

sets the entry to value, if the current value is lower

void VirtualRobot::WorkspaceGrid::setEntryCheckNeighbors ( float  x,
float  y,
int  value,
GraspPtr  grasp 
)

This method sets the grid value to nValue and checks if the neighbors have a lower value and in case the value of the neighbors is set to nValue.

void VirtualRobot::WorkspaceGrid::setGridPosition ( float  x,
float  y 
)

Move the grid to (x,y), given in global coordinate system. Sets the new center.

Field Documentation

int* VirtualRobot::WorkspaceGrid::data
protected
float VirtualRobot::WorkspaceGrid::discretizeSize
protected
std::vector<GraspPtr>* VirtualRobot::WorkspaceGrid::graspLink
protected
float VirtualRobot::WorkspaceGrid::gridExtendX
protected
float VirtualRobot::WorkspaceGrid::gridExtendY
protected
int VirtualRobot::WorkspaceGrid::gridSizeX
protected
int VirtualRobot::WorkspaceGrid::gridSizeY
protected
float VirtualRobot::WorkspaceGrid::maxX
protected
float VirtualRobot::WorkspaceGrid::maxY
protected
float VirtualRobot::WorkspaceGrid::minX
protected
float VirtualRobot::WorkspaceGrid::minY
protected