Simox  2.3.49
Saba::ApproachDiscretization Class Reference

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW ApproachDiscretization (float radius=100.0f, int steps=3)
 
virtual ~ApproachDiscretization ()
 
Eigen::Matrix4f getGlobalPose () const
 
void setGlobalPose (const Eigen::Matrix4f &pose)
 
int getNearestVertexId (const Eigen::Matrix4f &pose)
 
int getNearestVertexIdVec (const Eigen::Vector3f &pos)
 
int getNearestFaceId (const Eigen::Matrix4f &pose)
 
int getNearestFaceIdVec (const Eigen::Vector3f &pos)
 
void removeCSpaceNode (const Eigen::Vector3f &cartPose, CSpaceNodePtr node)
 
void removeCSpaceNode (int faceId, CSpaceNodePtr node)
 
void addCSpaceNode (const Eigen::Vector3f &cartPos, CSpaceNodePtr node)
 
void addCSpaceNode (int faceId, CSpaceNodePtr node)
 
void clearCSpaceNodeMapping ()
 Reset. More...
 
CSpaceNodePtr getGoodRatedNode (int loops=30)
 

Detailed Description

This discretization class can be used to encode approach directions. Internally all approach directions are mapped onto the surface of a discretized sphere. Each face of the PoseRelationSphere holds a list with corresponding RRT nodes, lying in the direction that is covered by the triangle. The method getGoodRatedNode can be used to retrieve RRT-nodes for which the approach directions are uniformly sampled. (further details can be found in "Integrated Grasp and Motion Planning", ICRA 2010, Vahrenkamp et al.)

Constructor & Destructor Documentation

Saba::ApproachDiscretization::ApproachDiscretization ( float  radius = 100.0f,
int  steps = 3 
)
Saba::ApproachDiscretization::~ApproachDiscretization ( )
virtual

Member Function Documentation

void Saba::ApproachDiscretization::addCSpaceNode ( const Eigen::Vector3f &  cartPos,
CSpaceNodePtr  node 
)

Add a node of the RRT with corresponding Cartesian pose. The pose should be the resulting EEF's pose.

void Saba::ApproachDiscretization::addCSpaceNode ( int  faceId,
CSpaceNodePtr  node 
)
void Saba::ApproachDiscretization::clearCSpaceNodeMapping ( )

Reset.

Eigen::Matrix4f Saba::ApproachDiscretization::getGlobalPose ( ) const
CSpaceNodePtr Saba::ApproachDiscretization::getGoodRatedNode ( int  loops = 30)

Return a RRT node, so that the corresponding Cartesian poses are uniformly sampled over the sphere. Removes the selected RRT node from the internal data. This method just checks loops faces of the sphere, so that the result is approximated.

int Saba::ApproachDiscretization::getNearestFaceId ( const Eigen::Matrix4f &  pose)

Returns Id of the surface face which is nearest to line through center of sphere and pose

Parameters
posein global coordinate system, (center of sphere is at globalPose)
int Saba::ApproachDiscretization::getNearestFaceIdVec ( const Eigen::Vector3f &  pos)
int Saba::ApproachDiscretization::getNearestVertexId ( const Eigen::Matrix4f &  pose)

Returns Id of the surface vertex which is nearest to line through center of sphere and pose

Parameters
posein global coordinate system, (center of sphere is at globalPose)
int Saba::ApproachDiscretization::getNearestVertexIdVec ( const Eigen::Vector3f &  pos)
void Saba::ApproachDiscretization::removeCSpaceNode ( const Eigen::Vector3f &  cartPose,
CSpaceNodePtr  node 
)
void Saba::ApproachDiscretization::removeCSpaceNode ( int  faceId,
CSpaceNodePtr  node 
)
void Saba::ApproachDiscretization::setGlobalPose ( const Eigen::Matrix4f &  pose)

Set the location (only position is used)