#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 ¶ms) |
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_ |
Environment to be used when planning for a Robotic Arm using the SBPL.
Definition at line 101 of file environment_chain3d.h.
sbpl_interface::EnvironmentChain3D::EnvironmentChain3D | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene | ) |
Default constructor.
Definition at line 51 of file environment_chain3d.cpp.
Destructor.
Definition at line 68 of file environment_chain3d.cpp.
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.
stateID | of first state |
stateID | of second state |
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.
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.
the | stateID of the current state |
the | stateID of the goal state |
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.
the | stateID of the current state |
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.
the | stateID of the current state |
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.
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.
name | of environment text file |
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.
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.
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.
Definition at line 117 of file environment_chain3d.cpp.
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.
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.
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.
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.