Classes | Public Types | Public Member Functions | Static Public Attributes | Protected Member Functions | Protected Attributes
footstep_planner::FootstepPlannerEnvironment Class Reference

A class defining a footstep planner environment for humanoid robots used by the SBPL to perform planning tasks. More...

#include <FootstepPlannerEnvironment.h>

List of all members.

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 &params)
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 ()
std::pair< int, int > updateGoal (const State &foot_left, const State &foot_right)
 Update the robot's feet poses in the goal state.
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)
std::pair< int, int > updateStart (const State &foot_left, const State &right_right)
 Update the robot's feet poses in the start state.
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 PlanningStatecreateHashEntryIfNotExists (const PlanningState &s)
const PlanningStatecreateNewHashEntry (const State &s)
 Wrapper for FootstepPlannerEnvironment::createNewHashEntry(PlanningState).
const PlanningStatecreateNewHashEntry (const PlanningState &s)
 Creates a new planning state for 's' and inserts it into the maps (PlanningState::ivStateId2State, PlanningState::ivpStateHash2State)
int GetFromToHeuristic (const PlanningState &from, const PlanningState &to)
const PlanningStategetHashEntry (const State &s)
 Wrapper for FootstepPlannerEnvironment::getHashEntry(PlanningState).
const PlanningStategetHashEntry (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.
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
 Indicates if heuristic has to be updated.
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 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.
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.)
bool * ivpStepRange
const int ivRandomNodeDist
 distance of random neighbors for R* (discretized in cells)
exp_states_t ivRandomStates
 random intermediate states for R*
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.)
const int ivStepCost
 The costs for each step (discretized with the help of cvMmScale).

Detailed Description

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.


Member Typedef Documentation

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.

Definition at line 87 of file FootstepPlannerEnvironment.h.

Definition at line 86 of file FootstepPlannerEnvironment.h.


Constructor & Destructor Documentation

Parameters:
footstep_setThe set of footsteps used for the path planning.
heuristicThe heuristic used by the planner.
footsize_xSize of the foot in x direction.
footsize_ySize of the foot in y direction.
origin_foot_shift_xShift in x direction from the foot's center.
origin_foot_shift_yShift in y direction from the foot's center.
max_footstep_xThe maximal translation in x direction performable by the robot.
max_footstep_yThe maximal translation in y direction performable by the robot.
max_footstep_thetaThe maximal rotation performable by the robot.
max_inverse_footstep_xThe minimal translation in x direction performable by the robot.
max_inverse_footstep_yThe minimal translation in y direction performable by the robot.
max_inverse_footstep_thetaThe minimal rotation performable by the robot.
step_costThe costs for each step.
collision_check_accuracyWhether 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_sizeSize of the hash table storing the planning states expanded during the search.
cell_sizeThe size of each grid cell used to discretize the robot positions.
num_angle_binsThe number of bins used to discretize the robot orientations.
forward_searchWhether to use forward search (1) or backward search (0).

Definition at line 26 of file FootstepPlannerEnvironment.cpp.

Definition at line 86 of file FootstepPlannerEnvironment.cpp.


Member Function Documentation

bool footstep_planner::FootstepPlannerEnvironment::AreEquivalent ( int  StateID1,
int  StateID2 
)
Returns:
True if two states meet the same condition. Used for R*.

Definition at line 1108 of file FootstepPlannerEnvironment.cpp.

Returns:
True iff 'goal' can be reached by an arbitrary footstep. (Used for forward planning.)

Definition at line 426 of file FootstepPlannerEnvironment.cpp.

Returns:
True iff 'start' can be reached by an arbitrary footstep. (Used for backward planning.)

Definition at line 412 of file FootstepPlannerEnvironment.cpp.

Definition at line 240 of file FootstepPlannerEnvironment.cpp.

Wrapper for FootstepPlannerEnvironment::createNewHashEntry(PlanningState).

Definition at line 179 of file FootstepPlannerEnvironment.cpp.

Creates a new planning state for 's' and inserts it into the maps (PlanningState::ivStateId2State, PlanningState::ivpStateHash2State)

Returns:
A pointer to the newly created PlanningState.

Definition at line 187 of file FootstepPlannerEnvironment.cpp.

Definition at line 174 of file FootstepPlannerEnvironment.h.

Definition at line 169 of file FootstepPlannerEnvironment.h.

int footstep_planner::FootstepPlannerEnvironment::GetFromToHeuristic ( int  FromStateID,
int  ToStateID 
)
Returns:
The costs (in mm, truncated as int) to reach the planning state ToStateID from within planning state FromStateID.

Definition at line 598 of file FootstepPlannerEnvironment.cpp.

Returns:
The costs (in mm, truncated as int) to reach the planning state ToStateID from within planning state FromStateID.

Definition at line 619 of file FootstepPlannerEnvironment.cpp.

Returns:
The heuristic value to reach the goal from within the planning state stateID (used for forward planning).

Definition at line 628 of file FootstepPlannerEnvironment.cpp.

Wrapper for FootstepPlannerEnvironment::getHashEntry(PlanningState).

Definition at line 216 of file FootstepPlannerEnvironment.cpp.

Returns:
The pointer to the planning state 's' stored in FootstepPlannerEnvironment::ivpStateHash2State.

Definition at line 224 of file FootstepPlannerEnvironment.cpp.

Returns:
The number of expanded states during the search.

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.

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 953 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.

Definition at line 926 of file FootstepPlannerEnvironment.cpp.

