A class defining a footstep planner environment for humanoid robots used by the SBPL to perform planning tasks. More...
#include <FootstepPlannerEnvironment.h>
Classes | |
struct | IntPairHash |
struct | less |
< operator for planning states. More... | |
Public Types | |
typedef exp_states_2d_t::const_iterator | exp_states_2d_iter_t |
typedef std::tr1::unordered_set< std::pair< int, int >, IntPairHash > | exp_states_2d_t |
typedef exp_states_t::const_iterator | exp_states_iter_t |
typedef std::vector< int > | exp_states_t |
Public Member Functions | |
bool | AreEquivalent (int StateID1, int StateID2) |
FootstepPlannerEnvironment (const environment_params ¶ms) | |
exp_states_2d_iter_t | getExpandedStatesEnd () |
exp_states_2d_iter_t | getExpandedStatesStart () |
int | GetFromToHeuristic (int FromStateID, int ToStateID) |
int | GetGoalHeuristic (int stateID) |
int | getNumExpandedStates () |
void | GetPreds (int TargetStateID, std::vector< int > *PredIDV, std::vector< int > *CostV) |
Calculates the predecessor states and the corresponding costs when reversing the footstep set on the planning state TargetStateID. (Used for backward planning.) More... | |
void | getPredsOfGridCells (const std::vector< State > &changed_states, std::vector< int > *pred_ids) |
virtual void | GetRandomPredsatDistance (int TargetStateID, std::vector< int > *PredIDV, std::vector< int > *CLowV) |
Used for RStar: generate succs/preds at some domain-dependent distance. The number of generated succs/preds is up to the environment. More... | |
exp_states_iter_t | getRandomStatesEnd () |
exp_states_iter_t | getRandomStatesStart () |
virtual void | GetRandomSuccsatDistance (int SourceStateID, std::vector< int > *SuccIDV, std::vector< int > *CLowV) |
Used for RStar: generate succs/preds at some domain-dependent distance. The number of generated succs/preds is up to the environment. More... | |
int | GetStartHeuristic (int stateID) |
bool | getState (unsigned int id, State *s) |
Try to receive a state with a certain ID. More... | |
void | GetSuccs (int SourceStateID, std::vector< int > *SuccIDV, std::vector< int > *CostV) |
Calculates the successor states and the corresponding costs when performing the footstep set on the planning state SourceStateID. (Used for forward planning.) More... | |
void | getSuccsOfGridCells (const std::vector< State > &changed_states, std::vector< int > *succ_ids) |
void | GetSuccsTo (int SourceStateID, int goalStateID, std::vector< int > *SuccIDV, std::vector< int > *CostV) |
Testing, for R*. More... | |
bool | InitializeEnv (const char *sEnvFile) |
bool | InitializeMDPCfg (MDPConfig *MDPCfg) |
bool | occupied (const State &s) |
void | PrintEnv_Config (FILE *fOut) |
void | PrintState (int stateID, bool bVerbose, FILE *fOut) |
bool | reachable (const PlanningState &from, const PlanningState &to) |
void | reset () |
Resets the current planning task (i.e. the start and goal poses). More... | |
void | SetAllActionsandAllOutcomes (CMDPSTATE *state) |
void | SetAllPreds (CMDPSTATE *state) |
int | SizeofCreatedEnv () |
std::pair< int, int > | updateGoal (const State &foot_left, const State &foot_right) |
Update the robot's feet poses in the goal state. More... | |
void | updateHeuristicValues () |
Update the heuristic values (e.g. after the map has changed). The environment takes care that the update is only done when it is necessary. More... | |
void | updateMap (gridmap_2d::GridMap2DPtr map) |
std::pair< int, int > | updateStart (const State &foot_left, const State &right_right) |
Update the robot's feet poses in the start state. More... | |
virtual | ~FootstepPlannerEnvironment () |
Static Public Attributes | |
static const int | cvMmScale = 1000 |
Used to scale continuous values in meter to discrete values in mm. More... | |
Protected Member Functions | |
bool | closeToGoal (const PlanningState &from) |
bool | closeToStart (const PlanningState &from) |
const PlanningState * | createHashEntryIfNotExists (const PlanningState &s) |
const PlanningState * | createNewHashEntry (const State &s) |
Wrapper for FootstepPlannerEnvironment::createNewHashEntry(PlanningState). More... | |
const PlanningState * | createNewHashEntry (const PlanningState &s) |
Creates a new planning state for 's' and inserts it into the maps (PlanningState::ivStateId2State, PlanningState::ivpStateHash2State) More... | |
int | GetFromToHeuristic (const PlanningState &from, const PlanningState &to) |
const PlanningState * | getHashEntry (const State &s) |
Wrapper for FootstepPlannerEnvironment::getHashEntry(PlanningState). More... | |
const PlanningState * | getHashEntry (const PlanningState &s) |
void | GetRandomNeighs (const PlanningState *currentState, std::vector< int > *NeighIDV, std::vector< int > *CLowV, int nNumofNeighs, int nDist_c, bool bSuccs) |
bool | occupied (const PlanningState &s) |
void | setStateArea (const PlanningState &left, const PlanningState &right) |
int | stepCost (const PlanningState &a, const PlanningState &b) |
Protected Attributes | |
const double | ivCellSize |
The size of each grid cell used to discretize the robot positions. More... | |
const int | ivCollisionCheckAccuracy |
Whether to check just the foot's inner circle (0), the hole outer circle (1) or exactly the foot's bounding box (2) for collision. More... | |
exp_states_2d_t | ivExpandedStates |
const double | ivFootsizeX |
Size of the foot in x direction. More... | |
const double | ivFootsizeY |
Size of the foot in y direction. More... | |
const std::vector< Footstep > & | ivFootstepSet |
The set of footsteps used for the path planning. More... | |
const bool | ivForwardSearch |
Whether to use forward search (1) or backward search (0). More... | |
const int | ivHashTableSize |
Size of the hash table storing the planning states expanded during the search. (Also referred to by max_hash_size.) More... | |
const boost::shared_ptr< Heuristic > | ivHeuristicConstPtr |
The heuristic function used by the planner. More... | |
bool | ivHeuristicExpired |
Indicates if heuristic has to be updated. More... | |
double | ivHeuristicScale |
int | ivIdGoalFootLeft |
ID of the goal pose of the left foot. More... | |
int | ivIdGoalFootRight |
ID of the goal pose of the right foot. More... | |
int | ivIdPlanningGoal |
ID of the planning goal, i.e. dependent on the planning direction (forward/backward) this ID is used to map to the goal/start poses. More... | |
int | ivIdStartFootLeft |
ID of the start pose of the left foot. More... | |
int | ivIdStartFootRight |
ID of the start pose of the right foot. More... | |
boost::shared_ptr< gridmap_2d::GridMap2D > | ivMapPtr |
Pointer to the map. More... | |
int | ivMaxFootstepTheta |
The maximal rotation (discretized into bins). More... | |
const int | ivMaxFootstepX |
The maximal translation in x direction (discretized in cell size). More... | |
const int | ivMaxFootstepY |
The maximal translation in y direction (discretized in cell size). More... | |
int | ivMaxInvFootstepTheta |
The minimal rotation (discretized into bins). More... | |
const int | ivMaxInvFootstepX |
The minimal translation in x direction (discretized in cell size). More... | |
const int | ivMaxInvFootstepY |
The minimal translation in y direction (discretized in cell size). More... | |
double | ivMaxStepWidth |
const int | ivNumAngleBins |
The number of bins used to discretize the robot orientations. More... | |
size_t | ivNumExpandedStates |
const int | ivNumRandomNodes |
number of random neighbors for R* More... | |
const double | ivOriginFootShiftX |
Shift in x direction from the foot's center. More... | |
const double | ivOriginFootShiftY |
Shift in y direction from the foot's center. More... | |
std::vector< const PlanningState * > * | ivpStateHash2State |
Maps from a hash tag to a list of corresponding planning states. (Used in FootstepPlannerEnvironment to identify a certain PlanningState.) More... | |
bool * | ivpStepRange |
const int | ivRandomNodeDist |
distance of random neighbors for R* (discretized in cells) More... | |
exp_states_t | ivRandomStates |
random intermediate states for R* More... | |
std::vector< int > | ivStateArea |
std::vector< const PlanningState * > | ivStateId2State |
Maps from an ID to the corresponding PlanningState. (Used in the SBPL to access a certain PlanningState.) More... | |
const int | ivStepCost |
The costs for each step (discretized with the help of cvMmScale). More... | |
A class defining a footstep planner environment for humanoid robots used by the SBPL to perform planning tasks.
The environment keeps track of all the planning states expanded during the search. Each planning state can be accessed via its ID. Furthermore
Definition at line 74 of file FootstepPlannerEnvironment.h.
typedef exp_states_2d_t::const_iterator footstep_planner::FootstepPlannerEnvironment::exp_states_2d_iter_t |
Definition at line 89 of file FootstepPlannerEnvironment.h.
typedef std::tr1::unordered_set<std::pair<int,int>, IntPairHash > footstep_planner::FootstepPlannerEnvironment::exp_states_2d_t |
Definition at line 88 of file FootstepPlannerEnvironment.h.
typedef exp_states_t::const_iterator footstep_planner::FootstepPlannerEnvironment::exp_states_iter_t |
Definition at line 87 of file FootstepPlannerEnvironment.h.
typedef std::vector<int> footstep_planner::FootstepPlannerEnvironment::exp_states_t |
Definition at line 86 of file FootstepPlannerEnvironment.h.
footstep_planner::FootstepPlannerEnvironment::FootstepPlannerEnvironment | ( | const environment_params & | params | ) |
footstep_set | The set of footsteps used for the path planning. |
heuristic | The heuristic used by the planner. |
footsize_x | Size of the foot in x direction. |
footsize_y | Size of the foot in y direction. |
origin_foot_shift_x | Shift in x direction from the foot's center. |
origin_foot_shift_y | Shift in y direction from the foot's center. |
max_footstep_x | The maximal translation in x direction performable by the robot. |
max_footstep_y | The maximal translation in y direction performable by the robot. |
max_footstep_theta | The maximal rotation performable by the robot. |
max_inverse_footstep_x | The minimal translation in x direction performable by the robot. |
max_inverse_footstep_y | The minimal translation in y direction performable by the robot. |
max_inverse_footstep_theta | The minimal rotation performable by the robot. |
step_cost | The costs for each step. |
collision_check_accuracy | Whether to check just the foot's circumcircle (0), the incircle (1) or recursively the circumcircle and the incircle for the whole foot (2) for collision. |
hash_table_size | Size of the hash table storing the planning states expanded during the search. |
cell_size | The size of each grid cell used to discretize the robot positions. |
num_angle_bins | The number of bins used to discretize the robot orientations. |
forward_search | Whether to use forward search (1) or backward search (0). |
Definition at line 26 of file FootstepPlannerEnvironment.cpp.
|
virtual |
Definition at line 86 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::AreEquivalent | ( | int | StateID1, |
int | StateID2 | ||
) |
Definition at line 1108 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 426 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 412 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 240 of file FootstepPlannerEnvironment.cpp.
|
protected |
Wrapper for FootstepPlannerEnvironment::createNewHashEntry(PlanningState).
Definition at line 179 of file FootstepPlannerEnvironment.cpp.
|
protected |
Creates a new planning state for 's' and inserts it into the maps (PlanningState::ivStateId2State, PlanningState::ivpStateHash2State)
Definition at line 187 of file FootstepPlannerEnvironment.cpp.
|
inline |
Definition at line 174 of file FootstepPlannerEnvironment.h.
|
inline |
Definition at line 169 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::GetFromToHeuristic | ( | int | FromStateID, |
int | ToStateID | ||
) |
Definition at line 598 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 619 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::GetGoalHeuristic | ( | int | stateID | ) |
Definition at line 628 of file FootstepPlannerEnvironment.cpp.
|
protected |
Wrapper for FootstepPlannerEnvironment::getHashEntry(PlanningState).
Definition at line 216 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 224 of file FootstepPlannerEnvironment.cpp.
|
inline |
Definition at line 167 of file FootstepPlannerEnvironment.h.
void footstep_planner::FootstepPlannerEnvironment::GetPreds | ( | int | TargetStateID, |
std::vector< int > * | PredIDV, | ||
std::vector< int > * | CostV | ||
) |
Calculates the predecessor states and the corresponding costs when reversing the footstep set on the planning state TargetStateID. (Used for backward planning.)
Definition at line 635 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::getPredsOfGridCells | ( | const std::vector< State > & | changed_states, |
std::vector< int > * | pred_ids | ||
) |
Definition at line 536 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 953 of file FootstepPlannerEnvironment.cpp.
|
virtual |
Used for RStar: generate succs/preds at some domain-dependent distance. The number of generated succs/preds is up to the environment.
Definition at line 926 of file FootstepPlannerEnvironment.cpp.
|
inline |
Definition at line 184 of file FootstepPlannerEnvironment.h.
|
inline |
Definition at line 179 of file FootstepPlannerEnvironment.h.
|
virtual |
Used for RStar: generate succs/preds at some domain-dependent distance. The number of generated succs/preds is up to the environment.
Definition at line 904 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::GetStartHeuristic | ( | int | stateID | ) |
Definition at line 723 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::getState | ( | unsigned int | id, |
State * | s | ||
) |
Try to receive a state with a certain ID.
Definition at line 302 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::GetSuccs | ( | int | SourceStateID, |
std::vector< int > * | SuccIDV, | ||
std::vector< int > * | CostV | ||
) |
Calculates the successor states and the corresponding costs when performing the footstep set on the planning state SourceStateID. (Used for forward planning.)
Definition at line 730 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::getSuccsOfGridCells | ( | const std::vector< State > & | changed_states, |
std::vector< int > * | succ_ids | ||
) |
Definition at line 567 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::GetSuccsTo | ( | int | SourceStateID, |
int | goalStateID, | ||
std::vector< int > * | SuccIDV, | ||
std::vector< int > * | CostV | ||
) |
Testing, for R*.
Definition at line 821 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::InitializeEnv | ( | const char * | sEnvFile | ) |
Definition at line 1135 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::InitializeMDPCfg | ( | MDPConfig * | MDPCfg | ) |
Definition at line 1144 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::occupied | ( | const State & | s | ) |
Definition at line 269 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 277 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::PrintEnv_Config | ( | FILE * | fOut | ) |
Definition at line 1159 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::PrintState | ( | int | stateID, |
bool | bVerbose, | ||
FILE * | fOut | ||
) |
Definition at line 1168 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::reachable | ( | const PlanningState & | from, |
const PlanningState & | to | ||
) |
Definition at line 441 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::reset | ( | ) |
Resets the current planning task (i.e. the start and goal poses).
Definition at line 377 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::SetAllActionsandAllOutcomes | ( | CMDPSTATE * | state | ) |
Definition at line 1197 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::SetAllPreds | ( | CMDPSTATE * | state | ) |
Definition at line 1211 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 1232 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::SizeofCreatedEnv | ( | ) |
Definition at line 1225 of file FootstepPlannerEnvironment.cpp.
|
protected |
Definition at line 253 of file FootstepPlannerEnvironment.cpp.
std::pair< int, int > footstep_planner::FootstepPlannerEnvironment::updateGoal | ( | const State & | foot_left, |
const State & | foot_right | ||
) |
Update the robot's feet poses in the goal state.
Definition at line 103 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::updateHeuristicValues | ( | ) |
Update the heuristic values (e.g. after the map has changed). The environment takes care that the update is only done when it is necessary.
Definition at line 337 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::updateMap | ( | gridmap_2d::GridMap2DPtr | map | ) |
Definition at line 319 of file FootstepPlannerEnvironment.cpp.
std::pair< int, int > footstep_planner::FootstepPlannerEnvironment::updateStart | ( | const State & | foot_left, |
const State & | right_right | ||
) |
Update the robot's feet poses in the start state.
Definition at line 141 of file FootstepPlannerEnvironment.cpp.
|
static |
Used to scale continuous values in meter to discrete values in mm.
Definition at line 284 of file FootstepPlannerEnvironment.h.
|
protected |
The size of each grid cell used to discretize the robot positions.
Definition at line 430 of file FootstepPlannerEnvironment.h.
|
protected |
Whether to check just the foot's inner circle (0), the hole outer circle (1) or exactly the foot's bounding box (2) for collision.
Definition at line 421 of file FootstepPlannerEnvironment.h.
|
protected |
Definition at line 456 of file FootstepPlannerEnvironment.h.
|
protected |
Size of the foot in x direction.
Definition at line 387 of file FootstepPlannerEnvironment.h.
|
protected |
Size of the foot in y direction.
Definition at line 389 of file FootstepPlannerEnvironment.h.
|
protected |
The set of footsteps used for the path planning.
Definition at line 381 of file FootstepPlannerEnvironment.h.
|
protected |
Whether to use forward search (1) or backward search (0).
Definition at line 435 of file FootstepPlannerEnvironment.h.
|
protected |
Size of the hash table storing the planning states expanded during the search. (Also referred to by max_hash_size.)
Definition at line 427 of file FootstepPlannerEnvironment.h.
|
protected |
The heuristic function used by the planner.
Definition at line 384 of file FootstepPlannerEnvironment.h.
|
protected |
Indicates if heuristic has to be updated.
Definition at line 451 of file FootstepPlannerEnvironment.h.
|
protected |
Scaling factor of heuristic, in case it underestimates by a constant factor.
Definition at line 448 of file FootstepPlannerEnvironment.h.
|
protected |
ID of the goal pose of the left foot.
Definition at line 361 of file FootstepPlannerEnvironment.h.
|
protected |
ID of the goal pose of the right foot.
Definition at line 363 of file FootstepPlannerEnvironment.h.
|
protected |
ID of the planning goal, i.e. dependent on the planning direction (forward/backward) this ID is used to map to the goal/start poses.
Definition at line 354 of file FootstepPlannerEnvironment.h.
|
protected |
ID of the start pose of the left foot.
Definition at line 357 of file FootstepPlannerEnvironment.h.
|
protected |
ID of the start pose of the right foot.
Definition at line 359 of file FootstepPlannerEnvironment.h.
|
protected |
Pointer to the map.
Definition at line 454 of file FootstepPlannerEnvironment.h.
|
protected |
The maximal rotation (discretized into bins).
Definition at line 401 of file FootstepPlannerEnvironment.h.
|
protected |
The maximal translation in x direction (discretized in cell size).
Definition at line 397 of file FootstepPlannerEnvironment.h.
|
protected |
The maximal translation in y direction (discretized in cell size).
Definition at line 399 of file FootstepPlannerEnvironment.h.
|
protected |
The minimal rotation (discretized into bins).
Definition at line 408 of file FootstepPlannerEnvironment.h.
|
protected |
The minimal translation in x direction (discretized in cell size).
Definition at line 404 of file FootstepPlannerEnvironment.h.
|
protected |
The minimal translation in y direction (discretized in cell size).
Definition at line 406 of file FootstepPlannerEnvironment.h.
|
protected |
Definition at line 437 of file FootstepPlannerEnvironment.h.
|
protected |
The number of bins used to discretize the robot orientations.
Definition at line 432 of file FootstepPlannerEnvironment.h.
|
protected |
Definition at line 458 of file FootstepPlannerEnvironment.h.
|
protected |
number of random neighbors for R*
Definition at line 440 of file FootstepPlannerEnvironment.h.
|
protected |
Shift in x direction from the foot's center.
Definition at line 392 of file FootstepPlannerEnvironment.h.
|
protected |
Shift in y direction from the foot's center.
Definition at line 394 of file FootstepPlannerEnvironment.h.
|
protected |
Maps from a hash tag to a list of corresponding planning states. (Used in FootstepPlannerEnvironment to identify a certain PlanningState.)
Definition at line 378 of file FootstepPlannerEnvironment.h.
|
protected |
Definition at line 460 of file FootstepPlannerEnvironment.h.
|
protected |
distance of random neighbors for R* (discretized in cells)
Definition at line 442 of file FootstepPlannerEnvironment.h.
|
protected |
random intermediate states for R*
Definition at line 457 of file FootstepPlannerEnvironment.h.
|
protected |
Definition at line 365 of file FootstepPlannerEnvironment.h.
|
protected |
Maps from an ID to the corresponding PlanningState. (Used in the SBPL to access a certain PlanningState.)
Definition at line 371 of file FootstepPlannerEnvironment.h.
|
protected |
The costs for each step (discretized with the help of cvMmScale).
Definition at line 414 of file FootstepPlannerEnvironment.h.