#include <environment_chain3d.h>

Public Member Functions
virtual 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	attemptShortcut (const trajectory_msgs::JointTrajectory &traj_in, trajectory_msgs::JointTrajectory &traj_out)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	EnvironmentChain3D (const planning_scene::PlanningSceneConstPtr &planning_scene)
	Default constructor.
virtual 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.
virtual int	GetFromToHeuristic (int FromStateID, int ToStateID)
	Get the heuristic from one state to another state.
virtual int	GetGoalHeuristic (int stateID)
	Get the heuristic of a state to the planner's goal state.
const Eigen::Affine3d &	getGoalPose () const
bool	getPlaneBFSMarker (visualization_msgs::Marker &plane_marker, double z_val)
const EnvChain3DPlanningData &	getPlanningData () const
const PlanningStatistics &	getPlanningStatistics () const
virtual void	GetPreds (int TargetStateID, vector< int > PredIDV, vector< int > CostV)
	Not defined.
virtual int	GetStartHeuristic (int stateID)
	Get the heuristic of a state to the planner's start state.
virtual 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.
virtual bool	InitializeEnv (const char *sEnvFile)
	Initialize the environment from a text file.
virtual bool	InitializeMDPCfg (MDPConfig *MDPCfg)
	Initialize the start and goal states of the MDP.
bool	populateTrajectoryFromStateIDSequence (const std::vector< int > &state_ids, trajectory_msgs::JointTrajectory &traj) const
virtual void	PrintEnv_Config (FILE *fOut)
	Not defined.
virtual void	PrintState (int stateID, bool bVerbose, FILE *fOut=NULL)
	This function prints out the state information of a state.
virtual void	SetAllActionsandAllOutcomes (CMDPSTATE *state)
	Not defined.
virtual void	SetAllPreds (CMDPSTATE *state)
	Not defined.
bool	setupForMotionPlan (const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res, const PlanningParameters &params)
virtual int	SizeofCreatedEnv ()
	This function returns the number of hash entries created.
	~EnvironmentChain3D ()
	Destructor.
Protected Member Functions
int	calculateCost (EnvChain3DHashEntry HashEntry1, EnvChain3DHashEntry HashEntry2)
void	convertCoordToJointAngles (const std::vector< int > &coord, std::vector< double > &angles)
void	convertJointAnglesToCoord (const std::vector< double > &angle, std::vector< int > &coord)
void	determineMaximumEndEffectorTravel ()
int	getBFSCostToGoal (int x, int y, int z) const
int	getEndEffectorHeuristic (int FromStateID, int ToStateID)
double	getEuclideanDistance (double x1, double y1, double z1, double x2, double y2, double z2) const
bool	getGridXYZInt (const Eigen::Affine3d &pose, int(&xyz)[3]) const
double	getJointDistanceDoubleSum (const std::vector< double > &angles1, const std::vector< double > &angles2) const
int	getJointDistanceIntegerMax (const std::vector< double > &angles1, const std::vector< double > &angles2, double delta) const
int	getJointDistanceIntegerSum (const std::vector< double > &angles1, const std::vector< double > &angles2, double delta) const
void	getMotionPrimitives (const std::string &group)
bool	interpolateAndCollisionCheck (const std::vector< double > angles1, const std::vector< double > angles2, std::vector< std::vector< double > > &state_values)
void	setMotionPrimitives (const std::string &group_name)
Protected Attributes
double	angle_discretization_
BFS_3D *	bfs_
double	closest_to_goal_
std::map< int, std::map< int, std::vector< std::vector < double > > > >	generated_interpolations_map_
kinematic_constraints::KinematicConstraintSet	goal_constraint_set_
Eigen::Affine3d	goal_pose_
boost::shared_ptr < collision_detection::GroupStateRepresentation >	gsr_
const collision_detection::CollisionRobotHybrid *	hy_robot_
const collision_detection::CollisionWorldHybrid *	hy_world_
planning_models::RobotState ::JointStateGroup	interpolation_joint_state_group_1_
planning_models::RobotState ::JointStateGroup	interpolation_joint_state_group_2_
planning_models::RobotState ::JointStateGroup	interpolation_joint_state_group_temp_
planning_models::RobotState *	interpolation_state_1_
planning_models::RobotState *	interpolation_state_2_
planning_models::RobotState *	interpolation_state_temp_
std::vector< boost::shared_ptr < JointMotionWrapper > >	joint_motion_wrappers_
planning_models::RobotState ::JointStateGroup	joint_state_group_
int	maximum_distance_for_motion_
kinematic_constraints::KinematicConstraintSet	path_constraint_set_
EnvChain3DPlanningData	planning_data_
std::string	planning_group_
PlanningParameters	planning_parameters_
planning_scene::PlanningSceneConstPtr	planning_scene_
PlanningStatistics	planning_statistics_
std::vector< boost::shared_ptr < JointMotionPrimitive > >	possible_actions_
planning_models::RobotState *	state_
const planning_models::RobotState ::LinkState	tip_link_state_