Definition at line 184 of file FootstepPlannerEnvironment.h.

Definition at line 179 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.

Definition at line 904 of file FootstepPlannerEnvironment.cpp.

Returns:
The heuristic value to reach the start from within the planning state stateID. (Used for backward planning.)

Definition at line 723 of file FootstepPlannerEnvironment.cpp.

Try to receive a state with a certain ID.

Returns:
True iff there is a state with such an 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.

Definition at line 1135 of file FootstepPlannerEnvironment.cpp.

Definition at line 1144 of file FootstepPlannerEnvironment.cpp.

Returns:
True iff the foot in State s is colliding with an obstacle.

Definition at line 269 of file FootstepPlannerEnvironment.cpp.

Returns:
True iff the foot in 's' is colliding with an obstacle.

Definition at line 277 of file FootstepPlannerEnvironment.cpp.

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.

Returns:
True iff 'to' can be reached by an arbitrary footstep that can be performed by the robot from within 'from'. (This method is used to check whether the goal/start can be reached from within the current state.)

Definition at line 441 of file FootstepPlannerEnvironment.cpp.

Resets the current planning task (i.e. the start and goal poses).

Definition at line 377 of file FootstepPlannerEnvironment.cpp.

Definition at line 1197 of file FootstepPlannerEnvironment.cpp.

Definition at line 1211 of file FootstepPlannerEnvironment.cpp.

void footstep_planner::FootstepPlannerEnvironment::setStateArea ( const PlanningState left,
const PlanningState right 
) [protected]

Definition at line 1232 of file FootstepPlannerEnvironment.cpp.

Definition at line 1225 of file FootstepPlannerEnvironment.cpp.

Returns:
The step cost for reaching 'b' from within 'a'.

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.

Returns:
The new IDs (left, right) of the planning state representing the feet.

Definition at line 103 of file FootstepPlannerEnvironment.cpp.

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.

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.

Returns:
The new IDs (left, right) of the planning states representing the feet.

Definition at line 141 of file FootstepPlannerEnvironment.cpp.


Member Data Documentation

Used to scale continuous values in meter to discrete values in mm.

Definition at line 284 of file FootstepPlannerEnvironment.h.

The size of each grid cell used to discretize the robot positions.

Definition at line 430 of file FootstepPlannerEnvironment.h.

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.

Definition at line 456 of file FootstepPlannerEnvironment.h.

Size of the foot in x direction.

Definition at line 387 of file FootstepPlannerEnvironment.h.

Size of the foot in y direction.

Definition at line 389 of file FootstepPlannerEnvironment.h.

The set of footsteps used for the path planning.

Definition at line 381 of file FootstepPlannerEnvironment.h.

Whether to use forward search (1) or backward search (0).

Definition at line 435 of file FootstepPlannerEnvironment.h.

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.

The heuristic function used by the planner.

Definition at line 384 of file FootstepPlannerEnvironment.h.

Indicates if heuristic has to be updated.

Definition at line 451 of file FootstepPlannerEnvironment.h.

Scaling factor of heuristic, in case it underestimates by a constant factor.

Definition at line 448 of file FootstepPlannerEnvironment.h.

ID of the goal pose of the left foot.

Definition at line 361 of file FootstepPlannerEnvironment.h.

ID of the goal pose of the right foot.

Definition at line 363 of file FootstepPlannerEnvironment.h.

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.

ID of the start pose of the left foot.

Definition at line 357 of file FootstepPlannerEnvironment.h.

ID of the start pose of the right foot.

Definition at line 359 of file FootstepPlannerEnvironment.h.

Pointer to the map.

Definition at line 454 of file FootstepPlannerEnvironment.h.

The maximal rotation (discretized into bins).

Definition at line 401 of file FootstepPlannerEnvironment.h.

The maximal translation in x direction (discretized in cell size).

Definition at line 397 of file FootstepPlannerEnvironment.h.

The maximal translation in y direction (discretized in cell size).

Definition at line 399 of file FootstepPlannerEnvironment.h.

The minimal rotation (discretized into bins).

Definition at line 408 of file FootstepPlannerEnvironment.h.

The minimal translation in x direction (discretized in cell size).

Definition at line 404 of file FootstepPlannerEnvironment.h.

The minimal translation in y direction (discretized in cell size).

Definition at line 406 of file FootstepPlannerEnvironment.h.

Definition at line 437 of file FootstepPlannerEnvironment.h.

The number of bins used to discretize the robot orientations.

Definition at line 432 of file FootstepPlannerEnvironment.h.

Definition at line 458 of file FootstepPlannerEnvironment.h.

number of random neighbors for R*

Definition at line 440 of file FootstepPlannerEnvironment.h.

Shift in x direction from the foot's center.

Definition at line 392 of file FootstepPlannerEnvironment.h.

Shift in y direction from the foot's center.

Definition at line 394 of file FootstepPlannerEnvironment.h.

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.

Definition at line 460 of file FootstepPlannerEnvironment.h.

distance of random neighbors for R* (discretized in cells)

Definition at line 442 of file FootstepPlannerEnvironment.h.

random intermediate states for R*

Definition at line 457 of file FootstepPlannerEnvironment.h.

Definition at line 365 of file FootstepPlannerEnvironment.h.

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.

The costs for each step (discretized with the help of cvMmScale).

Definition at line 414 of file FootstepPlannerEnvironment.h.


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


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Wed Aug 26 2015 11:54:32