Runs the GraspIt! Eigengrasp planning algorithm. More...

#include <EigenGraspPlanner.h>

Inheritance diagram for GraspIt::EigenGraspPlanner:

Public Types
enum	GraspItSearchEnergyType { EnergyContact }
enum	GraspItStateType { AxisAngle }
enum	PlannerType { SimAnn }
Public Slots
void	plannerCompleteSlot ()
void	plannerUpdateSlot ()
Public Member Functions
	EigenGraspPlanner (const std::string &name, const SHARED_PTR< GraspItSceneManager > &i)
void	getResults (std::vector< EigenGraspResult > &allGrasps) const
bool	plan (const int maxPlanningSteps, const int repeatPlanning, const int maxResultsPerRepeat, const bool finishWithAutograsp, const PlannerType &planType=SimAnn)
bool	plan (const std::string &handName, const std::string &objectName, const EigenTransform *objectPose, const int maxPlanningSteps, const int repeatPlanning, const int maxResultsPerRepeat, const bool finishWithAutograsp, const PlannerType &planType=SimAnn)
bool	saveResultsAsWorldFiles (const std::string &inDirectory, const std::string &fileNamePrefix, bool asGraspIt=true, bool asInventor=false, bool createDir=true, bool saveSeparatePoseIV=false)
virtual	~EigenGraspPlanner ()
Protected Member Functions
virtual void	idleEventFromSceneManager ()
virtual void	onSceneManagerShutdown ()
Private Types
enum	PlannerCommand { NONE, START, STOP }
Private Member Functions
bool	checkStateValidity (const GraspPlanningState *s) const
bool	copyResult (const GraspPlanningState *s, EigenGraspResult &result) const
void	deleteResults ()
void	getEigenGraspValues (const GraspPlanningState *s, std::vector< double > &egVals) const
void	getGraspJointDOFs (const GraspPlanningState *s, std::vector< double > &dofs) const
EigenTransform	getHandTransform (const GraspPlanningState *s) const
EigenTransform	getObjectTransform (const GraspPlanningState *s) const
PlannerCommand	getPlannerCommand ()
void	getPregraspJointDOFs (const GraspPlanningState *s, std::vector< double > &dofs) const
bool	initPlanner (const int maxPlanningSteps, const PlannerType &planType=SimAnn)
void	initPlannerType (const GraspPlanningState &stateTemplate, const PlannerType &pt)
void	initSearchType (GraspPlanningState &stateTemplate, const GraspItStateType &st)
void	ivIdleCallback ()
void	plannerComplete ()
void	plannerReset ()
void	plannerUpdate ()
void	printPlanningResults ()
void	printResult (int i, bool detailed=false)
void	setPlannerCommand (const PlannerCommand c)
void	setPlanningParameters ()
void	updateResults ()
Private Attributes
EGPlanner *	graspitEgPlanner
RECURSIVE_MUTEX	graspitEgPlannerMtx
GraspItSearchEnergyType	graspitSearchEnergyType
GraspItStateType	graspitStateType
PlannerCommand	planCommand
MUTEX	planCommandMtx
std::vector< const GraspPlanningState * >	results
bool	useContacts

Detailed Description

Runs the GraspIt! Eigengrasp planning algorithm.

This class extends the GraspItAccessor class to run the Eigengrasp planning algorithms in the original graspit source on the world.

In the current implementation, Qt is required for running the Eigengrasp planner. GraspItSceneManager::eventThreadRunsQt() needs to return true for the GraspItSceneManager passed into the constructor of this class. For an example on how to implement this class without direct Qt dependencies, check out EigenGraspPlannerNoQt.

