Inheritance diagram for Saba::GraspRrt:

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)

	~GraspRrt () override

bool	plan (bool bQuiet=false) override

void	printConfig (bool printOnlyParams=false) override

bool	setStart (const Eigen::VectorXf &c) override
	set start configuration More...

bool	setGoal (const Eigen::VectorXf &c) override
	This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown. More...

void	reset () override
	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 (CSpacePtr cspace, RrtMethod mode=eConnect, float probabilityExtendToGoal=0.1f, float samplingSize=-1)

	~Rrt () override

bool	plan (bool bQuiet=false) override

void	printConfig (bool printOnlyParams=false) override

void	reset () override
	reset the planner More...

bool	setStart (const Eigen::VectorXf &c) override
	set start configuration More...

bool	setGoal (const Eigen::VectorXf &c) override
	set goal configuration More...

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...

void	setPlanningTimeout (float timeoutMs)

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)

Rrt::ExtensionResult	connectComplete (Eigen::VectorXf &c, CSpaceTreePtr tree, int &storeLastAddedID) override

void	printGraspInfo (GraspInfo &GrInfo)

Protected Member Functions inherited from Saba::Rrt
bool	createSolution (bool bQuiet=false) override
	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::CSpaceNodePtr >	graspNodeMapping

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

std::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

float	planningTimeout
	Planning time in milliseconds. More...

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

◆ GraspInfoVector

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

Member Enumeration Documentation

◆ MoveArmResult

enum Saba::GraspRrt::MoveArmResult

Enumerator
eMovedOneStep
eGoalReached
eCollision_Environment
eGraspablePoseReached
eJointBoundaryViolation
eTrapped
eError
eFatalError

Constructor & Destructor Documentation

◆ GraspRrt()

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

cspace	The 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.
eef	The EndEffector that should be used for grasping.
object	The object to be grasped
measure	A measure object that should be used for evaluating the quality of generated grasping hypotheses.
graspCollisionObjects	These (optional) objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and need not be part of these object set.
probabGraspHypothesis	The probability that a grasping hypothesis is generated during a loop.
graspQualityMinScore	The minimum score that must be achieved for a valid grasp.

See also: GraspStudio::GraspQualityMeasureWrenchSpace

◆ ~GraspRrt()

Saba::GraspRrt::~GraspRrt ( )

overridedefault

Member Function Documentation

◆ calculateGlobalGraspPose()

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.

◆ calculateGraspScore()

float Saba::GraspRrt::calculateGraspScore	(	const Eigen::VectorXf &	c,
		int	nId = `-1`,
		bool	bStoreGraspInfoOnSuccess = `false`
	)

protected

◆ connectComplete() [1/2]

Rrt::ExtensionResult Saba::GraspRrt::connectComplete ( )

protected

◆ connectComplete() [2/2]

Rrt::ExtensionResult Saba::GraspRrt::connectComplete	(	Eigen::VectorXf &	c,
		CSpaceTreePtr	tree,
		int &	storeLastAddedID
	)

overrideprotectedvirtual

Reimplemented from Saba::Rrt.

◆ connectRandomGraspPositionJacobian()

GraspRrt::MoveArmResult Saba::GraspRrt::connectRandomGraspPositionJacobian ( )

protected

◆ createWorkSpaceSamplingStep() [1/2]

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

Computes a step towards goalPose.

Parameters

currentPose	The current pose
goalPose	The pose in global coord system.
storeCSpaceConf	The result is stored here.

Returns: The resulting state.

◆ createWorkSpaceSamplingStep() [2/2]

GraspRrt::MoveArmResult Saba::GraspRrt::createWorkSpaceSamplingStep	(	const Eigen::Matrix4f &	goalPose,
		CSpaceNodePtr	extendNode,
		Eigen::VectorXf &	storeCSpaceConf
	)

protected

◆ doPlanningCycle()

bool Saba::GraspRrt::doPlanningCycle ( )

protected

◆ getGraspInfoResult() [1/2]

void Saba::GraspRrt::getGraspInfoResult ( GraspRrt::GraspInfoVector & storeGraspInfo )

Stores all found grasps to given vector (thread safe)

◆ getGraspInfoResult() [2/2]

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

Returns a specific grasp result (thread safe)

◆ getNrOfGraspInfoResults()

int Saba::GraspRrt::getNrOfGraspInfoResults ( )

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

◆ getPerformanceMeasurement()

GraspRrt::PlanningPerformance Saba::GraspRrt::getPerformanceMeasurement ( )

◆ getPoseRelationSphere()

ApproachDiscretizationPtr Saba::GraspRrt::getPoseRelationSphere ( )

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

◆ init()

bool Saba::GraspRrt::init ( )

virtual

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

◆ limitWorkspaceStep()

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

◆ moveArmDiffKin()

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

protected

Current pose of RNS is moved.

Parameters

deltaPose	Consists 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.
storeCSpaceConf	The result is stored here.

◆ moveTowardsGoal()

GraspRrt::MoveArmResult Saba::GraspRrt::moveTowardsGoal	(	CSpaceNodePtr	startNode,
		const Eigen::Matrix4f &	targetPose,
		int	nMaxLoops
	)

protected

◆ plan()

bool Saba::GraspRrt::plan ( bool bQuiet = false )

overridevirtual

do the planning (blocking method)

Returns: true if solution was found, otherwise false

Implements Saba::MotionPlanner.

◆ printConfig()

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

overridevirtual

Print setup of planner.

Parameters

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

Reimplemented from Saba::MotionPlanner.