Detailed Description

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

Definition at line 101 of file environment_chain3d.h.

Constructor & Destructor Documentation

sbpl_interface::EnvironmentChain3D::EnvironmentChain3D ( const planning_scene::PlanningSceneConstPtr & planning_scene )

Default constructor.

Definition at line 51 of file environment_chain3d.cpp.

sbpl_interface::EnvironmentChain3D::~EnvironmentChain3D ( )

Destructor.

Definition at line 68 of file environment_chain3d.cpp.

Member Function Documentation

bool sbpl_interface::EnvironmentChain3D::AreEquivalent	(	int	StateID1,
		int	StateID2
	)		`[virtual]`

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 384 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::attemptShortcut	(	const trajectory_msgs::JointTrajectory &	traj_in,
		trajectory_msgs::JointTrajectory &	traj_out
	)

Definition at line 1029 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::calculateCost	(	EnvChain3DHashEntry *	HashEntry1,
		EnvChain3DHashEntry *	HashEntry2
	)		`[protected]`

Definition at line 652 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::convertCoordToJointAngles	(	const std::vector< int > &	coord,
		std::vector< double > &	angles
	)		`[inline, protected]`

Definition at line 322 of file environment_chain3d.h.

void sbpl_interface::EnvironmentChain3D::convertJointAnglesToCoord	(	const std::vector< double > &	angle,
		std::vector< int > &	coord
	)		`[inline, protected]`

Definition at line 329 of file environment_chain3d.h.

void sbpl_interface::EnvironmentChain3D::determineMaximumEndEffectorTravel ( ) [protected]

Definition at line 618 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::getBFSCostToGoal	(	int	x,
		int	y,
		int	z
	)		const `[protected]`

Definition at line 702 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::getEndEffectorHeuristic	(	int	FromStateID,
		int	ToStateID
	)		`[protected]`

Definition at line 789 of file environment_chain3d.cpp.

double sbpl_interface::EnvironmentChain3D::getEuclideanDistance	(	double	x1,
		double	y1,
		double	z1,
		double	x2,
		double	y2,
		double	z2
	)		const `[inline, protected]`

Definition at line 312 of file environment_chain3d.h.

virtual void sbpl_interface::EnvironmentChain3D::getExpandedStates ( std::vector< std::vector< double > > * ara_states ) [inline, virtual]

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 206 of file environment_chain3d.h.

int sbpl_interface::EnvironmentChain3D::GetFromToHeuristic	(	int	FromStateID,
		int	ToStateID
	)		`[virtual]`

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 96 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::GetGoalHeuristic ( int stateID ) [virtual]

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 102 of file environment_chain3d.cpp.

const Eigen::Affine3d& sbpl_interface::EnvironmentChain3D::getGoalPose ( ) const [inline]

Definition at line 238 of file environment_chain3d.h.

bool sbpl_interface::EnvironmentChain3D::getGridXYZInt	(	const Eigen::Affine3d &	pose,
		int(&)	xyz[3]
	)		const `[protected]`

Definition at line 821 of file environment_chain3d.cpp.

double sbpl_interface::EnvironmentChain3D::getJointDistanceDoubleSum	(	const std::vector< double > &	angles1,
		const std::vector< double > &	angles2
	)		const `[protected]`

Definition at line 745 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::getJointDistanceIntegerMax	(	const std::vector< double > &	angles1,
		const std::vector< double > &	angles2,
		double	delta
	)		const `[protected]`