*Notes*: See also [git issue #1](https://github.com/JenniferBuehler/graspit-pkgs/issues/1) for restrictions of this class.

*Important note for developers*: This class should not be derived further in its current implementation. This is because it uses an SoIdleSensor locally which it destroys in the destructor. This needs to be destroyed *before* the mandatory call of removeFromIdleListeners() in all subclasses destructors. Support for further extension is easy to achieve (by not using the local SoIdleSensor or adding a new method shutdownEigenGraspPlanner() or similar), but it has not been considered at this stage yet.

Author:: Jennifer Buehler

Date:: December 2015

Definition at line 78 of file EigenGraspPlanner.h.

Member Enumeration Documentation

enum GraspIt::EigenGraspPlanner::GraspItSearchEnergyType

Enumerator:

EnergyContact

Definition at line 92 of file EigenGraspPlanner.h.

enum GraspIt::EigenGraspPlanner::GraspItStateType

Enumerator:

AxisAngle

Definition at line 85 of file EigenGraspPlanner.h.

enum GraspIt::EigenGraspPlanner::PlannerCommand [private]

Enumerator:

NONE
START
STOP

Definition at line 340 of file EigenGraspPlanner.h.

enum GraspIt::EigenGraspPlanner::PlannerType

Enumerator:

SimAnn

Definition at line 98 of file EigenGraspPlanner.h.

Constructor & Destructor Documentation

EigenGraspPlanner::EigenGraspPlanner	(	const std::string &	name,
		const SHARED_PTR< GraspItSceneManager > &	i
	)

Creates and initializes the planner. Also registers the class to enable updates from the scene manager thread by calling addAsSchedulable(). If eventThreadRunsQt() returns false, an exception is thrown, because this implementation requires updates from the main Qt loop.

Definition at line 59 of file EigenGraspPlanner.cpp.

EigenGraspPlanner::~EigenGraspPlanner ( ) [virtual]

Definition at line 83 of file EigenGraspPlanner.cpp.

Member Function Documentation

bool EigenGraspPlanner::checkStateValidity ( const GraspPlanningState * s ) const [private]

Check if this is a state that the current implementation can work with

Definition at line 570 of file EigenGraspPlanner.cpp.

bool EigenGraspPlanner::copyResult	(	const GraspPlanningState *	s,
		EigenGraspResult &	result
	)		const `[private]`

Copies relevant fields from s to result

Returns:: false if state s is invalid

Definition at line 695 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::deleteResults ( ) [private]

Deletes the objects in the temporary result buffer (field results) and clears the array.

Definition at line 302 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::getEigenGraspValues	(	const GraspPlanningState *	s,
		std::vector< double > &	egVals
	)		const `[private]`

gets the Eigengrasp values associated with this grasp state

Definition at line 552 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::getGraspJointDOFs	(	const GraspPlanningState *	s,
		std::vector< double > &	dofs
	)		const `[private]`

Gets the DOFs of the joints for the grasp of this state

Definition at line 450 of file EigenGraspPlanner.cpp.

GraspIt::EigenTransform EigenGraspPlanner::getHandTransform ( const GraspPlanningState * s ) const [private]

Gets the transform of the Hand in a search state

Definition at line 417 of file EigenGraspPlanner.cpp.

GraspIt::EigenTransform EigenGraspPlanner::getObjectTransform ( const GraspPlanningState * s ) const [private]

Gets the transform of the object in a search state

Definition at line 423 of file EigenGraspPlanner.cpp.

EigenGraspPlanner::PlannerCommand EigenGraspPlanner::getPlannerCommand ( ) [private]

Threadsafe method to read planCommand

Definition at line 176 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::getPregraspJointDOFs	(	const GraspPlanningState *	s,
		std::vector< double > &	dofs
	)		const `[private]`

Gets the DOFs of the joints for the pre-grasp of this state. This will execute an auto-grasp to open the hand before reading the DOF values.

Definition at line 478 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::getResults ( std::vector< EigenGraspResult > & allGrasps ) const

Returns the results of the grasp planning as EigenGraspResult objects.

Definition at line 749 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::idleEventFromSceneManager ( ) [protected, virtual]

This method will be called from within the scene manager thread in its next loop, **if** the method addAsIdleListener() was called for this instance **and** isScheduledForIdleEvent() returns true.

To trigger repeated calls of this function in each loop of the scene manager event loop, call scheduleForIdleEventUpdate() from within this function.

IMPORTANT: To not slow down the whole system this method should be kept very efficient! Any more complex operations should be done in other threads.

TIPP: While Qt is still used in GraspItSceneManager, this method will be called from the thread which also runs the Inventor stuff. So it is also possible to use this method only once (without re-scheduling with scheduleForIdleEventUpdate()) to create a SoIdleSensor object, which will then also be run from within the inventor thread.

Implements GraspIt::GraspItAccessor.

Definition at line 143 of file EigenGraspPlanner.cpp.

bool EigenGraspPlanner::initPlanner	(	const int	maxPlanningSteps,
		const PlannerType &	planType = `SimAnn`
	)		`[private]`

initializes the planner to use the specified planning type. This method needs to be called before the GraspIt planner is started.

See also:: void MainWindow::eigenGraspEigenActivated()

Parameters:

maxPlanningSteps	maximum steps (iterations) for the planning algorithm to use
planType	the type of planning algorithm to use. To this point, only simulated annealing is supported.

Definition at line 817 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::initPlannerType	(	const GraspPlanningState &	stateTemplate,
		const PlannerType &	pt
	)		`[private]`

Parameters:

stateTemplate tells the planner how the state it is searching on looks like (how many variables, etc).

See also:: void EigenGraspPlannerDlg::plannerInit_clicked()

Definition at line 944 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::initSearchType	(	GraspPlanningState &	stateTemplate,
		const GraspItStateType &	st
	)		`[private]`

See also:: void EigenGraspPlannerDlg::stopPlanner() Iniitalizes the search type to use. Note: Only SPACE_AXIS_ANGLE tested here so far.

Parameters:

stateTemplate tells the planner how the state it is searching on looks like (how many variables, etc). This object is initialized in this function according to the state type and the current hand and the object set to grasp.

See also:: void EigenGraspPlannerDlg::spaceSearchBox_activated( const QString &s )

Definition at line 892 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::ivIdleCallback ( ) [private]

Method which will be called at regular intervals from the thread which is also running the SoQt loop.

Definition at line 207 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::onSceneManagerShutdown ( ) [protected, virtual]

If this GraspItAccessor is registered with GraspItSceneManager (by a call of addAsIdleListener()), this method will be called from GraspItSceneManager when it shuts down.

Implements GraspIt::GraspItAccessor.

Definition at line 118 of file EigenGraspPlanner.cpp.

bool EigenGraspPlanner::plan	(	const int	maxPlanningSteps,
		const int	repeatPlanning,
		const int	maxResultsPerRepeat,
		const bool	finishWithAutograsp,
		const PlannerType &	planType = `SimAnn`
	)

Starts planning with the specified planner type. NOTE: Only simulated annealing is tested so far.

See also:: void EigenGraspPlannerDlg::startPlanner() and void EigenGraspPlannerDlg::plannerStart_clicked()

Parameters:

maxPlanningSteps	maximum steps (iterations) for the planning algorithm to use
planType	the type of planning algorithm to use. To this point, only simulated annealing is supported.
repeatPlanning	number of times the planning process should be repeated. This would in effect be the same as calling this method <repeatPlanning> times and then pick the best results.

Returns:: false if planner could not be initialized or if it failed.

Definition at line 311 of file EigenGraspPlanner.cpp.

bool EigenGraspPlanner::plan	(	const std::string &	handName,
		const std::string &	objectName,
		const EigenTransform *	objectPose,
		const int	maxPlanningSteps,
		const int	repeatPlanning,
		const int	maxResultsPerRepeat,
		const bool	finishWithAutograsp,
		const PlannerType &	planType = `SimAnn`
	)

Sets the current hand and object before calling plan(const int, const int). This is achieved by calling GraspItSceneManager::setCurrentHand(std::string&) and GraspItSceneManager::setCurrentGraspableObject(std::string&), so it involves checking if these objects are currently loaded in the GraspIt world. And if they are not, this method returns false.

Aside from providing the possibility to change current hand and object, it is also possible to specify a new object pose, if the parameter is not NULL (otherwise the current pose will be used).

This method is threadsafe in regards to world changes. So if thread safety is a concern, you should rather use this method rather than first calling GraspItSceneManager::setCurrentHand() and then GraspItSceneManager::setCurrentGraspableObject() before calling plan(const int, const int).

Parameters:

maxPlanningSteps	maximum steps (iterations) for the planning algorithm to use
planType	the type of planning algorithm to use. To this point, only simulated annealing is supported.
repeatPlanning	number of times the planning process should be repeated. This would in effect be the same as calling this method <repeatPlanning> times and then pick the best results.
maxResultsPerRepeat	keep the n best results of each planning run. This number multiplied by repeatPlanning is the maximum number of resulting grasps for a call fo the service.
finishWithAutograsp	Each result is 'finalized' with an additional Auto-Grasp, to ensure fingers are really closed around the object. Sometimes, a resulting grasp does not really touch the object, so setting this to true can help to ensure there are actual contact points.

Returns:: false if planner could not be initialized or if it failed.

Definition at line 258 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::plannerComplete ( ) [private]

To be called when the planning is finished to display the final results

See also:: void EigenGraspPlannerDlg::plannerComplete()

Definition at line 1047 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::plannerCompleteSlot ( ) [slot]

Qt slot which the EGPlanner class in the original graspit source can use to notify this class when the planning process is completed.

Definition at line 169 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::plannerReset ( ) [private]

See also:: void EigenGraspPlannerDlg::plannerReset_clicked()

Definition at line 1024 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::plannerUpdate ( ) [private]

To be called at regular intervals to display the current state of the planning process

See also:: void EigenGraspPlannerDlg::plannerUpdate()

Definition at line 1040 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::plannerUpdateSlot ( ) [slot]

Qt slot which the EGPlanner class in the original graspit source can use to notify this class about updates in the planning process.

Definition at line 164 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::printPlanningResults ( ) [private]

See also:: void EigenGraspPlannerDlg::bestGraspButton_clicked()

Definition at line 1162 of file EigenGraspPlanner.cpp.

void EigenGraspPlanner::printResult	(	int	i,
		bool	detailed = `false`
	)		`[private]`

Load an input grasp file (".txt") TODO TEST ME

See also:: void EigenGraspPlannerDlg::inputLoadButton_clicked() prints the result of the i'th grasp found. While the planning is ongoing, this method should be called from within the planner thread to ensure thread safety.

Definition at line 1093 of file EigenGraspPlanner.cpp.

bool EigenGraspPlanner::saveResultsAsWorldFiles	(	const std::string &	inDirectory,
		const std::string &	fileNamePrefix,
		bool	asGraspIt = `true`,
		bool	asInventor = `false`,
		bool	createDir = `true`,
		bool	saveSeparatePoseIV = `false`
	)

Saves the results as a GraspIt world files and/or inventor files in the given directory, with the given filename prefix. The files can be used to display the results with the original GraspIt simulator GUI or an Inventor files viewer.

Parameters:

createDir	if true, the directory in which the files are to be saved is created if it does not exist.
asGraspIt	if true, the world files will be saved in the GraspIt world format
asInventor	if true, the world files will be saved in the Inventor format. Saving as Inventor is slower than as GraspIt.
saveSeparatePoseIV	if true, (additional) inventor files will be saved with the robot poses (without the object).