Public Member Functions | Protected Member Functions | Protected Attributes
sbpl_interface::EnvironmentChain3D Class Reference

#include <environment_chain3d.h>

List of all members.

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 EnvChain3DPlanningDatagetPlanningData () const
const PlanningStatisticsgetPlanningStatistics () 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_3Dbfs_
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 95 of file environment_chain3d.h.


Constructor & Destructor Documentation

Default constructor.

Definition at line 45 of file environment_chain3d.cpp.

Destructor.

Definition at line 62 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:
stateIDof first state
stateIDof second state
Returns:
true if equivalent, false otherwise

Definition at line 378 of file environment_chain3d.cpp.

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

Definition at line 1023 of file environment_chain3d.cpp.

Definition at line 646 of file environment_chain3d.cpp.

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

Definition at line 316 of file environment_chain3d.h.

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

Definition at line 323 of file environment_chain3d.h.

Definition at line 612 of file environment_chain3d.cpp.

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

Definition at line 696 of file environment_chain3d.cpp.

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

Definition at line 783 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 306 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:
apointer to a vector of the poses of all of the states expanded during the search (when using ARA*)

Definition at line 200 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:
thestateID of the current state
thestateID of the goal state
Returns:
h(s,s')

Definition at line 90 of file environment_chain3d.cpp.

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

Parameters:
thestateID of the current state
Returns:
h(s,s_goal)

Definition at line 96 of file environment_chain3d.cpp.

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

Definition at line 232 of file environment_chain3d.h.

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

Definition at line 815 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 739 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 721 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 706 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 915 of file environment_chain3d.cpp.

Definition at line 216 of file environment_chain3d.h.

Definition at line 224 of file environment_chain3d.h.

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

Not defined.

Definition at line 372 of file environment_chain3d.cpp.

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

Parameters:
thestateID of the current state
Returns:
h(s,s_start)

Definition at line 106 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:
thestate ID of the state to be expanded
apointer to a vector that will be populated with the successor state IDs.
apointer to a vector that will be populated with the costs of transitioning from the current state to the successor state.

Definition at line 142 of file environment_chain3d.cpp.

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

Initialize the environment from a text file.

Parameters:
nameof environment text file
Returns:
true if successful, false otherwise

Definition at line 84 of file environment_chain3d.cpp.

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

Initialize the start and goal states of the MDP.

Parameters:
alwaysreturns true...

Definition at line 73 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 966 of file environment_chain3d.cpp.

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

Definition at line 834 of file environment_chain3d.cpp.

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

Not defined.

Definition at line 136 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:
thestate ID of the desired state
printsout a little extra information
thefile pointer to print to (stdout by default)

Definition at line 116 of file environment_chain3d.cpp.

Not defined.

Definition at line 384 of file environment_chain3d.cpp.

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

Not defined.

Definition at line 390 of file environment_chain3d.cpp.

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

Definition at line 591 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 400 of file environment_chain3d.cpp.

This function returns the number of hash entries created.

Returns:
number of hash entries

Definition at line 111 of file environment_chain3d.cpp.


Member Data Documentation

Definition at line 248 of file environment_chain3d.h.

Definition at line 249 of file environment_chain3d.h.

Definition at line 312 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 275 of file environment_chain3d.h.

Definition at line 261 of file environment_chain3d.h.

Definition at line 264 of file environment_chain3d.h.

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

Definition at line 257 of file environment_chain3d.h.

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

Definition at line 255 of file environment_chain3d.h.

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

Definition at line 254 of file environment_chain3d.h.

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

Definition at line 272 of file environment_chain3d.h.

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

Definition at line 273 of file environment_chain3d.h.

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

Definition at line 274 of file environment_chain3d.h.

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

Definition at line 269 of file environment_chain3d.h.

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

Definition at line 270 of file environment_chain3d.h.

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

Definition at line 271 of file environment_chain3d.h.

Definition at line 251 of file environment_chain3d.h.

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

Definition at line 256 of file environment_chain3d.h.

Definition at line 267 of file environment_chain3d.h.

Definition at line 262 of file environment_chain3d.h.

Definition at line 260 of file environment_chain3d.h.

Definition at line 263 of file environment_chain3d.h.

Definition at line 266 of file environment_chain3d.h.

Definition at line 246 of file environment_chain3d.h.

Definition at line 265 of file environment_chain3d.h.

Definition at line 252 of file environment_chain3d.h.

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

Definition at line 253 of file environment_chain3d.h.

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

Definition at line 259 of file environment_chain3d.h.


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


sbpl_interface
Author(s): Gil Jones
autogenerated on Mon Oct 6 2014 11:11:34