Class HyRRT
Defined in File HyRRT.h
Nested Relationships
Nested Types
Inheritance Relationships
Base Type
public ompl::base::Planner(Class Planner)
Class Documentation
-
class HyRRT : public ompl::base::Planner
Hybrid Rapidly-exploring Random Trees.
- Hybrid RRT (HyRRT) is an RRT algorithm that solves separate optimization
problems associated with the flows and jumps of the systems, to solve a variety of robotic motion planning problems. As an RRT algorithm, HyRRT is probabilistically-complete. The logical flow of the algorithm is as follows:
Initialize the tree with a start state.
While a solution has not been found: a. Sample a random state. b. Find the nearest state in the tree to the random state. c. Extend from that state under either the flow or jump regimes, using the continuous or discrete simulators, respectively. d. Continue until the state is in collision, or the maximum flow time has been exceeded. e. If the state is not within the unsafe set, add the state to the tree. If the state is in collision, proceed to jump.
Return the solution.
- External documentation: https://ieeexplore.ieee.org/document/9992444
DOI: [10.1109/CDC51059.2022.9992444]
Public Functions
-
HyRRT(const control::SpaceInformationPtr &si)
Constructor.
-
~HyRRT() override
Destructor.
-
virtual void clear() override
Clear all allocated memory.
-
virtual void setup() override
Set the problem instance to solve.
-
virtual void getPlannerData(base::PlannerData &data) const override
Get the PlannerData object associated with this planner.
- Parameters:
data – the PlannerData object storing the edges and vertices of the solution
-
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. If this option is used, it is assumed the problem definition is not changed (unpredictable results otherwise). The only change in the problem definition that is accounted for is the addition of starting or goal states (but not changing previously added start/goal states). If clearQuery() is called, the planner may retain prior datastructures generated from a previous query on a new problem definition. The function terminates if the call to ptc returns true.
-
void freeMemory()
Free the memory allocated by this planner.
-
inline void setTm(double tM)
Set the maximum time allocated to a full continuous simulator step.
- Parameters:
tM – the maximum time allocated. Must be greater than 0, and greater than than the time allocated to a single continuous simulator call.
-
inline void setFlowStepDuration(double duration)
Set the time allocated to a single continuous simulator call, within the full period of a continuous simulator step.
- Parameters:
duration – the time allocated per simulator step. Must be greater than 0 and less than the time allocated to a full continuous simulator step.
-
inline void setJumpSet(std::function<bool(Motion *motion)> jumpSet)
Define the jump set.
- Parameters:
jumpSet – the jump set associated with the hybrid system.
-
inline void setFlowSet(std::function<bool(Motion *motion)> flowSet)
Define the flow set.
- Parameters:
flowSet – the flow set associated with the hybrid system.
-
inline void setUnsafeSet(std::function<bool(Motion *motion)> unsafeSet)
Define the unsafe set.
- Parameters:
unsafeSet – the unsafe set associated with the hybrid system.
-
inline void setDistanceFunction(std::function<double(base::State*, base::State*)> function)
Define the distance measurement function.
- Parameters:
function – the distance function associated with the motion planning problem.
-
inline void setDiscreteSimulator(std::function<base::State*(base::State *curState, const control::Control *u, base::State *newState)> function)
Define the discrete dynamics simulator.
- Parameters:
function – the discrete simulator associated with the hybrid system.
-
inline void setCollisionChecker(std::function<bool(Motion *motion, std::function<bool(Motion *motion)> obstacleSet, base::State *newState, double *collisionTime)> function)
Define the collision checker.
- Parameters:
function – the collision checker associated with the state space. Default is a point-by-point collision checker.
-
template<template<typename T> class NN>
inline void setNearestNeighbors(void) Set a different nearest neighbors datastructure.
-
inline void checkMandatoryParametersSet(void) const
Check if all required parameters have been set.
Public Members
-
std::function<ompl::base::State*(const control::Control *control, ompl::base::State *x_cur, double tFlow, ompl::base::State *new_state)> continuousSimulator =
[this](const control::Control *control, base::State *x_cur, double tFlow, base::State*new_state){siC_->getStatePropagator()->propagate(x_cur, control, tFlow, new_state);return new_state;} Simulates the dynamics of the multicopter when in flow regime, with no nonnegligble forces other than input acceleration.
Protected Functions
-
inline void setContinuousSimulator(std::function<base::State*(const control::Control *u, base::State *curState, double tFlowMax, base::State *newState)> function)
Define the continuous dynamics simulator.
- Parameters:
function – the continuous simulator associated with the hybrid system.
-
void initTree(void)
Runs the initial setup tasks for the tree.
-
void randomSample(Motion *randomMotion)
Sample the random motion.
- Parameters:
randomMotion – The motion to be initialized
-
base::PlannerStatus constructSolution(Motion *lastMotion)
Construct the path, starting at the last edge.
- Parameters:
lastMotion – The last motion in the solution
- Returns:
the planner status (APPROXIMATE, EXACT, or UNKNOWN)
Protected Attributes
-
Motion *lastGoalMotion_ = {nullptr}
The most recent goal motion. Used for PlannerData computation.
-
std::shared_ptr<NearestNeighbors<Motion*>> nn_
A nearest-neighbors datastructure containing the tree of motions.
-
std::function<bool(Motion *motion, std::function<bool(Motion *motion)> obstacleSet, base::State *newState, double *collisionTime)> collisionChecker_ =
[this](Motion*motion,std::function<bool(Motion *motion)> obstacleSet, base::State*newState, double *collisionTime) -> bool{if (obstacleSet(motion)) {si_->copyState(newState, motion->solutionPair->back());*collisionTime =ompl::base::HybridStateSpace::getStateTime(motion->solutionPair->back()) - flowStepDuration_;motion->solutionPair->resize(motion->solutionPair->size() - 1);return true;}return false;} Collision checker. Default is point-by-point collision checking using the jump set.
The following are all customizeable parameters, and affect how cHyRRT generates trajectories. Customize using setter functions above.
- Param motion:
The motion to check for collision
- Param obstacleSet:
A function that returns true if the motion’s solution pair intersects with the obstacle set
- Param ts:
The start time of the motion. Default is -1.0
- Param tf:
The end time of the motion. Default is -1.0
- Param newState:
The collision state (if a collision occurs)
- Param collisionTime:
The time of collision (if a collision occurs). If no collision occurs, this value is -1.0
- Return:
true if a collision occurs, false otherwise
-
control::DirectedControlSamplerPtr controlSampler_
Control Sampler.
-
control::SpaceInformation *siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
-
std::function<double(base::State *state1, base::State *state2)> distanceFunc_ =
[this](base::State *state1, base::State*state2) -> double{returnsi_->distance(state1, state2);} Compute distance between states, default is Euclidean distance.
- Param state1:
The first state
- Param state2:
The second state
- Return:
The distance between the two states
-
double tM_
The maximum flow time for a given flow propagation step. Must be set by the user.
-
double flowStepDuration_
The flow time for a given integration step, within a flow propagation step. Must be set by user.
-
std::function<base::State*(base::State *curState, const control::Control *u, base::State *newState)> discreteSimulator_
Simulator for propagation under jump regime.
- Param curState:
The current state
- Param u:
The input
- Param newState:
The newly propagated state
- Return:
The newly propagated state
-
std::function<bool(Motion *motion)> jumpSet_
Function that returns true if a motion intersects with the jump set, and false if not.
- Param motion:
The motion to check
- Return:
True if the state is in the jump set, false if not
-
std::function<bool(Motion *motion)> flowSet_
Function that returns true if a motion intersects with the flow set, and false if not.
- Param motion:
The motion to check
- Return:
True if the state is in the flow set, false if not
-
std::function<bool(Motion *motion)> unsafeSet_
Function that returns true if a motion intersects with the unsafe set, and false if not.
- Param motion:
The motion to check
- Return:
True if the state is in the unsafe set, false if not
-
std::function<base::State*(const control::Control *u, base::State *curState, double tFlowMax, base::State *newState)> continuousSimulator_
Simulator for propagation under flow regime.
- Param input:
The input
- Param curState:
The current state
- Param tFlowMax:
The random maximum flow time
- Param newState:
The newly propagated state
- Return:
The newly propagated state
-
base::StateSamplerPtr sampler_
State sampler.
-
double dist_
Minimum distance from goal to final vertex of generated trajectories.
Protected Static Functions