sbpl_arm_planner::EnvironmentROBARM3D Class Reference

#include <environment_robarm3d.h>

Public Member Functions
bool	AreEquivalent (int StateID1, int StateID2)
	Check if the states with StateID1 & StateID2 are equivalent based on an equivalency function with some declared tolerance.
void	debugAdaptiveMotionPrims ()
	For debugging. Print out the IK stats and return the IK solution found during the search.
std::vector< int >	debugExpandedStates ()
	This function returns a vector of all of the stateIDs of the states that were expanded during the search. (To be updated soon to return the coordinates of each state.
	EnvironmentROBARM3D ()
	Default constructor.
void	getArmChainRootLinkName (std::string &name)
SBPLCollisionSpace *	getCollisionSpace () const
bool	getElbowCellsAtGoal (std::vector< int > &shoulder, std::vector< double > &goal_m, double rad1, double rad2, std::vector< std::vector< int > > &cells)
	compute possible elbow positions based on end effector goal
void	getElbowPoints (std::vector< std::vector< double > > &elbow_points)
double	getEpsilon ()
	Get the epsilon value used by the planner. Epsilon is a bounds on the suboptimality allowed by the planner.
void	getExpandedStates (std::vector< std::vector< double > > *ara_states)
	This function is for debugging purposes. It returns the pose of the states that were expanded. The planner node has a function to display these as visualizations in rviz.
int	GetFromToHeuristic (int FromStateID, int ToStateID)
	Get the heuristic from one state to another state.
int	GetGoalHeuristic (int stateID)
	Get the heuristic of a state to the planner's goal state.
OccupancyGrid *	getOccupancyGrid () const
std::vector< double >	getPlanningStats ()
void	GetPreds (int TargetStateID, vector< int > PredIDV, vector< int > CostV)
	Not defined.
std::vector< std::vector < double > >	getShortestPath ()
	This function returns the shortest path to the goal from the starting state. If the dijkstra heuristic is enabled, then it returns the shortest path solved for by dijkstra's algorithm. If it is disabled then it returns the straight line path to the goal.
int	GetStartHeuristic (int stateID)
	Get the heuristic of a state to the planner's start state.
void	GetSuccs (int SourceStateID, vector< int > SuccIDV, vector< int > CostV)
	Get the successors of the desired state to be expanded. Return vectors with the successors' state IDs and the cost to move from the current state to that state. If the vectors return to the planner empty then the search quits.
bool	initArmModel (FILE *aCfg, const std::string robot_description)
bool	initEnvironment (std::string arm_description_filename, std::string mprims_filename)
	Initialize the environment and arm planner parameters from text files. Also, initialize KDL chain from a URDF file.
bool	InitializeEnv (const char *sEnvFile, std::string params_file, std::string arm_file)
bool	InitializeEnv (const char *sEnvFile)
	Initialize the environment from a text file.
bool	InitializeMDPCfg (MDPConfig *MDPCfg)
	Initialize the start and goal states of the MDP.
bool	interpolatePathToGoal (std::vector< std::vector< double > > &path, double inc)
	interpolate the path between the waypoints in path by desired increment
bool	isPathValid (const std::vector< std::vector< double > > path)
	Check if the path is valid using the current occupancy grid.
bool	isValidJointConfiguration (const std::vector< double > angles)
	Check if a certain joint configuration is valid using the current occupancy grid.
void	PrintEnv_Config (FILE *fOut)
	Not defined.
void	PrintState (int stateID, bool bVerbose, FILE *fOut=NULL)
	This function prints out the state information of a state.
void	SetAllActionsandAllOutcomes (CMDPSTATE *state)
	Not defined.
void	SetAllPreds (CMDPSTATE *state)
	Not defined.
bool	setGoalPosition (const std::vector< std::vector< double > > &goals, const std::vector< std::vector< double > > &tolerances)
	This function sets the goal position. It must be called before starting a new search.
void	setReferenceFrameTransform (KDL::Frame f, std::string &name)
bool	setStartConfiguration (std::vector< double > angles)
	Set the initial joint configuration of the manipulator. Needs to be set every time the planner is called.
int	SizeofCreatedEnv ()
	This function returns the number of hash entries created.
void	StateID2Angles (int stateID, std::vector< double > &angles)
	This function searches the hash table for a state by a stateID and returns the joint angles of that entry.
void	updateOccupancyGridFromCollisionMap (const mapping_msgs::CollisionMap &collision_map)
void	visualizeOccupancyGrid ()
	Visualize the occupancy grid in rviz.
	~EnvironmentROBARM3D ()
	Destructor.
Public Attributes
std::vector< int >	expanded_states
bool	save_expanded_states
Private Member Functions
void	anglesToCoord (const std::vector< double > &angle, std::vector< short unsigned int > &coord)
void	clearStats ()
void	computeCostPerCell ()
void	computeCostPerRadian ()
void	coordToAngles (const std::vector< short unsigned int > &coord, std::vector< double > &angles)
int	cost (EnvROBARM3DHashEntry_t HashEntry1, EnvROBARM3DHashEntry_t HashEntry2, bool bState2IsGoal)
EnvROBARM3DHashEntry_t *	createHashEntry (const std::vector< short unsigned int > &coord, short unsigned int endeff[3], short unsigned int action)
void	discretizeAngles ()
int	getActionCost (const std::vector< double > &from_config, const std::vector< double > &to_config, int dist)
void	getBresenhamPath (const short unsigned int a[], const short unsigned int b[], std::vector< std::vector< int > > *path)
int	getCombinedHeuristic (int FromStateID, int ToStateID)
int	getDijkstraDistToGoal (short unsigned int x, short unsigned int y, short unsigned int z) const
double	getDistToClosestGoal (double xyz, int goal_num)
int	getEdgeCost (int FromStateID, int ToStateID)
int	getElbowHeuristic (int FromStateID, int ToStateID)
int	getEndEffectorHeuristic (int FromStateID, int ToStateID)
int	getEuclideanDistance (int x1, int y1, int z1, int x2, int y2, int z2) const
unsigned int	getHashBin (const std::vector< short unsigned int > &coord)
EnvROBARM3DHashEntry_t *	getHashEntry (const std::vector< short unsigned int > &coord, short unsigned int action, bool bIsGoal)
void	initDijkstra ()
void	initElbowDijkstra ()
bool	initEnvConfig ()
bool	initGeneral ()
unsigned int	intHash (unsigned int key)
bool	isGoalPosition (const std::vector< double > &pose, const GoalPos &goal, std::vector< double > jnt_angles, int &cost)
bool	isGoalStateWithIK (const std::vector< double > &pose, const GoalPos &goal, std::vector< double > jnt_angles)
bool	isGoalStateWithOrientationSolver (const GoalPos &goal, std::vector< double > jnt_angles)
bool	precomputeElbowHeuristic ()
bool	precomputeHeuristics ()
void	printHashTableHist ()
void	printJointArray (FILE fOut, EnvROBARM3DHashEntry_t HashEntry, bool bGoal, bool bVerbose)
void	readConfiguration (FILE *fCfg)
Private Attributes
SBPLArmModel *	arm_
std::string	arm_desc_filename_
SBPLCollisionSpace *	cspace_
BFS3D *	dijkstra_
std::vector< std::vector< int > >	elbow_cells_
BFS3D *	elbow_dijkstra_
bool	elbow_heuristic_completed_
std::vector< std::vector < double > >	elbow_poses_
EnvironmentROBARM3D_t	EnvROBARM
EnvROBARM3DConfig_t	EnvROBARMCfg
std::vector< double >	final_joint_config
int(EnvironmentROBARM3D::*	getHeuristic_ )(int FromStateID, int ToStateID)
OccupancyGrid *	grid_
boost::mutex	heuristic_mutex_
boost::thread *	heuristic_thread_
std::string	params_filename_
std::vector< double >	prefinal_joint_config
SBPLArmPlannerParams	prms_
RPYSolver *	rpysolver_
bool	using_short_mprims_

Detailed Description

Environment to be used when planning for a Robotic Arm using the SBPL.

Definition at line 132 of file environment_robarm3d.h.

Constructor & Destructor Documentation

sbpl_arm_planner::EnvironmentROBARM3D::EnvironmentROBARM3D ( )

Default constructor.

Definition at line 68 of file environment_robarm3d.cpp.

sbpl_arm_planner::EnvironmentROBARM3D::~EnvironmentROBARM3D ( )

Destructor.

Definition at line 92 of file environment_robarm3d.cpp.

Member Function Documentation

void sbpl_arm_planner::EnvironmentROBARM3D::anglesToCoord	(	const std::vector< double > &	angle,
		std::vector< short unsigned int > &	coord
	)			`[inline, private]`

Definition at line 476 of file environment_robarm3d.h.

bool sbpl_arm_planner::EnvironmentROBARM3D::AreEquivalent	(	int	StateID1,
		int	StateID2
	)

Check if the states with StateID1 & StateID2 are equivalent based on an equivalency function with some declared tolerance.

Parameters:

	stateID	of first state
	stateID	of second state

Returns:: true if equivalent, false otherwise

Definition at line 460 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::clearStats ( ) [private]

Definition at line 1347 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::computeCostPerCell ( ) [private]

Definition at line 1453 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::computeCostPerRadian ( ) [private]

void sbpl_arm_planner::EnvironmentROBARM3D::coordToAngles	(	const std::vector< short unsigned int > &	coord,
		std::vector< double > &	angles
	)			`[inline, private]`

Definition at line 470 of file environment_robarm3d.h.

int sbpl_arm_planner::EnvironmentROBARM3D::cost	(	EnvROBARM3DHashEntry_t *	HashEntry1,
		EnvROBARM3DHashEntry_t *	HashEntry2,
		bool	bState2IsGoal
	)			`[private]`

costs

Definition at line 633 of file environment_robarm3d.cpp.

EnvROBARM3DHashEntry_t * sbpl_arm_planner::EnvironmentROBARM3D::createHashEntry	(	const std::vector< short unsigned int > &	coord,
		short unsigned int	endeff[3],
		short unsigned int	action
	)			`[private]`

Definition at line 549 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::debugAdaptiveMotionPrims ( )

For debugging. Print out the IK stats and return the IK solution found during the search.

Definition at line 1680 of file environment_robarm3d.cpp.

std::vector< int > sbpl_arm_planner::EnvironmentROBARM3D::debugExpandedStates ( )

This function returns a vector of all of the stateIDs of the states that were expanded during the search. (To be updated soon to return the coordinates of each state.

Returns:: a vector of state IDs

Definition at line 1434 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::discretizeAngles ( ) [private]

coordinate frame/angle functions

Definition at line 624 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::getActionCost	(	const std::vector< double > &	from_config,
		const std::vector< double > &	to_config,
		int	dist
	)			`[private]`

Definition at line 1019 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::getArmChainRootLinkName ( std::string & name )

Definition at line 1726 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::getBresenhamPath	(	const short unsigned int	a[],
		const short unsigned int	b[],
		std::vector< std::vector< int > > *	path
	)			`[private]`

Definition at line 1664 of file environment_robarm3d.cpp.

SBPLCollisionSpace * sbpl_arm_planner::EnvironmentROBARM3D::getCollisionSpace ( ) const [inline]

Definition at line 500 of file environment_robarm3d.h.

int sbpl_arm_planner::EnvironmentROBARM3D::getCombinedHeuristic	(	int	FromStateID,
		int	ToStateID
	)			`[private]`

Definition at line 1943 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::getDijkstraDistToGoal	(	short unsigned int	x,
		short unsigned int	y,
		short unsigned int	z
	)			const `[private]`

distance

Definition at line 1516 of file environment_robarm3d.cpp.

double sbpl_arm_planner::EnvironmentROBARM3D::getDistToClosestGoal	(	double *	xyz,
		int *	goal_num
	)			`[private]`

Definition at line 1495 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::getEdgeCost	(	int	FromStateID,
		int	ToStateID
	)			`[private]`

Definition at line 1050 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::getElbowCellsAtGoal	(	std::vector< int > &	shoulder,
		std::vector< double > &	goal_m,
		double	rad1,
		double	rad2,
		std::vector< std::vector< int > > &	cells
	)

compute possible elbow positions based on end effector goal

Parameters:

	cell	of shoulder position {x,y,z}
	end	effector goal coordinates {x, y, z}
	radius	of upper arm link in meters
	radius	of fore arm link in meters
	list	of possible elbow cells {{x,y,z}, {x,y,z}...}

Definition at line 1808 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::getElbowHeuristic	(	int	FromStateID,
		int	ToStateID
	)			`[private]`

Definition at line 1892 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::getElbowPoints ( std::vector< std::vector< double > > & elbow_points )

Definition at line 1964 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::getEndEffectorHeuristic	(	int	FromStateID,
		int	ToStateID
	)			`[private]`

Definition at line 1917 of file environment_robarm3d.cpp.

double sbpl_arm_planner::EnvironmentROBARM3D::getEpsilon ( )

Get the epsilon value used by the planner. Epsilon is a bounds on the suboptimality allowed by the planner.

Returns:: epsilon

Definition at line 804 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::getEuclideanDistance	(	int	x1,
		int	y1,
		int	z1,
		int	x2,
		int	y2,
		int	z2
	)			const `[private]`

Definition at line 495 of file environment_robarm3d.h.

void sbpl_arm_planner::EnvironmentROBARM3D::getExpandedStates ( std::vector< std::vector< double > > * ara_states )

This function is for debugging purposes. It returns the pose of the states that were expanded. The planner node has a function to display these as visualizations in rviz.

Parameters:

a

pointer to a vector of the poses of all of the states expanded during the search (when using ARA*)

Definition at line 1439 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::GetFromToHeuristic	(	int	FromStateID,
		int	ToStateID
	)

Get the heuristic from one state to another state.

Parameters:

	the	stateID of the current state
	the	stateID of the goal state

Returns:: h(s,s')

Definition at line 237 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::GetGoalHeuristic ( int stateID )

Get the heuristic of a state to the planner's goal state.

Parameters:

the

stateID of the current state

Returns:: h(s,s_goal)

Definition at line 242 of file environment_robarm3d.cpp.

unsigned int sbpl_arm_planner::EnvironmentROBARM3D::getHashBin ( const std::vector< short unsigned int > & coord ) [inline, private]

Definition at line 459 of file environment_robarm3d.h.

EnvROBARM3DHashEntry_t * sbpl_arm_planner::EnvironmentROBARM3D::getHashEntry	(	const std::vector< short unsigned int > &	coord,
		short unsigned int	action,
		bool	bIsGoal
	)			`[private]`

Definition at line 507 of file environment_robarm3d.cpp.

OccupancyGrid * sbpl_arm_planner::EnvironmentROBARM3D::getOccupancyGrid ( ) const [inline]

Definition at line 505 of file environment_robarm3d.h.

std::vector< double > sbpl_arm_planner::EnvironmentROBARM3D::getPlanningStats ( )

Definition at line 1731 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::GetPreds	(	int	TargetStateID,
		vector< int > *	PredIDV,
		vector< int > *	CostV
	)

Not defined.

Definition at line 454 of file environment_robarm3d.cpp.

std::vector< std::vector< double > > sbpl_arm_planner::EnvironmentROBARM3D::getShortestPath ( )

This function returns the shortest path to the goal from the starting state. If the dijkstra heuristic is enabled, then it returns the shortest path solved for by dijkstra's algorithm. If it is disabled then it returns the straight line path to the goal.

Returns:: a 2D vector of waypoints {{x1,y1,z1},{x2,y2,z2},...}

Definition at line 1623 of file environment_robarm3d.cpp.

int sbpl_arm_planner::EnvironmentROBARM3D::GetStartHeuristic ( int stateID )

Get the heuristic of a state to the planner's start state.

Parameters:

the

stateID of the current state

Returns:: h(s,s_start)

Definition at line 255 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::GetSuccs	(	int	SourceStateID,
		vector< int > *	SuccIDV,
		vector< int > *	CostV
	)

Get the successors of the desired state to be expanded. Return vectors with the successors' state IDs and the cost to move from the current state to that state. If the vectors return to the planner empty then the search quits.

Parameters:

	the	state ID of the state to be expanded
	a	pointer to a vector that will be populated with the successor state IDs.
	a	pointer to a vector that will be populated with the costs of transitioning from the current state to the successor state.

Definition at line 309 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::initArmModel	(	FILE *	aCfg,
		const std::string	robot_description
	)

Definition at line 685 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::initDijkstra ( ) [private]

Definition at line 591 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::initElbowDijkstra ( ) [private]

Definition at line 607 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::initEnvConfig ( ) [private]

initialization

Definition at line 655 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::initEnvironment	(	std::string	arm_description_filename,
		std::string	mprims_filename
	)

Initialize the environment and arm planner parameters from text files. Also, initialize KDL chain from a URDF file.

Parameters:

	is	pointer to file describing the environment
	is	a pointer to file with the Arm planner parameters
	is	a URDF describing the manipulator

Returns:: true if successful, false otherwise

Definition at line 705 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::initGeneral ( ) [private]

Definition at line 745 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::InitializeEnv	(	const char *	sEnvFile,
		std::string	params_file,
		std::string	arm_file
	)

Definition at line 142 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::InitializeEnv ( const char * sEnvFile )

Initialize the environment from a text file.

Parameters:

name

of environment text file

Returns:: true if successful, false otherwise

Definition at line 150 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::InitializeMDPCfg ( MDPConfig * MDPCfg )

Initialize the start and goal states of the MDP.

Parameters:

always

returns true...

Definition at line 134 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::interpolatePathToGoal	(	std::vector< std::vector< double > > &	path,
		double	inc
	)

interpolate the path between the waypoints in path by desired increment

Parameters:

	path
	increment	to interpolate by

Definition at line 1558 of file environment_robarm3d.cpp.

unsigned int sbpl_arm_planner::EnvironmentROBARM3D::intHash ( unsigned int key ) [inline, private]

hash table

Definition at line 446 of file environment_robarm3d.h.

bool sbpl_arm_planner::EnvironmentROBARM3D::isGoalPosition	(	const std::vector< double > &	pose,
		const GoalPos &	goal,
		std::vector< double >	jnt_angles,
		int &	cost
	)			`[private]`

planning

Definition at line 883 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::isGoalStateWithIK	(	const std::vector< double > &	pose,
		const GoalPos &	goal,
		std::vector< double >	jnt_angles
	)			`[private]`

Definition at line 940 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::isGoalStateWithOrientationSolver	(	const GoalPos &	goal,
		std::vector< double >	jnt_angles
	)			`[private]`

Definition at line 1068 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::isPathValid ( const std::vector< std::vector< double > > path )

Check if the path is valid using the current occupancy grid.

Parameters:

a

2D vector of joint angles

Returns:: true if successful, false otherwise

Definition at line 1542 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::isValidJointConfiguration ( const std::vector< double > angles )

Check if a certain joint configuration is valid using the current occupancy grid.

Parameters:

a

vector of joint angles

Returns:: true if successful, false otherwise

Definition at line 1536 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::precomputeElbowHeuristic ( ) [private]

Definition at line 1286 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::precomputeHeuristics ( ) [private]

Definition at line 1224 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::PrintEnv_Config ( FILE * fOut )

Not defined.

Definition at line 301 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::printHashTableHist ( ) [private]

output

Definition at line 482 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::printJointArray	(	FILE *	fOut,
		EnvROBARM3DHashEntry_t *	HashEntry,
		bool	bGoal,
		bool	bVerbose
	)			`[private]`

Definition at line 1407 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::PrintState	(	int	stateID,
		bool	bVerbose,
		FILE *	fOut = `NULL`
	)

This function prints out the state information of a state.

Parameters:

	the	state ID of the desired state
	prints	out a little extra information
	the	file pointer to print to (stdout by default)

Definition at line 273 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::readConfiguration ( FILE * fCfg ) [private]

Definition at line 809 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::SetAllActionsandAllOutcomes ( CMDPSTATE * state )

Not defined.

Definition at line 466 of file environment_robarm3d.cpp.

void sbpl_arm_planner::EnvironmentROBARM3D::SetAllPreds ( CMDPSTATE * state )

Not defined.

Definition at line 472 of file environment_robarm3d.cpp.

bool sbpl_arm_planner::EnvironmentROBARM3D::setGoalPosition	(	const std::vector< std::vector< double > > &	goals,
		const std::vector< std::vector< double > > &	tolerances
	)

This function sets the goal position. It must be called before starting a new search.

Parameters:

	a	2D vector of pose constraints {{x1,y1,z1,r1,p1,y1},...}
	a	2D vector of tolerances on the pose constraints {{allowed_err_meters1,allowed_error_radians1},...}