Definition of a problem to be solved. This includes the start state(s) for the system and a goal specification. More...
#include <ProblemDefinition.h>
Public Member Functions | |
void | addStartState (const ScopedState<> &state) |
void | addStartState (const State *state) |
Add a start state. The state is copied. | |
void | clearGoal (void) |
Clear the goal. Memory is freed. | |
void | clearStartStates (void) |
Clear all start states (memory is freed). | |
bool | fixInvalidInputStates (double distStart, double distGoal, unsigned int attempts) |
Many times the start or goal state will barely touch an obstacle. In this case, we may want to automatically find a nearby state that is valid so motion planning can be performed. This function enables this behaviour. The allowed distance for both start and goal states is specified. The number of attempts is also specified. Returns true if all states are valid after completion. | |
const GoalPtr & | getGoal (void) const |
Return the current goal. | |
void | getInputStates (std::vector< const State * > &states) const |
Get all the input states. This includes start states and states that are part of goal regions that can be casted as ompl::base::GoalState or ompl::base::GoalStates. | |
State * | getStartState (unsigned int index) |
const State * | getStartState (unsigned int index) const |
Returns a specific start state. | |
unsigned int | getStartStateCount (void) const |
Returns the number of start states. | |
bool | hasStartState (const State *state, unsigned int *startIndex=NULL) |
Check whether a specified starting state is already included in the problem definition and optionally return the index of that starting state. | |
bool | isTrivial (unsigned int *startIndex=NULL, double *distance=NULL) const |
A problem is trivial if a given starting state already in the goal region, so we need no motion planning. startID will be set to the index of the starting state that satisfies the goal. The distance to the goal can optionally be returned as well. | |
void | print (std::ostream &out=std::cout) const |
Print information about the start and goal states. | |
ProblemDefinition (const SpaceInformationPtr &si) | |
Create a problem definition given the SpaceInformation it is part of. | |
void | setGoal (const GoalPtr &goal) |
Set the goal. | |
void | setGoalState (const ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon()) |
void | setGoalState (const State *goal, const double threshold=std::numeric_limits< double >::epsilon()) |
A simple form of setting the goal. This is called by setStartAndGoalStates(). A more general form is setGoal(). | |
void | setStartAndGoalStates (const ScopedState<> &start, const ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon()) |
void | setStartAndGoalStates (const State *start, const State *goal, const double threshold=std::numeric_limits< double >::epsilon()) |
In the simplest case possible, we have a single starting state and a single goal state. | |
virtual | ~ProblemDefinition (void) |
Protected Member Functions | |
bool | fixInvalidInputState (State *state, double dist, bool start, unsigned int attempts) |
Helper function for fixInvalidInputStates(). Attempts to fix an individual state. | |
Protected Attributes | |
GoalPtr | goal_ |
The goal representation. | |
msg::Interface | msg_ |
Interface for console output. | |
SpaceInformationPtr | si_ |
The space information this problem definition is for. | |
std::vector< State * > | startStates_ |
The set of start states. |
Definition of a problem to be solved. This includes the start state(s) for the system and a goal specification.
Definition at line 67 of file ProblemDefinition.h.
ompl::base::ProblemDefinition::ProblemDefinition | ( | const SpaceInformationPtr & | si | ) | [inline] |
Create a problem definition given the SpaceInformation it is part of.
Definition at line 72 of file ProblemDefinition.h.
virtual ompl::base::ProblemDefinition::~ProblemDefinition | ( | void | ) | [inline, virtual] |
Definition at line 76 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::addStartState | ( | const ScopedState<> & | state | ) | [inline] |
Add a start state. The state is copied.
Definition at line 88 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::addStartState | ( | const State * | state | ) | [inline] |
Add a start state. The state is copied.
Definition at line 82 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::clearGoal | ( | void | ) | [inline] |
Clear the goal. Memory is freed.
Definition at line 131 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::clearStartStates | ( | void | ) | [inline] |
Clear all start states (memory is freed).
Definition at line 99 of file ProblemDefinition.h.
bool ompl::base::ProblemDefinition::fixInvalidInputState | ( | State * | state, | |
double | dist, | |||
bool | start, | |||
unsigned int | attempts | |||
) | [protected] |
Helper function for fixInvalidInputStates(). Attempts to fix an individual state.
bool ompl::base::ProblemDefinition::fixInvalidInputStates | ( | double | distStart, | |
double | distGoal, | |||
unsigned int | attempts | |||
) |
Many times the start or goal state will barely touch an obstacle. In this case, we may want to automatically find a nearby state that is valid so motion planning can be performed. This function enables this behaviour. The allowed distance for both start and goal states is specified. The number of attempts is also specified. Returns true if all states are valid after completion.
const GoalPtr& ompl::base::ProblemDefinition::getGoal | ( | void | ) | const [inline] |
Return the current goal.
Definition at line 137 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::getInputStates | ( | std::vector< const State * > & | states | ) | const |
Get all the input states. This includes start states and states that are part of goal regions that can be casted as ompl::base::GoalState or ompl::base::GoalStates.
State* ompl::base::ProblemDefinition::getStartState | ( | unsigned int | index | ) | [inline] |
Returns a specific start state.
Definition at line 119 of file ProblemDefinition.h.
const State* ompl::base::ProblemDefinition::getStartState | ( | unsigned int | index | ) | const [inline] |
Returns a specific start state.
Definition at line 113 of file ProblemDefinition.h.
unsigned int ompl::base::ProblemDefinition::getStartStateCount | ( | void | ) | const [inline] |
Returns the number of start states.
Definition at line 107 of file ProblemDefinition.h.
bool ompl::base::ProblemDefinition::hasStartState | ( | const State * | state, | |
unsigned int * | startIndex = NULL | |||
) |
Check whether a specified starting state is already included in the problem definition and optionally return the index of that starting state.
bool ompl::base::ProblemDefinition::isTrivial | ( | unsigned int * | startIndex = NULL , |
|
double * | distance = NULL | |||
) | const |
A problem is trivial if a given starting state already in the goal region, so we need no motion planning. startID will be set to the index of the starting state that satisfies the goal. The distance to the goal can optionally be returned as well.
void ompl::base::ProblemDefinition::print | ( | std::ostream & | out = std::cout |
) | const |
Print information about the start and goal states.
void ompl::base::ProblemDefinition::setGoal | ( | const GoalPtr & | goal | ) | [inline] |
Set the goal.
Definition at line 125 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::setGoalState | ( | const ScopedState<> & | goal, | |
const double | threshold = std::numeric_limits<double>::epsilon() | |||
) | [inline] |
A simple form of setting the goal. This is called by setStartAndGoalStates(). A more general form is setGoal().
Definition at line 167 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::setGoalState | ( | const State * | goal, | |
const double | threshold = std::numeric_limits< double >::epsilon() | |||
) |
A simple form of setting the goal. This is called by setStartAndGoalStates(). A more general form is setGoal().
void ompl::base::ProblemDefinition::setStartAndGoalStates | ( | const ScopedState<> & | start, | |
const ScopedState<> & | goal, | |||
const double | threshold = std::numeric_limits<double>::epsilon() | |||
) | [inline] |
In the simplest case possible, we have a single starting state and a single goal state. This function simply configures the problem definition using these states (performs the needed calls to addStartState(), creates an instance of ompl::base::GoalState and calls setGoal() on it.
Definition at line 161 of file ProblemDefinition.h.
void ompl::base::ProblemDefinition::setStartAndGoalStates | ( | const State * | start, | |
const State * | goal, | |||
const double | threshold = std::numeric_limits< double >::epsilon() | |||
) |
In the simplest case possible, we have a single starting state and a single goal state.
This function simply configures the problem definition using these states (performs the needed calls to addStartState(), creates an instance of ompl::base::GoalState and calls setGoal() on it.
GoalPtr ompl::base::ProblemDefinition::goal_ [protected] |
The goal representation.
Definition at line 200 of file ProblemDefinition.h.
msg::Interface ompl::base::ProblemDefinition::msg_ [protected] |
Interface for console output.
Definition at line 203 of file ProblemDefinition.h.
The space information this problem definition is for.
Definition at line 194 of file ProblemDefinition.h.
std::vector<State*> ompl::base::ProblemDefinition::startStates_ [protected] |
The set of start states.
Definition at line 197 of file ProblemDefinition.h.