$search
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.). | |
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. | |
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. | |
int | GetStartHeuristic (int stateID) |
bool | getState (unsigned int id, State *s) |
Try to receive a state with a certain ID. | |
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.). | |
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*. | |
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). | |
void | SetAllActionsandAllOutcomes (CMDPSTATE *state) |
void | SetAllPreds (CMDPSTATE *state) |
int | SizeofCreatedEnv () |
void | updateGoal (const State &foot_left, const State &foot_right) |
Update the goal pose for both feet. | |
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. | |
void | updateMap (gridmap_2d::GridMap2DPtr map) |
void | updateStart (const State &foot_left, const State &right_right) |
Update the start pose for both feet. | |
virtual | ~FootstepPlannerEnvironment () |
Static Public Attributes | |
static const int | cvMmScale = 1000 |
Used to scale continuous values in meter to discrete values in mm. | |
Protected Member Functions | |
bool | closeToGoal (const PlanningState &from) |
bool | closeToStart (const PlanningState &from) |
const PlanningState * | createHashEntryIfNotExists (const PlanningState &s) |
const PlanningState * | createNewHashEntry (const PlanningState &s) |
Creates a new planning state for 's' and inserts it into the maps (PlanningState::ivStateId2State, PlanningState::ivpStateHash2State). | |
const PlanningState * | createNewHashEntry (const State &s) |
Wrapper for FootstepPlannerEnvironment::createNewHashEntry(PlanningState). | |
int | GetFromToHeuristic (const PlanningState &from, const PlanningState &to) |
const PlanningState * | getHashEntry (const PlanningState &s) |
const PlanningState * | getHashEntry (const State &s) |
Wrapper for FootstepPlannerEnvironment::getHashEntry(PlanningState). | |
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) |
bool | pointWithinPolygon (const std::pair< int, int > &point, const std::vector< std::pair< int, int > > &edges) |
Crossing number method to determine whether a point lies within a polygon or not. | |
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. | |
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. | |
exp_states_2d_t | ivExpandedStates |
const double | ivFootsizeX |
Size of the foot in x direction. | |
const double | ivFootsizeY |
Size of the foot in y direction. | |
const std::vector< Footstep > & | ivFootstepSet |
The set of footsteps used for the path planning. | |
const bool | ivForwardSearch |
Whether to use forward search (1) or backward search (0). | |
const int | ivHashTableSize |
Size of the hash table storing the planning states expanded during the search. (Also referred to by max_hash_size.). | |
const boost::shared_ptr < Heuristic > | ivHeuristicConstPtr |
The heuristic function used by the planner. | |
bool | ivHeuristicExpired |
double | ivHeuristicScale |
int | ivIdGoalFootLeft |
ID of the goal pose of the left foot. | |
int | ivIdGoalFootRight |
ID of the goal pose of the right foot. | |
int | ivIdStartFootLeft |
ID of the start pose of the left foot. | |
int | ivIdStartFootRight |
ID of the start pose of the right foot. | |
boost::shared_ptr < gridmap_2d::GridMap2D > | ivMapPtr |
Pointer to the map. | |
int | ivMaxFootstepTheta |
The maximal rotation (discretized into bins). | |
const int | ivMaxFootstepX |
The maximal translation in x direction (discretized in cell size). | |
const int | ivMaxFootstepY |
The maximal translation in y direction (discretized in cell size). | |
int | ivMaxInvFootstepTheta |
The minimal rotation (discretized into bins). | |
const int | ivMaxInvFootstepX |
The minimal translation in x direction (discretized in cell size). | |
const int | ivMaxInvFootstepY |
The minimal translation in y direction (discretized in cell size). | |
double | ivMaxStepWidth |
const int | ivNumAngleBins |
The number of bins used to discretize the robot orientations. | |
size_t | ivNumExpandedStates |
const int | ivNumRandomNodes |
number of random neighbors for R* | |
const double | ivOriginFootShiftX |
Shift in x direction from the foot's center. | |
const double | ivOriginFootShiftY |
Shift in y direction from the foot's center. | |
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.). | |
const int | ivRandomNodeDist |
distance of random neighbors for R* (discretized in cells) | |
exp_states_t | ivRandomStates |
random intermediate states for R* | |
std::vector< const PlanningState * > | ivStateId2State |
Maps from an ID to the corresponding PlanningState. (Used in the SBPL to access a certain PlanningState.). | |
const int | ivStepCost |
The costs for each step (discretized with the help of cvMmScale). | |
bool * | ivStepRange |
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 29 of file FootstepPlannerEnvironment.cpp.
footstep_planner::FootstepPlannerEnvironment::~FootstepPlannerEnvironment | ( | ) | [virtual] |
Definition at line 86 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::AreEquivalent | ( | int | StateID1, | |
int | StateID2 | |||
) | [virtual] |
Reimplemented from DiscreteSpaceInformation.
Definition at line 1084 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::closeToGoal | ( | const PlanningState & | from | ) | [protected] |
Definition at line 446 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::closeToStart | ( | const PlanningState & | from | ) | [protected] |
Definition at line 432 of file FootstepPlannerEnvironment.cpp.
const PlanningState * footstep_planner::FootstepPlannerEnvironment::createHashEntryIfNotExists | ( | const PlanningState & | s | ) | [protected] |
Definition at line 257 of file FootstepPlannerEnvironment.cpp.
const PlanningState * footstep_planner::FootstepPlannerEnvironment::createNewHashEntry | ( | const PlanningState & | s | ) | [protected] |
Creates a new planning state for 's' and inserts it into the maps (PlanningState::ivStateId2State, PlanningState::ivpStateHash2State).
Definition at line 204 of file FootstepPlannerEnvironment.cpp.
const PlanningState * footstep_planner::FootstepPlannerEnvironment::createNewHashEntry | ( | const State & | s | ) | [protected] |
Wrapper for FootstepPlannerEnvironment::createNewHashEntry(PlanningState).
Definition at line 196 of file FootstepPlannerEnvironment.cpp.
exp_states_2d_iter_t footstep_planner::FootstepPlannerEnvironment::getExpandedStatesEnd | ( | ) | [inline] |
Definition at line 164 of file FootstepPlannerEnvironment.h.
exp_states_2d_iter_t footstep_planner::FootstepPlannerEnvironment::getExpandedStatesStart | ( | ) | [inline] |
Definition at line 159 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::GetFromToHeuristic | ( | const PlanningState & | from, | |
const PlanningState & | to | |||
) | [protected] |
Definition at line 639 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::GetFromToHeuristic | ( | int | FromStateID, | |
int | ToStateID | |||
) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 618 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::GetGoalHeuristic | ( | int | stateID | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 648 of file FootstepPlannerEnvironment.cpp.
const PlanningState * footstep_planner::FootstepPlannerEnvironment::getHashEntry | ( | const PlanningState & | s | ) | [protected] |
Definition at line 241 of file FootstepPlannerEnvironment.cpp.
const PlanningState * footstep_planner::FootstepPlannerEnvironment::getHashEntry | ( | const State & | s | ) | [protected] |
Wrapper for FootstepPlannerEnvironment::getHashEntry(PlanningState).
Definition at line 233 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::getNumExpandedStates | ( | ) | [inline] |
Definition at line 157 of file FootstepPlannerEnvironment.h.
void footstep_planner::FootstepPlannerEnvironment::GetPreds | ( | int | TargetStateID, | |
std::vector< int > * | PredIDV, | |||
std::vector< int > * | CostV | |||
) | [virtual] |
Calculates the predecessor states and the corresponding costs when reversing the footstep set on the planning state TargetStateID. (Used for backward planning.).
Implements DiscreteSpaceInformation.
Definition at line 655 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::getPredsOfGridCells | ( | const std::vector< State > & | changed_states, | |
std::vector< int > * | pred_ids | |||
) |
Definition at line 556 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::GetRandomNeighs | ( | const PlanningState * | currentState, | |
std::vector< int > * | NeighIDV, | |||
std::vector< int > * | CLowV, | |||
int | nNumofNeighs, | |||
int | nDist_c, | |||
bool | bSuccs | |||
) | [protected] |
Definition at line 929 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::GetRandomPredsatDistance | ( | int | TargetStateID, | |
std::vector< int > * | PredIDV, | |||
std::vector< int > * | CLowV | |||
) | [virtual] |
Used for RStar: generate succs/preds at some domain-dependent distance. The number of generated succs/preds is up to the environment.
Reimplemented from DiscreteSpaceInformation.
Definition at line 902 of file FootstepPlannerEnvironment.cpp.
exp_states_iter_t footstep_planner::FootstepPlannerEnvironment::getRandomStatesEnd | ( | ) | [inline] |
Definition at line 174 of file FootstepPlannerEnvironment.h.
exp_states_iter_t footstep_planner::FootstepPlannerEnvironment::getRandomStatesStart | ( | ) | [inline] |
Definition at line 169 of file FootstepPlannerEnvironment.h.
void footstep_planner::FootstepPlannerEnvironment::GetRandomSuccsatDistance | ( | int | SourceStateID, | |
std::vector< int > * | SuccIDV, | |||
std::vector< int > * | CLowV | |||
) | [virtual] |
Used for RStar: generate succs/preds at some domain-dependent distance. The number of generated succs/preds is up to the environment.
Reimplemented from DiscreteSpaceInformation.
Definition at line 880 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::GetStartHeuristic | ( | int | stateID | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 721 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 319 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::GetSuccs | ( | int | SourceStateID, | |
std::vector< int > * | SuccIDV, | |||
std::vector< int > * | CostV | |||
) | [virtual] |
Calculates the successor states and the corresponding costs when performing the footstep set on the planning state SourceStateID. (Used for forward planning.).
Implements DiscreteSpaceInformation.
Definition at line 728 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::getSuccsOfGridCells | ( | const std::vector< State > & | changed_states, | |
std::vector< int > * | succ_ids | |||
) |
Definition at line 587 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 797 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::InitializeEnv | ( | const char * | sEnvFile | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1111 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::InitializeMDPCfg | ( | MDPConfig * | MDPCfg | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1120 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::occupied | ( | const PlanningState & | s | ) | [protected] |
Definition at line 294 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::occupied | ( | const State & | s | ) |
Definition at line 286 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::pointWithinPolygon | ( | const std::pair< int, int > & | point, | |
const std::vector< std::pair< int, int > > & | edges | |||
) | [protected] |
Crossing number method to determine whether a point lies within a polygon or not.
Check http://geomalgorithms.com/a03-_inclusion.html for further details.
Definition at line 1208 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::PrintEnv_Config | ( | FILE * | fOut | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1135 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::PrintState | ( | int | stateID, | |
bool | bVerbose, | |||
FILE * | fOut | |||
) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1144 of file FootstepPlannerEnvironment.cpp.
bool footstep_planner::FootstepPlannerEnvironment::reachable | ( | const PlanningState & | from, | |
const PlanningState & | to | |||
) |
Definition at line 461 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::reset | ( | ) |
Resets the current planning task (i.e. the start and goal poses).
Definition at line 392 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::SetAllActionsandAllOutcomes | ( | CMDPSTATE * | state | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1173 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::SetAllPreds | ( | CMDPSTATE * | state | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1187 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::SizeofCreatedEnv | ( | ) | [virtual] |
Implements DiscreteSpaceInformation.
Definition at line 1201 of file FootstepPlannerEnvironment.cpp.
int footstep_planner::FootstepPlannerEnvironment::stepCost | ( | const PlanningState & | a, | |
const PlanningState & | b | |||
) | [protected] |
Definition at line 270 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::updateGoal | ( | const State & | foot_left, | |
const State & | foot_right | |||
) |
Update the goal pose for both feet.
Definition at line 100 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 352 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::updateMap | ( | gridmap_2d::GridMap2DPtr | map | ) |
Definition at line 336 of file FootstepPlannerEnvironment.cpp.
void footstep_planner::FootstepPlannerEnvironment::updateStart | ( | const State & | foot_left, | |
const State & | right_right | |||
) |
Update the start pose for both feet.
Definition at line 148 of file FootstepPlannerEnvironment.cpp.
const int footstep_planner::FootstepPlannerEnvironment::cvMmScale = 1000 [static] |
Used to scale continuous values in meter to discrete values in mm.
Definition at line 274 of file FootstepPlannerEnvironment.h.
const double footstep_planner::FootstepPlannerEnvironment::ivCellSize [protected] |
The size of each grid cell used to discretize the robot positions.
Definition at line 419 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivCollisionCheckAccuracy [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 410 of file FootstepPlannerEnvironment.h.
Definition at line 444 of file FootstepPlannerEnvironment.h.
const double footstep_planner::FootstepPlannerEnvironment::ivFootsizeX [protected] |
Size of the foot in x direction.
Definition at line 376 of file FootstepPlannerEnvironment.h.
const double footstep_planner::FootstepPlannerEnvironment::ivFootsizeY [protected] |
Size of the foot in y direction.
Definition at line 378 of file FootstepPlannerEnvironment.h.
const std::vector<Footstep>& footstep_planner::FootstepPlannerEnvironment::ivFootstepSet [protected] |
The set of footsteps used for the path planning.
Definition at line 370 of file FootstepPlannerEnvironment.h.
const bool footstep_planner::FootstepPlannerEnvironment::ivForwardSearch [protected] |
Whether to use forward search (1) or backward search (0).
Definition at line 424 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivHashTableSize [protected] |
Size of the hash table storing the planning states expanded during the search. (Also referred to by max_hash_size.).
Definition at line 416 of file FootstepPlannerEnvironment.h.
const boost::shared_ptr<Heuristic> footstep_planner::FootstepPlannerEnvironment::ivHeuristicConstPtr [protected] |
The heuristic function used by the planner.
Definition at line 373 of file FootstepPlannerEnvironment.h.
bool footstep_planner::FootstepPlannerEnvironment::ivHeuristicExpired [protected] |
Definition at line 439 of file FootstepPlannerEnvironment.h.
double footstep_planner::FootstepPlannerEnvironment::ivHeuristicScale [protected] |
Scaling factor of heuristic, in case it underestimates by a constant factor.
Definition at line 437 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::ivIdGoalFootLeft [protected] |
ID of the goal pose of the left foot.
Definition at line 352 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::ivIdGoalFootRight [protected] |
ID of the goal pose of the right foot.
Definition at line 354 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::ivIdStartFootLeft [protected] |
ID of the start pose of the left foot.
Definition at line 348 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::ivIdStartFootRight [protected] |
ID of the start pose of the right foot.
Definition at line 350 of file FootstepPlannerEnvironment.h.
boost::shared_ptr<gridmap_2d::GridMap2D> footstep_planner::FootstepPlannerEnvironment::ivMapPtr [protected] |
Pointer to the map.
Definition at line 442 of file FootstepPlannerEnvironment.h.
int footstep_planner::FootstepPlannerEnvironment::ivMaxFootstepTheta [protected] |
The maximal rotation (discretized into bins).
Definition at line 390 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivMaxFootstepX [protected] |
The maximal translation in x direction (discretized in cell size).
Definition at line 386 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivMaxFootstepY [protected] |
The maximal translation in y direction (discretized in cell size).
Definition at line 388 of file FootstepPlannerEnvironment.h.
The minimal rotation (discretized into bins).
Definition at line 397 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivMaxInvFootstepX [protected] |
The minimal translation in x direction (discretized in cell size).
Definition at line 393 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivMaxInvFootstepY [protected] |
The minimal translation in y direction (discretized in cell size).
Definition at line 395 of file FootstepPlannerEnvironment.h.
double footstep_planner::FootstepPlannerEnvironment::ivMaxStepWidth [protected] |
Definition at line 426 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivNumAngleBins [protected] |
The number of bins used to discretize the robot orientations.
Definition at line 421 of file FootstepPlannerEnvironment.h.
size_t footstep_planner::FootstepPlannerEnvironment::ivNumExpandedStates [protected] |
Definition at line 446 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivNumRandomNodes [protected] |
number of random neighbors for R*
Definition at line 429 of file FootstepPlannerEnvironment.h.
const double footstep_planner::FootstepPlannerEnvironment::ivOriginFootShiftX [protected] |
Shift in x direction from the foot's center.
Definition at line 381 of file FootstepPlannerEnvironment.h.
const double footstep_planner::FootstepPlannerEnvironment::ivOriginFootShiftY [protected] |
Shift in y direction from the foot's center.
Definition at line 383 of file FootstepPlannerEnvironment.h.
std::vector<const PlanningState*>* footstep_planner::FootstepPlannerEnvironment::ivpStateHash2State [protected] |
Maps from a hash tag to a list of corresponding planning states. (Used in FootstepPlannerEnvironment to identify a certain PlanningState.).
Definition at line 367 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivRandomNodeDist [protected] |
distance of random neighbors for R* (discretized in cells)
Definition at line 431 of file FootstepPlannerEnvironment.h.
random intermediate states for R*
Definition at line 445 of file FootstepPlannerEnvironment.h.
std::vector<const PlanningState*> footstep_planner::FootstepPlannerEnvironment::ivStateId2State [protected] |
Maps from an ID to the corresponding PlanningState. (Used in the SBPL to access a certain PlanningState.).
Definition at line 360 of file FootstepPlannerEnvironment.h.
const int footstep_planner::FootstepPlannerEnvironment::ivStepCost [protected] |
The costs for each step (discretized with the help of cvMmScale).
Definition at line 403 of file FootstepPlannerEnvironment.h.
bool* footstep_planner::FootstepPlannerEnvironment::ivStepRange [protected] |
Definition at line 448 of file FootstepPlannerEnvironment.h.