libmove3d-planners
 All Classes Namespaces Files Functions Variables Enumerations Enumerator Friends Macros Groups Pages
Public Member Functions | List of all members
HRICS::AgentGrid Class Reference
Inheritance diagram for HRICS::AgentGrid:
API::ThreeDGrid API::BaseGrid

Public Member Functions

 AgentGrid (Robot *robot, Distance *distCostSpace, Visibility *VisiCostSpace, Natural *NatuCostSpace)
 
 AgentGrid (std::vector< int > size)
 
 AgentGrid (double pace, std::vector< double > envSize, Robot *robot, Distance *distCostSpace, Visibility *VisiCostSpace, Natural *NatuCostSpace)
 
 AgentGrid (const AgentGrid &grid)
 Creates a copy of a given grid. More...
 
API::ThreeDCellcreateNewCell (unsigned int index, unsigned int x, unsigned int y, unsigned int z)
 Virtual function that creates a new cell. More...
 
RobotgetRobot ()
 
DistancegetDistance ()
 
VisibilitygetVisibility ()
 
NaturalgetNatural ()
 
Eigen::Affine3d getTransformFromRobotPos ()
 Get the transform matrix between the origin and the robot current position All grid points are stored relative to the agent first joint To get points in the global frame points must be transformed by this matrix.
 
Eigen::Vector3d getTranformedToRobotFrame (const Eigen::Vector3d &WSPoint)
 Get the global frame points in the robot frame. More...
 
bool isReachable (const Eigen::Vector3d &WSPoint)
 
double getCellCostAt (const Eigen::Vector3d &WSPoint)
 Get the cell containing WSPoint and returns the cost.
 
double getCompleteCellCostAt (const Eigen::Vector3d &WSPoint, std::vector< double > &costs)
 Get the cell containing WSPoint and returns the cost.
 
std::vector< Eigen::Vector3d > getBox ()
 Returns the bounding box of the human grid The initial box is tranformed to the current agent position A vector of 3D dimensional vertex in the global frame is returned.
 
void resetCellCost ()
 Reset Grid Cost.
 
void resetReachability ()
 
void initReachable ()
 
int robotConfigInCell (int i)
 get config
 
void computeReachability ()
 
void computeAllCellCost ()
 Compute Grid Cost.
 
void computeCellVectors ()
 Compute Grid Cost.
 
void computeRadius ()
 
void computeCostCombination ()
 
void draw ()
 Draw a openGl view of the grid.
 
- Public Member Functions inherited from API::ThreeDGrid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ThreeDGrid ()
 Constructor. More...
 
 ThreeDGrid (Eigen::Vector3i size, std::vector< double > envSize)
 
 ThreeDGrid (double samplingRate, std::vector< double > envSize)
 
 ThreeDGrid (const ThreeDGrid &grid)
 Copy.
 
virtual ~ThreeDGrid ()
 Destructor.
 
void createAllCells ()
 Creates All Cells. More...
 
Eigen::Vector3d getCellSize ()
 
unsigned int getXNumberOfCells () const
 
unsigned int getYNumberOfCells () const
 
unsigned int getZNumberOfCells () const
 
ThreeDCellgetCell (unsigned int x, unsigned int y, unsigned int z) const
 Retruns the Cell at (x,y,z) More...
 
ThreeDCellgetCell (Eigen::Vector3i cell) const
 
ThreeDCellgetCell (const Eigen::Vector3d &pos) const
 
ThreeDCellgetCell (double *pos) const
 Get Cell in 3D ThreeDGrid. More...
 
Eigen::Vector3i getCellCoord (ThreeDCell *ptrCell) const
 Get place in grid. More...
 
ThreeDCellgetNeighbour (const Eigen::Vector3i &pos, unsigned int i) const
 Get Neighboor Cell.
 
Eigen::Vector3d getCoordinates (ThreeDCell *cell) const
 Retrive the X Y Z coordinate of the cell from its index.
 
unsigned int getXlineOfCell (unsigned int ith)
 
unsigned int getYlineOfCell (unsigned int ith)
 
unsigned int getZlineOfCell (unsigned int ith)
 
bool writeToXmlFile (std::string file)
 Writes the grid to en xml file.
 
bool loadFromXmlFile (std::string file)
 Reads the grid from an xml file.
 
- Public Member Functions inherited from API::BaseGrid
 BaseGrid (const BaseGrid &grid)
 
BaseCellgetCell (unsigned int i)
 Get Cell. More...
 
unsigned int getNumberOfCells ()
 Get Number Of Cells.
 
std::string getName ()
 

Additional Inherited Members

- Protected Member Functions inherited from API::ThreeDGrid
Eigen::Vector3d computeCellCorner (unsigned int x, unsigned int y, unsigned int z)
 Computes the corner of a cell. More...
 
- Protected Attributes inherited from API::ThreeDGrid
Eigen::Vector3d _originCorner
 
Eigen::Vector3d _cellSize
 
unsigned int _nbCellsX
 
unsigned int _nbCellsY
 
unsigned int _nbCellsZ
 
- Protected Attributes inherited from API::BaseGrid
std::vector< BaseCell * > _cells
 
std::string m_name
 

Constructor & Destructor Documentation

AgentGrid::AgentGrid ( const AgentGrid grid)

Creates a copy of a given grid.

Parameters
theagent grid to be copied

Member Function Documentation

API::ThreeDCell * AgentGrid::createNewCell ( unsigned int  index,
unsigned int  x,
unsigned int  y,
unsigned int  z 
)
virtual

Virtual function that creates a new cell.

Parameters
integerindex
integerx position in the grid
integery position in the grid
integerz position in the grid

Reimplemented from API::ThreeDGrid.

Vector3d AgentGrid::getTranformedToRobotFrame ( const Eigen::Vector3d &  WSPoint)

Get the global frame points in the robot frame.

Parameters
Apoint in the global frame All grid points are stored relative to the agent first joint To get global frame points in the robot frame the points are transformed, using this function which uses the invers of the first joint matrix

The documentation for this class was generated from the following files: