Simox  2.3.50
Saba::GraspRrt Class Reference
Inheritance diagram for Saba::GraspRrt:
Saba::Rrt Saba::MotionPlanner

Data Structures

struct  GraspInfo
 
struct  PlanningPerformance
 

Public Types

enum  MoveArmResult {
  eMovedOneStep, eGoalReached, eCollision_Environment, eGraspablePoseReached,
  eJointBoundaryViolation, eTrapped, eError, eFatalError
}
 
typedef std::vector< GraspInfo, Eigen::aligned_allocator< GraspInfo > > GraspInfoVector
 
- Public Types inherited from Saba::Rrt
enum  RrtMethod { eExtend, eConnect, eConnectCompletePath }
 
enum  ExtensionResult { eError, eFailed, eSuccess, ePartial }
 

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW GraspRrt (CSpaceSampledPtr cspace, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, VirtualRobot::BasicGraspQualityMeasurePtr measure, VirtualRobot::SceneObjectSetPtr graspCollisionObjects=VirtualRobot::SceneObjectSetPtr(), float probabGraspHypothesis=0.1f, float graspQualityMinScore=0.01f)
 
virtual ~GraspRrt ()
 
virtual bool plan (bool bQuiet=false)
 
virtual void printConfig (bool printOnlyParams=false)
 
virtual bool setStart (const Eigen::VectorXf &c)
 set start configuration More...
 
virtual bool setGoal (const Eigen::VectorXf &c)
 This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown. More...
 
virtual void reset ()
 reset the planner More...
 
ApproachDiscretizationPtr getPoseRelationSphere ()
 
void getGraspInfoResult (GraspInfoVector &vStoreGraspInfo)
 Stores all found grasps to given vector (thread safe) More...
 
bool getGraspInfoResult (int index, GraspInfo &storeGraspInfo)
 Returns a specific grasp result (thread safe) More...
 
int getNrOfGraspInfoResults ()
 Returns number of (currently) found grasps (thread safe) More...
 
PlanningPerformance getPerformanceMeasurement ()
 
void printPerformanceResults ()
 
bool calculateGlobalGraspPose (const Eigen::VectorXf &c, Eigen::Matrix4f &storeGoal)
 
MoveArmResult createWorkSpaceSamplingStep (const Eigen::Matrix4f &currentPose, const Eigen::Matrix4f &goalPose, Eigen::VectorXf &storeCSpaceConf)
 
virtual bool init ()
 
void setMinGraspContacts (int nr)
 
- Public Member Functions inherited from Saba::Rrt
 Rrt (CSpaceSampledPtr cspace, RrtMethod mode=eConnect, float probabilityExtendToGoal=0.1f)
 
virtual ~Rrt ()
 
void setProbabilityExtendToGoal (float p)
 
CSpaceTreePtr getTree ()
 
- Public Member Functions inherited from Saba::MotionPlanner
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MotionPlanner (CSpacePtr cspace)
 
virtual ~MotionPlanner ()
 destructor More...
 
CSpacePathPtr getSolution ()
 
void setMaxCycles (unsigned int mc)
 
Eigen::VectorXf getStartConfig ()
 return start configuration More...
 
Eigen::VectorXf getGoalConfig ()
 return goal configuration More...
 
unsigned int getNrOfCycles ()
 check that planner is initialized More...
 
CSpacePtr getCSpace ()
 The CSpace. More...
 
virtual void stopExecution ()
 
void setName (std::string sName)
 Give the planner a name. More...
 
std::string getName ()
 The name of the planner. More...
 
float getPlanningTimeMS ()
 
virtual bool isInitialized ()
 returns true, when start and goal config have been set More...
 

Protected Member Functions

bool doPlanningCycle ()
 
bool processNodes (unsigned int startId, unsigned int endId)
 
bool processNode (CSpaceNodePtr n)
 Compute tcp (gcp) pose and update internal map and pose relation sphere. More...
 
MoveArmResult connectRandomGraspPositionJacobian ()
 
Rrt::ExtensionResult connectComplete ()
 
MoveArmResult moveTowardsGoal (CSpaceNodePtr startNode, const Eigen::Matrix4f &targetPose, int nMaxLoops)
 
MoveArmResult createWorkSpaceSamplingStep (const Eigen::Matrix4f &goalPose, CSpaceNodePtr extendNode, Eigen::VectorXf &storeCSpaceConf)
 
void limitWorkspaceStep (Eigen::Matrix4f &p)
 
MoveArmResult moveArmDiffKin (const Eigen::Matrix4f &deltaPose, Eigen::VectorXf &storeCSpaceConf)
 
float calculateGraspScore (const Eigen::VectorXf &c, int nId=-1, bool bStoreGraspInfoOnSuccess=false)
 
virtual Rrt::ExtensionResult connectComplete (Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID)
 
void printGraspInfo (GraspInfo &GrInfo)
 
- Protected Member Functions inherited from Saba::Rrt
virtual bool createSolution (bool bQuiet=false)
 create the solution More...
 
virtual ExtensionResult extend (Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID)
 
virtual ExtensionResult connectUntilCollision (Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID)
 

Protected Attributes

VirtualRobot::ObstaclePtr targetObject
 
VirtualRobot::RobotNodeSetPtr rns
 
VirtualRobot::EndEffectorPtr eef
 
VirtualRobot::RobotPtr robot
 
std::map< VirtualRobot::GraspPtr, Saba::CSpaceNodePtrgraspNodeMapping
 
std::vector< Eigen::VectorXf > ikSolutions
 
PlanningPerformance performanceMeasure
 
int minGraspContacts
 
float workSpaceSamplingCount
 
VirtualRobot::ObstaclePtr gcpOject
 
VirtualRobot::SceneObjectSetPtr graspCollisionObjects
 These objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and must not be part of these object set. More...
 
MoveArmResult extendGraspStatus
 
float cartSamplingOriStepSize
 
float cartSamplingPosStepSize
 
GraspInfoVector grasps
 
ApproachDiscretizationPtr poseSphere
 
VirtualRobot::DifferentialIKPtr diffIK
 
Eigen::Vector3f targetObjectPosition
 
float tryGraspsDistance2
 
int colChecksOverall
 
bool foundSolution
 
boost::mutex graspInfoMutex
 
bool verbose
 
std::map< CSpaceNodePtr, Eigen::Matrix4f, std::less< CSpaceNodePtr >, Eigen::aligned_allocator< std::pair< const CSpaceNodePtr, Eigen::Matrix4f > > > mapConfigTcp
 Store the tcp (gcp) poses that correspond with the config. More...
 
VirtualRobot::BasicGraspQualityMeasurePtr graspQualityMeasure
 
float probabGraspHypothesis
 
float graspQualityMinScore
 
bool plannerInitialized
 
- Protected Attributes inherited from Saba::Rrt
CSpaceTreePtr tree
 the rrt on which are operating More...
 
CSpaceNodePtr startNode
 start node (root of RRT) More...
 
CSpaceNodePtr goalNode
 goal node (set when RRT weas successfully connected to goalConfig) More...
 
Eigen::VectorXf tmpConfig
 tmp config More...
 
float extendGoToGoal
 the probability that the goal config is used instead of a randomly created configuration More...
 
float extendStepSize
 step size for one rrt extend (copied from cspace) More...
 
int lastAddedID
 ID of last added node. More...
 
RrtMethod rrtMode
 
- Protected Attributes inherited from Saba::MotionPlanner
CSpacePtr cspace
 the cspace on which are operating More...
 
CSpacePathPtr solution
 the solution More...
 
bool stopSearch
 indicates that the search should be interrupted More...
 
unsigned int dimension
 dimension of c-space More...
 
Eigen::VectorXf startConfig
 start config More...
 
bool startValid
 
Eigen::VectorXf goalConfig
 goal config More...
 
bool goalValid
 
unsigned int maxCycles
 maximum cycles for searching More...
 
unsigned int cycles
 current cycles done in the run method More...
 
std::string name
 Name of this planner (standard: "Motion Planner") More...
 
float planningTime
 

Detailed Description

A planner that combines the search for a feasible grasping pose with RRT-based motion planning. No predefined grasps are needed, a feasible grasp is searched during the planning process. Makes use of the DifferntialIK methods to generate potential approach movements. A GraspQualityMeasure object is needed in order to evaluate generated grasping hypotheses.

Related publication: "Integrated Grasp and Motion Planning", N. Vahrenkamp, M. Do, T. Asfour, R. Dillmann, ICRA 2010

Member Typedef Documentation

typedef std::vector< GraspInfo, Eigen::aligned_allocator<GraspInfo> > Saba::GraspRrt::GraspInfoVector

Member Enumeration Documentation

Enumerator
eMovedOneStep 
eGoalReached 
eCollision_Environment 
eGraspablePoseReached 
eJointBoundaryViolation 
eTrapped 
eError 
eFatalError 

Constructor & Destructor Documentation

Saba::GraspRrt::GraspRrt ( CSpaceSampledPtr  cspace,
VirtualRobot::EndEffectorPtr  eef,
VirtualRobot::ObstaclePtr  object,
VirtualRobot::BasicGraspQualityMeasurePtr  measure,
VirtualRobot::SceneObjectSetPtr  graspCollisionObjects = VirtualRobot::SceneObjectSetPtr(),
float  probabGraspHypothesis = 0.1f,
float  graspQualityMinScore = 0.01f 
)

Constructor

Parameters
cspaceThe C-Space that should be used for collision detection. The RobotNodeSet, that was used to define the cspace, is internally used to generate approach movements.
eefThe EndEffector that should be used for grasping.
objectThe object to be grasped
measureA measure object that should be used for evaluating the quality of generated grasping hypotheses.
graspCollisionObjectsThese (optional) objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and need not be part of these object set.
probabGraspHypothesisThe probability that a grasping hypothesis is generated during a loop.
graspQualityMinScoreThe minimum score that must be achieved for a valid grasp.
See also
GraspStudio::GraspQualityMeasureWrenchSpace
Saba::GraspRrt::~GraspRrt ( )
virtual

Member Function Documentation

bool Saba::GraspRrt::calculateGlobalGraspPose ( const Eigen::VectorXf &  c,
Eigen::Matrix4f &  storeGoal 
)

This method generates a grasping pose on the object's surface. The z-axis of the GCP's coordinate system is aligned with the normal. We assume that the z axis specifies the favorite approach movement of the EEF.

float Saba::GraspRrt::calculateGraspScore ( const Eigen::VectorXf &  c,
int  nId = -1,
bool  bStoreGraspInfoOnSuccess = false 
)
protected
Rrt::ExtensionResult Saba::GraspRrt::connectComplete ( )
protected
Rrt::ExtensionResult Saba::GraspRrt::connectComplete ( Eigen::VectorXf &  c,
CSpaceTreePtr  tree,
int &  storeLastAddedID 
)
protectedvirtual

Reimplemented from Saba::Rrt.

GraspRrt::MoveArmResult Saba::GraspRrt::connectRandomGraspPositionJacobian ( )
protected
GraspRrt::MoveArmResult Saba::GraspRrt::createWorkSpaceSamplingStep ( const Eigen::Matrix4f &  currentPose,
const Eigen::Matrix4f &  goalPose,
Eigen::VectorXf &  storeCSpaceConf 
)

Computes a step towards goalPose.

Parameters
currentPoseThe current pose
goalPoseThe pose in global coord system.
storeCSpaceConfThe result is stored here.
Returns
The resulting state.
GraspRrt::MoveArmResult Saba::GraspRrt::createWorkSpaceSamplingStep ( const Eigen::Matrix4f &  goalPose,
CSpaceNodePtr  extendNode,
Eigen::VectorXf &  storeCSpaceConf 
)
protected
bool Saba::GraspRrt::doPlanningCycle ( )
protected
void Saba::GraspRrt::getGraspInfoResult ( GraspRrt::GraspInfoVector storeGraspInfo)

Stores all found grasps to given vector (thread safe)

bool Saba::GraspRrt::getGraspInfoResult ( int  index,
GraspInfo storeGraspInfo 
)

Returns a specific grasp result (thread safe)

int Saba::GraspRrt::getNrOfGraspInfoResults ( )

Returns number of (currently) found grasps (thread safe)

GraspRrt::PlanningPerformance Saba::GraspRrt::getPerformanceMeasurement ( )
ApproachDiscretizationPtr Saba::GraspRrt::getPoseRelationSphere ( )

Returns the internal representation of the pose sphere. The pose sphere is used to encode approach directions.

bool Saba::GraspRrt::init ( )
virtual

Can be used for custom initialization. Usually this method is automatically called at the beginning of a planning query.

void Saba::GraspRrt::limitWorkspaceStep ( Eigen::Matrix4f &  p)
protected

Limit the step that is defined with p, so that the translational and rotational stepsize is limited by cartSamplingOriStepSize and cartSamplingPosStepSize

GraspRrt::MoveArmResult Saba::GraspRrt::moveArmDiffKin ( const Eigen::Matrix4f &  deltaPose,
Eigen::VectorXf &  storeCSpaceConf 
)
protected

Current pose of RNS is moved.

Parameters
deltaPoseConsists of the 3x3 rotation delta R and the 3 dim translational delta T in homogeneous notation. R and T must be given in global coordinate system.
storeCSpaceConfThe result is stored here.
GraspRrt::MoveArmResult Saba::GraspRrt::moveTowardsGoal ( CSpaceNodePtr  startNode,
const Eigen::Matrix4f &  targetPose,
int  nMaxLoops 
)
protected
bool Saba::GraspRrt::plan ( bool  bQuiet = false)
virtual

do the planning (blocking method)

Returns
true if solution was found, otherwise false

Reimplemented from Saba::Rrt.

void Saba::GraspRrt::printConfig ( bool  printOnlyParams = false)
virtual

Print setup of planner.

Parameters
printOnlyParamsIf set the decorating start and end is skipped (can be used to print derived classes).

Reimplemented from Saba::Rrt.

void Saba::GraspRrt::printGraspInfo ( GraspInfo GrInfo)
protected
void Saba::GraspRrt::printPerformanceResults ( )
bool Saba::GraspRrt::processNode ( CSpaceNodePtr  n)
protected

Compute tcp (gcp) pose and update internal map and pose relation sphere.

bool Saba::GraspRrt::processNodes ( unsigned int  startId,
unsigned int  endId 
)
protected

For all nodes in tree, that form the path from startId to endId processNode is called. startId is not processed.

void Saba::GraspRrt::reset ( )
virtual

reset the planner

Reimplemented from Saba::Rrt.

bool Saba::GraspRrt::setGoal ( const Eigen::VectorXf &  c)
virtual

This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown.

Reimplemented from Saba::Rrt.

void Saba::GraspRrt::setMinGraspContacts ( int  nr)

By default min 3 contacts are needed, this number can be adjusted here.

bool Saba::GraspRrt::setStart ( const Eigen::VectorXf &  c)
virtual

set start configuration

Reimplemented from Saba::Rrt.

Field Documentation

float Saba::GraspRrt::cartSamplingOriStepSize
protected
float Saba::GraspRrt::cartSamplingPosStepSize
protected
int Saba::GraspRrt::colChecksOverall
protected
VirtualRobot::DifferentialIKPtr Saba::GraspRrt::diffIK
protected
VirtualRobot::EndEffectorPtr Saba::GraspRrt::eef
protected
MoveArmResult Saba::GraspRrt::extendGraspStatus
protected
bool Saba::GraspRrt::foundSolution
protected
VirtualRobot::ObstaclePtr Saba::GraspRrt::gcpOject
protected
VirtualRobot::SceneObjectSetPtr Saba::GraspRrt::graspCollisionObjects
protected

These objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and must not be part of these object set.

boost::mutex Saba::GraspRrt::graspInfoMutex
protected
std::map< VirtualRobot::GraspPtr, Saba::CSpaceNodePtr > Saba::GraspRrt::graspNodeMapping
protected
VirtualRobot::BasicGraspQualityMeasurePtr Saba::GraspRrt::graspQualityMeasure
protected
float Saba::GraspRrt::graspQualityMinScore
protected
GraspInfoVector Saba::GraspRrt::grasps
protected
std::vector< Eigen::VectorXf > Saba::GraspRrt::ikSolutions
protected
std::map< CSpaceNodePtr, Eigen::Matrix4f, std::less<CSpaceNodePtr>, Eigen::aligned_allocator<std::pair<const CSpaceNodePtr, Eigen::Matrix4f> > > Saba::GraspRrt::mapConfigTcp
protected

Store the tcp (gcp) poses that correspond with the config.

int Saba::GraspRrt::minGraspContacts
protected
PlanningPerformance Saba::GraspRrt::performanceMeasure
protected
bool Saba::GraspRrt::plannerInitialized
protected
ApproachDiscretizationPtr Saba::GraspRrt::poseSphere
protected
float Saba::GraspRrt::probabGraspHypothesis
protected
VirtualRobot::RobotNodeSetPtr Saba::GraspRrt::rns
protected
VirtualRobot::RobotPtr Saba::GraspRrt::robot
protected
VirtualRobot::ObstaclePtr Saba::GraspRrt::targetObject
protected
Eigen::Vector3f Saba::GraspRrt::targetObjectPosition
protected
float Saba::GraspRrt::tryGraspsDistance2
protected
bool Saba::GraspRrt::verbose
protected
float Saba::GraspRrt::workSpaceSamplingCount
protected