◆ printGraspInfo()

void Saba::GraspRrt::printGraspInfo ( GraspInfo & GrInfo )

protected

◆ printPerformanceResults()

void Saba::GraspRrt::printPerformanceResults ( )

◆ processNode()

bool Saba::GraspRrt::processNode ( CSpaceNodePtr n )

protected

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

◆ processNodes()

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.

◆ reset()

void Saba::GraspRrt::reset ( )

overridevirtual

reset the planner

Reimplemented from Saba::MotionPlanner.

◆ setGoal()

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

overridevirtual

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

Reimplemented from Saba::MotionPlanner.

◆ setMinGraspContacts()

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

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

◆ setStart()

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

overridevirtual

set start configuration

Reimplemented from Saba::MotionPlanner.

Field Documentation

◆ cartSamplingOriStepSize

float Saba::GraspRrt::cartSamplingOriStepSize

protected

◆ cartSamplingPosStepSize

float Saba::GraspRrt::cartSamplingPosStepSize

protected

◆ colChecksOverall

int Saba::GraspRrt::colChecksOverall

protected

◆ diffIK

VirtualRobot::DifferentialIKPtr Saba::GraspRrt::diffIK

protected

◆ eef

VirtualRobot::EndEffectorPtr Saba::GraspRrt::eef

protected

◆ extendGraspStatus

MoveArmResult Saba::GraspRrt::extendGraspStatus

protected

◆ foundSolution

bool Saba::GraspRrt::foundSolution

protected

◆ gcpOject

VirtualRobot::ObstaclePtr Saba::GraspRrt::gcpOject

protected

◆ graspCollisionObjects

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.

◆ graspInfoMutex

std::mutex Saba::GraspRrt::graspInfoMutex

protected

◆ graspNodeMapping

std::map< VirtualRobot::GraspPtr, Saba::CSpaceNodePtr > Saba::GraspRrt::graspNodeMapping

protected

◆ graspQualityMeasure

VirtualRobot::BasicGraspQualityMeasurePtr Saba::GraspRrt::graspQualityMeasure

protected

◆ graspQualityMinScore

float Saba::GraspRrt::graspQualityMinScore

protected

◆ grasps

GraspInfoVector Saba::GraspRrt::grasps

protected

◆ ikSolutions

std::vector< Eigen::VectorXf > Saba::GraspRrt::ikSolutions

protected

◆ mapConfigTcp

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.

◆ minGraspContacts

int Saba::GraspRrt::minGraspContacts

protected

◆ performanceMeasure

PlanningPerformance Saba::GraspRrt::performanceMeasure

protected

◆ plannerInitialized

bool Saba::GraspRrt::plannerInitialized

protected

◆ poseSphere

ApproachDiscretizationPtr Saba::GraspRrt::poseSphere

protected

◆ probabGraspHypothesis

float Saba::GraspRrt::probabGraspHypothesis

protected

◆ rns

VirtualRobot::RobotNodeSetPtr Saba::GraspRrt::rns

protected

◆ robot

VirtualRobot::RobotPtr Saba::GraspRrt::robot

protected

◆ targetObject

VirtualRobot::ObstaclePtr Saba::GraspRrt::targetObject

protected

◆ targetObjectPosition

Eigen::Vector3f Saba::GraspRrt::targetObjectPosition

protected

◆ tryGraspsDistance2

float Saba::GraspRrt::tryGraspsDistance2

protected

◆ verbose

bool Saba::GraspRrt::verbose

protected

◆ workSpaceSamplingCount

float Saba::GraspRrt::workSpaceSamplingCount

protected

Data Structures

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ GraspInfoVector

Member Enumeration Documentation

◆ MoveArmResult

Constructor & Destructor Documentation

◆ GraspRrt()

◆ ~GraspRrt()

Member Function Documentation

◆ calculateGlobalGraspPose()

◆ calculateGraspScore()

◆ connectComplete() [1/2]

◆ connectComplete() [2/2]

◆ connectRandomGraspPositionJacobian()

◆ createWorkSpaceSamplingStep() [1/2]

◆ createWorkSpaceSamplingStep() [2/2]

◆ doPlanningCycle()

◆ getGraspInfoResult() [1/2]

◆ getGraspInfoResult() [2/2]

◆ getNrOfGraspInfoResults()

◆ getPerformanceMeasurement()

◆ getPoseRelationSphere()

◆ init()

◆ limitWorkspaceStep()

◆ moveArmDiffKin()

◆ moveTowardsGoal()

◆ plan()

◆ printConfig()

◆ printGraspInfo()

◆ printPerformanceResults()

◆ processNode()

◆ processNodes()

◆ reset()

◆ setGoal()

◆ setMinGraspContacts()

◆ setStart()

Field Documentation

◆ cartSamplingOriStepSize

◆ cartSamplingPosStepSize

◆ colChecksOverall

◆ diffIK

◆ eef

◆ extendGraspStatus

◆ foundSolution

◆ gcpOject

◆ graspCollisionObjects

◆ graspInfoMutex

◆ graspNodeMapping

◆ graspQualityMeasure

◆ graspQualityMinScore

◆ grasps

◆ ikSolutions

◆ mapConfigTcp

◆ minGraspContacts

◆ performanceMeasure

◆ plannerInitialized

◆ poseSphere

◆ probabGraspHypothesis

◆ rns

◆ robot

◆ targetObject

◆ targetObjectPosition

◆ tryGraspsDistance2

◆ verbose

◆ workSpaceSamplingCount