Definition at line 727 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::getJointDistanceIntegerSum	(	const std::vector< double > &	angles1,
		const std::vector< double > &	angles2,
		double	delta
	)		const `[protected]`

Definition at line 712 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::getMotionPrimitives ( const std::string & group ) [protected]

bool sbpl_interface::EnvironmentChain3D::getPlaneBFSMarker	(	visualization_msgs::Marker &	plane_marker,
		double	z_val
	)

Definition at line 921 of file environment_chain3d.cpp.

const EnvChain3DPlanningData& sbpl_interface::EnvironmentChain3D::getPlanningData ( ) const [inline]

Definition at line 222 of file environment_chain3d.h.

const PlanningStatistics& sbpl_interface::EnvironmentChain3D::getPlanningStatistics ( ) const [inline]

Definition at line 230 of file environment_chain3d.h.

void sbpl_interface::EnvironmentChain3D::GetPreds	(	int	TargetStateID,
		vector< int > *	PredIDV,
		vector< int > *	CostV
	)		`[virtual]`

Not defined.

Definition at line 378 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::GetStartHeuristic ( int stateID ) [virtual]

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 112 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::GetSuccs	(	int	SourceStateID,
		vector< int > *	SuccIDV,
		vector< int > *	CostV
	)		`[virtual]`

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 148 of file environment_chain3d.cpp.

bool sbpl_interface::EnvironmentChain3D::InitializeEnv ( const char * sEnvFile ) [virtual]

Initialize the environment from a text file.

Parameters:

name	of environment text file

Returns:: true if successful, false otherwise

Definition at line 90 of file environment_chain3d.cpp.

bool sbpl_interface::EnvironmentChain3D::InitializeMDPCfg ( MDPConfig * MDPCfg ) [virtual]

Initialize the start and goal states of the MDP.

Parameters:

always returns true...

Definition at line 79 of file environment_chain3d.cpp.

bool sbpl_interface::EnvironmentChain3D::interpolateAndCollisionCheck	(	const std::vector< double >	angles1,
		const std::vector< double >	angles2,
		std::vector< std::vector< double > > &	state_values
	)		`[protected]`

Definition at line 972 of file environment_chain3d.cpp.

bool sbpl_interface::EnvironmentChain3D::populateTrajectoryFromStateIDSequence	(	const std::vector< int > &	state_ids,
		trajectory_msgs::JointTrajectory &	traj
	)		const

Definition at line 840 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::PrintEnv_Config ( FILE * fOut ) [virtual]

Not defined.

Definition at line 142 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::PrintState	(	int	stateID,
		bool	bVerbose,
		FILE *	fOut = `NULL`
	)		`[virtual]`

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 122 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::SetAllActionsandAllOutcomes ( CMDPSTATE * state ) [virtual]

Not defined.

Definition at line 390 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::SetAllPreds ( CMDPSTATE * state ) [virtual]

Not defined.

Definition at line 396 of file environment_chain3d.cpp.

void sbpl_interface::EnvironmentChain3D::setMotionPrimitives ( const std::string & group_name ) [protected]

Definition at line 597 of file environment_chain3d.cpp.

bool sbpl_interface::EnvironmentChain3D::setupForMotionPlan	(	const planning_scene::PlanningSceneConstPtr &	planning_scene,
		const moveit_msgs::GetMotionPlan::Request &	req,
		moveit_msgs::GetMotionPlan::Response &	res,
		const PlanningParameters &	params
	)

Definition at line 406 of file environment_chain3d.cpp.

int sbpl_interface::EnvironmentChain3D::SizeofCreatedEnv ( ) [virtual]

This function returns the number of hash entries created.

Returns:: number of hash entries

Definition at line 117 of file environment_chain3d.cpp.

Member Data Documentation

double sbpl_interface::EnvironmentChain3D::angle_discretization_ [protected]

Definition at line 254 of file environment_chain3d.h.

BFS_3D* sbpl_interface::EnvironmentChain3D::bfs_ [protected]

Definition at line 255 of file environment_chain3d.h.

double sbpl_interface::EnvironmentChain3D::closest_to_goal_ [protected]

Definition at line 318 of file environment_chain3d.h.

std::map<int, std::map<int, std::vector<std::vector<double> > > > sbpl_interface::EnvironmentChain3D::generated_interpolations_map_ [protected]

Definition at line 281 of file environment_chain3d.h.

kinematic_constraints::KinematicConstraintSet sbpl_interface::EnvironmentChain3D::goal_constraint_set_ [protected]

Definition at line 267 of file environment_chain3d.h.

Eigen::Affine3d sbpl_interface::EnvironmentChain3D::goal_pose_ [protected]

Definition at line 270 of file environment_chain3d.h.

boost::shared_ptr<collision_detection::GroupStateRepresentation> sbpl_interface::EnvironmentChain3D::gsr_ [protected]

Definition at line 263 of file environment_chain3d.h.

const collision_detection::CollisionRobotHybrid* sbpl_interface::EnvironmentChain3D::hy_robot_ [protected]

Definition at line 261 of file environment_chain3d.h.

const collision_detection::CollisionWorldHybrid* sbpl_interface::EnvironmentChain3D::hy_world_ [protected]

Definition at line 260 of file environment_chain3d.h.

planning_models::RobotState* ::JointStateGroup* sbpl_interface::EnvironmentChain3D::interpolation_joint_state_group_1_ [protected]

Definition at line 278 of file environment_chain3d.h.

planning_models::RobotState* ::JointStateGroup* sbpl_interface::EnvironmentChain3D::interpolation_joint_state_group_2_ [protected]

Definition at line 279 of file environment_chain3d.h.

planning_models::RobotState* ::JointStateGroup* sbpl_interface::EnvironmentChain3D::interpolation_joint_state_group_temp_ [protected]

Definition at line 280 of file environment_chain3d.h.

planning_models::RobotState* sbpl_interface::EnvironmentChain3D::interpolation_state_1_ [protected]

Definition at line 275 of file environment_chain3d.h.

planning_models::RobotState* sbpl_interface::EnvironmentChain3D::interpolation_state_2_ [protected]

Definition at line 276 of file environment_chain3d.h.

planning_models::RobotState* sbpl_interface::EnvironmentChain3D::interpolation_state_temp_ [protected]

Definition at line 277 of file environment_chain3d.h.

std::vector<boost::shared_ptr<JointMotionWrapper> > sbpl_interface::EnvironmentChain3D::joint_motion_wrappers_ [protected]

Definition at line 257 of file environment_chain3d.h.

planning_models::RobotState* ::JointStateGroup* sbpl_interface::EnvironmentChain3D::joint_state_group_ [protected]

Definition at line 262 of file environment_chain3d.h.

int sbpl_interface::EnvironmentChain3D::maximum_distance_for_motion_ [protected]

Definition at line 273 of file environment_chain3d.h.

kinematic_constraints::KinematicConstraintSet sbpl_interface::EnvironmentChain3D::path_constraint_set_ [protected]

Definition at line 268 of file environment_chain3d.h.

EnvChain3DPlanningData sbpl_interface::EnvironmentChain3D::planning_data_ [protected]

Definition at line 266 of file environment_chain3d.h.

std::string sbpl_interface::EnvironmentChain3D::planning_group_ [protected]

Definition at line 269 of file environment_chain3d.h.

PlanningParameters sbpl_interface::EnvironmentChain3D::planning_parameters_ [protected]

Definition at line 272 of file environment_chain3d.h.

planning_scene::PlanningSceneConstPtr sbpl_interface::EnvironmentChain3D::planning_scene_ [protected]

Definition at line 252 of file environment_chain3d.h.

PlanningStatistics sbpl_interface::EnvironmentChain3D::planning_statistics_ [protected]

Definition at line 271 of file environment_chain3d.h.

std::vector<boost::shared_ptr<JointMotionPrimitive> > sbpl_interface::EnvironmentChain3D::possible_actions_ [protected]

Definition at line 258 of file environment_chain3d.h.

planning_models::RobotState* sbpl_interface::EnvironmentChain3D::state_ [protected]

Definition at line 259 of file environment_chain3d.h.

const planning_models::RobotState* ::LinkState* sbpl_interface::EnvironmentChain3D::tip_link_state_ [protected]

Definition at line 265 of file environment_chain3d.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation