rrt.h File Reference IntroductionDefinition of a rrt on a point on a manifold. This is implemented for comparison with atlas and, eventually to complement atlas. Definition in file rrt.h.
Macro Definition Documentation◆ RRT_VERBOSE
◆ WEIGTHED_NN
◆ PENALIZE_BLOCKED_NODES
◆ PENALIZE_PARENT_NODES
If a node is added to the RRT after detecting a collision or a limit in the domain, the parent node may also be in the region of inevitable collision. If this flag is set, parent nodes of blocked nodes are also blocked. This flag is only used if PENALIZE_BLOCKED_NODES is also set) Recall that penalized nodes are also expanded, but with less probability. ◆ WEIGTHED_REACHED_GOAL
If not null, the distance used to determine if the goal is reached is weighted. Otherwise, we use the standard Euclidean distance. If set to 2, the weighted distance is also used to evaluated the progress of the RRT extension to the random sample (or, eventually, the goal). When the branch approaches the random sample (goal), the extension is repeated
◆ GET_RRT_STATISTICS
◆ HEURISTIC_RRT_STAR
If true we use an heuristic stratetgy in the AtlasRRT*. There are four heuristic that can be applied separately
The different heuristics can be combined via HEURISTIC_ATLASRRT_STAR (1+2) ◆ RRTSTAR_UPDATE_COSTS
On the fly update of the cost in the RRTstar. If set to one, we cost for the tree nodes for RRT* are updated for the nodes close to the just added node to the tree (in a gamma-ball). The cost is updated by accumulating cost to parent for all the neighbouring nodes. This option also affects AtlasRRTStar. ◆ RRTSTAR_SYMMETRIC_COST
◆ EXPLORATION_RRT
When 0 we use the normal RRT with bias to goal and that stops as soon as the goal is found. If 1 the RRT is build without considering the goal and the construction stops when the tree includes the given number of samples (or nodes). Normally this should be 0. We only use it to test the coverage properties of the RRT. This is also used for AtlasRRT.
◆ RRT_PLOT_NODES
◆ RRT_NN_TOPOLOGY
◆ INIT_NUM_SAMPLES_RRT
◆ ONE_TREE
◆ TWO_TREES
◆ TWO_TREES_WITH_SWAP
In this mode only two trees are defined, one from start to goal and one from goal to start. In this mode samples can change between trees. This is only used for bidirectional RRT*. In this mode only one big pool of samples (i.e., one KD-tree) is maintained for NN searching. This is so since the kd-tree implementation does not allow samples to be removed from the kd-tree. ◆ NOTREE◆ START2GOAL
◆ GOAL2START
◆ BOTHTREES
Function Documentation◆ CopyRRTStep()
Copy constructor for RRTSteps.
Definition at line 730 of file rrt.c. Referenced by AtlasBiRRTstar(), BiRRTstar(), and RRTPathSteps(). ◆ InitRRTStatistics()
Init the RRT statistics.
Definition at line 195 of file rrt.c. References TRRTStatistics::dQrand, TRRTStatistics::n, TRRTStatistics::nBranch, TRRTStatistics::nCollision, TRRTStatistics::nCollisionChecks, TRRTStatistics::nConvergenceError, TRRTStatistics::nDistanceIncreases, TRRTStatistics::nHighCost, TRRTStatistics::nNoEmptyBranch, TRRTStatistics::nNoEmptyTreeConnection, TRRTStatistics::nOutOfDomain, TRRTStatistics::nQrandReached, TRRTStatistics::nRandom, TRRTStatistics::nRejections, TRRTStatistics::nSample, TRRTStatistics::nStalled, TRRTStatistics::nStep, TRRTStatistics::nTooFar, and TRRTStatistics::nTreeConnection. Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), main(), and RRTstar(). ◆ AccumulateRRTStatistics()
Accumulates two sets of RRT statistics. This is used to accumulate statistics when executing a long sequence of experiments.
Definition at line 318 of file rrt.c. References TRRTStatistics::dQrand, TRRTStatistics::n, TRRTStatistics::nBranch, TRRTStatistics::nCollision, TRRTStatistics::nCollisionChecks, TRRTStatistics::nDistanceIncreases, TRRTStatistics::nNoEmptyBranch, TRRTStatistics::nNoEmptyTreeConnection, TRRTStatistics::nOutOfDomain, TRRTStatistics::nQrandReached, TRRTStatistics::nRandom, TRRTStatistics::nRejections, TRRTStatistics::nSample, TRRTStatistics::nStalled, TRRTStatistics::nStep, TRRTStatistics::nTooFar, and TRRTStatistics::nTreeConnection. Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), and RRTstar(). ◆ PrintRRTStatistics()
Prints a report about the collected RRT statistics.
Definition at line 352 of file rrt.c. References TRRTStatistics::dQrand, DynamicDomainRRT(), GetDynamicDomainRadius(), INF, TRRTStatistics::n, TRRTStatistics::nBranch, TRRTStatistics::nCollision, TRRTStatistics::nCollisionChecks, TRRTStatistics::nConvergenceError, TRRTStatistics::nDistanceIncreases, TRRTStatistics::nHighCost, TRRTStatistics::nNoEmptyBranch, TRRTStatistics::nNoEmptyTreeConnection, TRRTStatistics::nOutOfDomain, TRRTStatistics::nQrandReached, TRRTStatistics::nRandom, TRRTStatistics::nRejections, Trrt::ns, TRRTStatistics::nSample, TRRTStatistics::nStalled, TRRTStatistics::nStep, TRRTStatistics::nTooFar, and TRRTStatistics::nTreeConnection. Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), main(), and RRTstar(). ◆ DeleteRRTStatistics()
◆ InitRRT()
Defines a RRT with a single sample, the root.
Definition at line 1266 of file rrt.c. References TRRTSampleInfo::actions, Trrt::ambient, TRRTSampleInfo::blocked, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_IN_COLLISION, CS_WD_INIT_CD, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_DYNAMIC_DOMAIN, CT_EPSILON, CT_N_DOF, CT_SAMPLING, Trrt::da, Trrt::dd, TRRTSampleInfo::ddr, Trrt::delta, dr(), Error(), FALSE, TRRTSampleInfo::g, GetBoxInterval(), GetBoxNIntervals(), GetDistanceWeightsFromBox(), GetParameter(), GOAL2START, Trrt::graph, INIT_NUM_SAMPLES_RRT, Trrt::k, KDTREE_SAMPLING, LowerLimit(), TRRTSampleInfo::m, Trrt::m, Trrt::mode, Trrt::ms, Tbox::n, TRRTSampleInfo::n, Trrt::n, Trrt::nCores, NEW, NEWZ, Trrt::nFail, TRRTSampleInfo::nn, NO_UINT, TRRTSampleInfo::ns, Trrt::ns, ONE_TREE, Trrt::parallel, TRRTSampleInfo::parent, TRRTSampleInfo::path, PointInBoxTopology(), RRT_NN_TOPOLOGY, TRRTSampleInfo::samples, SetRRTTopology(), Trrt::si, START2GOAL, Trrt::temperature, TEMPERATURE_INIT, TRRTSampleInfo::time, TRRTSampleInfo::times, Trrt::tp, TRRTSampleInfo::tree, TRUE, TWO_TREES, TRRTSampleInfo::u, UpperLimit(), Trrt::w, and Trrt::weight. Referenced by InitAtlasRRT(), and main(). ◆ GetTRRTTemperature()
Returns the current value for the temperature of the T-RRT.
Definition at line 1575 of file rrt.c. References Trrt::temperature. Referenced by AtlasTRRT(). ◆ IsRRTGraph()Identifies RRTs where we store the whole set of neighnbours for each node. This is used in some variants of RRT* to accelerate the propagation of the path improvements.
Definition at line 1580 of file rrt.c. References Trrt::graph. Referenced by AtlasBiRRTstar(), AtlasRRTstar(), and WireAtlasRRTstar(). ◆ RRTSample()
Generates a random sample to expand the RRT. The sample can be generated with different policies according to the mode (from the ambient space, or near the existing nodes) and the goal (if defined the goal is used as random sample 1 out of 100 times).
Definition at line 1585 of file rrt.c. References Trrt::ambient, BOTHTREES, KDTREE_SAMPLING, Trrt::m, Trrt::mode, NewRRTRandomSample(), ONE_TREE, randomDouble(), RandomPointInBox(), and START2GOAL. Referenced by AtlasRRTSample(), BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), kinobiRRT(), kinoRRT(), and RRTstar(). ◆ RRTValidateSample()
Checks if a given sample is valid to expand the RRT.
Definition at line 1624 of file rrt.c. References CS_WD_SIMP_INEQUALITIES_HOLD, Trrt::dd, DistanceTopology(), GetRRTNN(), HEURISTIC_RRT_STAR, InDynamicDomain(), INF, Trrt::m, NewRRTSampleRejection(), TRRTSampleInfo::samples, Trrt::si, Trrt::tp, TRUE, and Trrt::w. Referenced by BiRRTstar(), cBiRRT(), ccRRT(), ccTRRT(), kinobiRRT(), kinoRRT(), and RRTstar(). ◆ PointInRRT()Points should not be duplicated in the RRT. This function detects such duplicated points. This is speciallly relevant in problems with dynamics where the actions are fixed. Then, a given node is always expanded in the same way and the next state may be already in the tree.
Definition at line 1661 of file rrt.c. References BOTHTREES, GOAL2START, INF, Trrt::m, Trrt::mode, Trrt::ns, ONE_TREE, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, TWO_TREES_WITH_SWAP, Trrt::weight, and WeightedDistanceTopologyMin(). Referenced by AddBranchToAtlasDynamicRRT(), kinobiRRT(), kinoEST(), and kinoRRT(). ◆ GetRRTNN()
Finds the sample in the tree that is closer (in the Euclidean distance) to the given point.
Definition at line 1750 of file rrt.c. References BOTHTREES, GOAL2START, INF, Trrt::m, Trrt::mode, NO_UINT, Trrt::ns, ONE_TREE, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, TWO_TREES_WITH_SWAP, Trrt::weight, and WeightedDistanceTopologyMin(). Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), AtlasRRTValidateSample(), BiRRTstar(), cBiRRT(), kinobiRRT(), PathStart2GoalInRRT(), and RRTValidateSample(). ◆ GetRRTNNInBall()
Finds the set of samples inside a ball of radious 'r' centered at a given point.
Definition at line 1855 of file rrt.c. References BOTHTREES, GOAL2START, Trrt::m, MEM_DUP, Trrt::mode, NEW, Trrt::ns, ONE_TREE, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, TWO_TREES_WITH_SWAP, Trrt::weight, and WeightedDistanceTopologyMin(). Referenced by AtlasBiRRTstar(), AtlasRRTstar(), BiRRTstar(), kinoEST(), and RRTstar(). ◆ GetRRTNNInBranch()
Finds the sample in the given tree that is close to a given branch in another tree. The branch is defined by samples n1 and n2 where n1 is antecesor of n2.
Definition at line 1976 of file rrt.c. References BOTHTREES, Error(), INF, Trrt::m, Trrt::mode, NO_UINT, Trrt::ns, ONE_TREE, TRRTSampleInfo::parent, TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES_WITH_SWAP, Trrt::weight, and WeightedDistanceTopologyMin(). Referenced by AtlasRRT(). ◆ AddNodeToRRT()
A debug utility to add points to RRTs without checking for collisions nor moving slowly toward to q_rand. It just adds the point and links it to the parent (i_near)
Definition at line 2067 of file rrt.c. References TRRTSampleInfo::actions, AddEdgeToRRT(), ArrayPi2Pi(), TRRTSampleInfo::blocked, BOTHTREES, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, Trrt::da, TRRTSampleInfo::ddr, DeleteTrajectory(), DistanceTopology(), Error(), FALSE, TRRTSampleInfo::g, GetParameter(), Trrt::graph, INF, TRRTSampleInfo::m, Trrt::m, MEM_DUP, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, NEW, NewRRTSample(), TRRTSampleInfo::nn, NO_UINT, TRRTSampleInfo::ns, Trrt::ns, TRRTSampleInfo::parent, TRRTSampleInfo::path, TRRTSampleInfo::samples, Trrt::si, START2GOAL, TRRTSampleInfo::time, TRRTSampleInfo::times, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, TRRTSampleInfo::u, and TRRTSampleInfo::userInfo. Referenced by AddBranchToRRT(), AddSample2AtlasRRT(), AddStepToRRTstar(), kinobiRRT(), kinoEST(), kinoRRT(), and PathStart2GoalInRRT(). ◆ AddEdgeToRRT()
Stores an edge between nodes 'i' and 'j' with cost 'c' This only has effect if the RRT is in graph mode.
Definition at line 761 of file rrt.c. References TRRTSampleInfo::cn, Error(), Trrt::graph, TRRTSampleInfo::m, MEM_DUP, MEM_EXPAND, TRRTSampleInfo::n, TRRTSampleInfo::nn, Trrt::ns, and Trrt::si. Referenced by AddNodeToRRT(), AtlasBiRRTstar(), BiRRTstar(), WireAtlasRRTstar(), and WireRRTstar(). ◆ TransitionTestRRT()
transition Test for Transition RRT
Definition at line 786 of file rrt.c. References TRRTSampleInfo::cost, CT_COEF_TEMP, CT_EPSILON, CT_NFAIL_MAX, GetParameter(), Trrt::nFail, randomDouble(), Trrt::si, Trrt::temperature, and TRUE. Referenced by AddBranchToRRT(), and NewTemptativeSample(). ◆ RecursiveReWireRRTstar()
A version of ReWireRRTstar to be used when the RRT is in graph mode. This re-wire uses the neighbours and cost-to-neighbour stored in the RRT nodes and propagages the recursively propagates the updates to all the necessary nodes in the graph.
Definition at line 2411 of file rrt.c. References ChangeBiRRTSteps(), TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CT_EPSILON, DistanceTopology(), Error(), ExtractMinElement(), FirstInPair(), TRRTSampleInfo::g, GetHeapElement(), GetParameter(), Trrt::graph, HeapEmpty(), Trrt::m, Trrt::mode, TRRTSampleInfo::n, NewDoublePair(), TRRTSampleInfo::nn, NO_UINT, TRRTSampleInfo::parent, TRRTSampleInfo::samples, SetRRTCostAndParent(), Trrt::si, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES_WITH_SWAP, UpdateCostAndTree(), and UpdateHeapElement(). Referenced by AtlasBiRRTstar(), AtlasRRTstar(), BiRRTstar(), and RRTstar(). ◆ RRTstar()
Tries to determine an optimal path to the goal using the RRT* method by S. Karaman and E. Frazzoli "Sampling-based algorithms for optimal motion planning" International Journal of Robotics Research, 2011 but adapted to operate on manifolds. In this case the connections of the connection between points on the manifold is done using the method by Berenson, D., Srinivasa, S., and Kuffner, J. (2011). Task space regions: A framework for pose-constrained manipulation planning. International Journal of Robotics Research. doi 10.1177/0278364910396389. Note that in this case the RRT construction is not stopped when the goal is reached but continues refining the path for the maximum time allowed for the tree construction. If constant GAMMA is 0 this procedure only finds the first path to the goal and then stops (to attempt to improve the path is done).
Definition at line 2618 of file rrt.c. References AccumulateRRTStatistics(), AddStepToRRTstar(), Trrt::ambient, BiRRTstar(), CopyDoublePair(), TRRTSampleInfo::cost, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteDoublePair(), DeleteHeap(), DeleteRRTStatistics(), DeleteStatistics(), Trrt::delta, DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNNInBall(), Trrt::graph, HEURISTIC_RRT_STAR, INF, InitHeap(), InitRRTStatistics(), InitStatistics(), Trrt::k, LessThanDoublePair(), Trrt::m, Trrt::mode, Trrt::ms, Trrt::n, Trrt::nCores, NEW, NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PointInBoxTopology(), PrintRRTStatistics(), ReconstructRRTPath(), RecursiveReWireRRTstar(), ReWireRRTstar(), RRTSample(), RRTstarCloseIteration(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, UpdateCostAndTree(), Trrt::w, and WireRRTstar(). Referenced by BiRRTstar(), and main(). ◆ BiRRTstar()
This is the same as RRTstar but implements a bi-directional search.
Definition at line 2851 of file rrt.c. References AccumulateRRTStatistics(), AddEdgeToRRT(), AddStepToRRTstar(), Trrt::ambient, BiRRTstarCloseIteration(), BOTHTREES, ChangeBiRRTSteps(), ConnectSamples(), CopyDoublePair(), CopyRRTStep(), TRRTSampleInfo::cost, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteDoublePair(), DeleteHeap(), DeleteRRTStatistics(), DeleteStatistics(), DeleteVector(), Trrt::delta, DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GetRRTNNInBall(), GOAL2START, Trrt::graph, HEURISTIC_RRT_STAR, INF, InitHeap(), InitRRTStatistics(), InitStatistics(), InitVector(), Trrt::k, LessThanDoublePair(), Trrt::m, Trrt::mode, Trrt::ms, Trrt::n, Trrt::nCores, NEW, NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PrintRRTStatistics(), RecursiveReWireRRTstar(), ReWireRRTstar(), RRTSample(), RRTstar(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Steps2PathinRRT(), Trrt::tp, TRRTSampleInfo::tree, TRUE, TWO_TREES_WITH_SWAP, UpdateBiRRTSteps(), Trrt::w, and WireRRTstar(). Referenced by RRTstar(). ◆ ccRRT()
Adds as many branches as necessary to the RRT (using AddBranchToRRT) until a targed configuration is reached (approached at a small distance). The tree extension is performed using the strategy defined in Dalibard, S., Nakhaei, A., Lamiraux, F., and Laumond, J.-P. (2009). Whole-body task planning for a humanoid robot: a way to integrate collision avoidance. In IEEE-RAS International Conference on Humanoid Robots, pages 355-360.
Definition at line 3233 of file rrt.c. References AccumulateRRTStatistics(), AddBranchToRRT(), Trrt::ambient, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), INF, InitRRTStatistics(), InitStatistics(), Trrt::m, Trrt::mode, Trrt::n, Trrt::nCores, NEW, NewRRTBranch(), NewRRTDistanceQrand(), NewRRTNoEmptyBranch(), NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PathStart2GoalInRRT(), PointInBoxTopology(), PrintRRTStatistics(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, Trrt::w, and Warning(). Referenced by AddBranchToRRT(), and main(). ◆ ccTRRT()
Adds as many branches as necessary to the RRT (using AddBranchToRRT) until a targed configuration is reached (approached at a small distance). The difference with respect ccRRT is that here we take into account a cost function using the strategy proposed in L. Jaillet, J. Cortes, T. Simeon, Sampling-based path planning on configuration-space costmaps IEEE Transactions on Robotics, Vol. 26(4), pp. 635 - 646, 2010. http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5477164
Definition at line 3090 of file rrt.c. References AccumulateRRTStatistics(), AddBranchToRRT(), Trrt::ambient, TRRTSampleInfo::cost, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNode(), INF, InitRRTStatistics(), InitStatistics(), Trrt::m, Trrt::mode, Trrt::n, Trrt::nCores, NEW, NewRRTBranch(), NewRRTDistanceQrand(), NewRRTNoEmptyBranch(), NewRRTSample(), NO_UINT, Trrt::ns, ONE_TREE, PathStart2GoalInRRT(), PointInBoxTopology(), PrintRRTStatistics(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, Trrt::w, and Warning(). Referenced by main(). ◆ cBiRRT()
Adds as many branches as necessary to the RRT (using AddBranchToRRT) until a targed configuration is reached (approached at a small distance). The tree extension is performed using the strategy defined in Berenson, D., Srinivasa, S., and Kuffner, J. (2011). Task space regions: A framework for pose-constrained manipulation planning. International Journal of Robotics Research. doi 10.1177/0278364910396389.
Definition at line 3366 of file rrt.c. References AccumulateRRTStatistics(), AddBranchToRRT(), Trrt::ambient, CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_SOLUTION_POINT, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GOAL2START, INF, InitRRTStatistics(), InitStatistics(), Trrt::m, Trrt::mode, Trrt::n, Trrt::nCores, NEW, NewRRTBranch(), NewRRTDistanceQrand(), NewRRTNoEmptyBranch(), NewRRTNoEmptyTreeConnection(), NewRRTSample(), NewRRTTreeConnection(), Trrt::ns, PathStart2GoalInRRT(), PointInBoxTopology(), PrintRRTStatistics(), RRT_VERBOSE, RRTSample(), RRTValidateSample(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRUE, TWO_TREES, Trrt::w, and Warning(). Referenced by main(). ◆ RRTPathSteps()Produces a vector with the nodes connecting a given node with the root of the RRT. The vector is created inside this function but must be destroyed externally.
Definition at line 3546 of file rrt.c. References CopyRRTStep(), TRRTStep::cost, TRRTSampleInfo::costp, TRRTStep::id, InitVector(), NewVectorElement(), NO_UINT, TRRTSampleInfo::parent, and Trrt::si. Referenced by ChangeBiRRTSteps(), and ReconstructRRTPath(). ◆ ChangeBiRRTSteps()
Replaces a given path by another passing by two nodes in the two trees of a bidirectional RRT. Note that in this case the path is just a list of nodes to be visited in order. The actual path between them is not computed, but will be computed at the end of the planning (this saves time during planning).
Definition at line 3567 of file rrt.c. References ConcatVectors(), TRRTStep::cost, Trrt::da, DeleteVector(), Error(), GetVectorElement(), TRRTStep::id, NewVectorElement(), ResetVector(), RRTPathSteps(), Trrt::si, START2GOAL, TRRTSampleInfo::tree, and VectorSize(). Referenced by AtlasBiRRTstar(), BiRRTstar(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), and UpdateBiRRTSteps(). ◆ UpdateBiRRTSteps()Tries to incorporate the recent changes in the trees to the optimal path. The optimal paths in a BiRRT are defined from two nodes, each one in a different tree. This path is represented by a list of nodes. However, the optimal path can be improved taking into accont the wires and rewires in the trees. This functions takes the list of nodes that define the optimal path and incorporates the shortcuts discobered in the tree since the path was defined.
Definition at line 3629 of file rrt.c. References ChangeBiRRTSteps(), TRRTStep::cost, TRRTSampleInfo::cost, Error(), GetVectorElement(), TRRTStep::id, INF, NO_UINT, Trrt::si, TRRTSampleInfo::tree, UpdateCostAndTree(), and VectorSize(). Referenced by AtlasBiRRTstar(), and BiRRTstar(). ◆ ReconstructRRTPath()
Reconstruct the path from the tree root to the sample in the tree closer to the goal. This is typically used to get the targed is reached.
Definition at line 1065 of file rrt.c. References DeleteVector(), RRTPathSteps(), and Steps2PathinRRT(). Referenced by main(), PathStart2GoalInRRT(), and RRTstar(). ◆ PathStart2GoalInRRT()
Determines the path from start to goal using an RRT, if one exists.
Definition at line 3674 of file rrt.c. References AddNodeToRRT(), CT_DYNAMIC_GOAL_ERROR, CT_DYNAMICS, Trrt::da, DeleteTrajectory(), Trrt::delta, DistanceTopology(), FALSE, GetParameter(), GetRRTNN(), Trrt::m, Trrt::mode, Trrt::ns, ONE_TREE, ReconstructRRTPath(), ReverseConcatTrajectories(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, Trrt::tp, TRRTSampleInfo::tree, TRUE, Trrt::weight, and WeightedDistanceTopology(). Referenced by AtlasRRT(), AtlasTRRT(), cBiRRT(), ccRRT(), ccTRRT(), kinobiRRT(), kinoEST(), and kinoRRT(). ◆ Bidirectional()Identifies bidirectional RRTs.
Definition at line 3785 of file rrt.c. References Trrt::mode, and ONE_TREE. ◆ GetRRTMode()
Identifies mode of the RRTs.
Definition at line 3791 of file rrt.c. References Trrt::mode. Referenced by kinobiRRT(), kinoEST(), kinoRRT(), ReWireAtlasRRTstar(), and WireAtlasRRTstar(). ◆ InDynamicDomain()Checks if a sample is in the dynamic domaiin of a given node. Random samples out of the dynamic domain should be rejected.
Definition at line 3796 of file rrt.c. References Trrt::dd, TRRTSampleInfo::ddr, DistanceTopology(), Trrt::m, TRRTSampleInfo::samples, Trrt::si, Trrt::tp, and TRUE. Referenced by AtlasRRTValidateSample(), and RRTValidateSample(). ◆ AdjustDynamicDomain()If the dynamic domain technique is used, and a problem is detected when extending a branch from a given node, we set the dynamic domain for this node so that new random samples outside the dynamic domain are rejected.
Definition at line 3804 of file rrt.c. References Trrt::dd, TRRTSampleInfo::ddr, MOV_AVG_DOWN, MOV_AVG_UP, and Trrt::si. Referenced by AddBranchToAtlasRRT(), AddBranchToRRT(), and AddStepToRRTstar(). ◆ GetDynamicDomainRadius()
Return the dynamic domain radious for a given sample. If the dynamic domain technique is not active or the radiius for the sample has not been set yet, this function returns 0.
Definition at line 3836 of file rrt.c. References Trrt::dd, TRRTSampleInfo::ddr, and Trrt::si. Referenced by PrintAtlasRRTStatistics(), and PrintRRTStatistics(). ◆ DynamicDomainRRT()TRUE if the dynamic domain technique is used. Using this technique the random sample are rejected if they are no in the dynamic domain of the nearest-neighbour sample.
Definition at line 3844 of file rrt.c. References Trrt::dd. Referenced by PrintAtlasRRTStatistics(), and PrintRRTStatistics(). ◆ GetRRTNumNodes()
Returns the number of nodes (samples) in the RRT.
Definition at line 3849 of file rrt.c. References Trrt::ns. Referenced by InitAtlasRRT(), kinobiRRT(), kinoEST(), kinoRRT(), and main(). ◆ GetRRTActionSpaceDimension()
◆ GetRRTNode()
Returns a pointer to the sample in the RRT with the given identifier.
Definition at line 3859 of file rrt.c. References Trrt::ns, TRRTSampleInfo::samples, and Trrt::si. Referenced by AddSample2AtlasRRT(), AtlasTRRT(), ccTRRT(), InitAtlasRRT(), kinobiRRT(), kinoEST(), kinoRRT(), LoadAtlasRRTSampleInfo(), and main(). ◆ GetRRTNodeTree()
In bidirectional RRTs, identifies the tree including a given node.
Definition at line 3867 of file rrt.c. References Trrt::mode, NO_UINT, Trrt::ns, ONE_TREE, Trrt::si, START2GOAL, and TRRTSampleInfo::tree. Referenced by AddBranchToAtlasDynamicRRT(), AtlasBiRRTstar(), GetRRTNNInChart(), InitBranchState(), main(), PlotAtlasRRT(), PopulateWithSamples(), ReWireAtlasRRTstar(), Time2GoNNToTree(), and WireAtlasRRTstar(). ◆ GetRRTParent()
Returns the identifier of the parent for a given RRT node.
Definition at line 3880 of file rrt.c. References NO_UINT, Trrt::ns, TRRTSampleInfo::parent, and Trrt::si. Referenced by AddStepToAtlasRRTstar(), PlotAtlasRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), and WireAtlasRRTstar(). ◆ SetRRTParent()
Changes the parent for a given node in the RRT. By changing the parent we also change the tree of the node to that of its new parent.
Definition at line 3888 of file rrt.c. References Error(), Trrt::mode, NO_UINT, Trrt::ns, TRRTSampleInfo::parent, Trrt::si, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP. Referenced by SmoothPathInAtlasRRT(). ◆ GetRRTAction()
Action executed to get the RRT node from the parent node. Only used in dynamical systems.
Definition at line 3905 of file rrt.c. References Trrt::ns, Trrt::si, and TRRTSampleInfo::u. ◆ GetRRTTime()
Time taken to reach the RRT node form the parent node executing the action returned by GetRRTAction Only used in dynamical systems.
Definition at line 3913 of file rrt.c. References Trrt::ns, Trrt::si, and TRRTSampleInfo::time. ◆ GetRRTTimeFromRoot()
Time taken to reach the RRT node form the root node. Only used in dynamical systems.
Definition at line 3921 of file rrt.c. References NO_UINT, Trrt::ns, TRRTSampleInfo::parent, Trrt::si, and TRRTSampleInfo::time. Referenced by main(). ◆ GetRRTNodeInfo()
Returns the user info associated with a node.
Definition at line 3942 of file rrt.c. References Trrt::si, and TRRTSampleInfo::userInfo. ◆ SetRRTNodeInfo()
Changes the user info associated with a node.
Definition at line 3950 of file rrt.c. References Error(), Trrt::si, and TRRTSampleInfo::userInfo. ◆ GetRRTNodeCost()
Returns the cost associated with a node.
Definition at line 3966 of file rrt.c. References TRRTSampleInfo::cost, and Trrt::si. Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), AtlasRRTstar(), AtlasRRTstarCloseIteration(), kinoEST(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar(). ◆ GetRRTNodeCostFromParent()
Returns the cost from the parent node.
Definition at line 3958 of file rrt.c. References TRRTSampleInfo::costp, and Trrt::si. Referenced by ReWireAtlasRRTstar(), and WireAtlasRRTstar(). ◆ SetRRTNodeCost()
Changes the cost associated with a node.
Definition at line 3974 of file rrt.c. References TRRTSampleInfo::cost, TRRTSampleInfo::costp, Error(), and Trrt::si. Referenced by AddStepToAtlasRRTstar(), AtlasTRRT(), kinoEST(), and SmoothPathInAtlasRRT(). ◆ SetRRTCostAndParent()
Changes the cost and the parent associated with a node.
Definition at line 3985 of file rrt.c. References TRRTSampleInfo::cost, TRRTSampleInfo::costp, Error(), Trrt::mode, TRRTSampleInfo::ns, TRRTSampleInfo::parent, Trrt::si, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP. Referenced by RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), WireAtlasRRTstar(), and WireRRTstar(). ◆ RRTBlockedProb()
Estimates the prob of a node to lead to an unfruitfull extension (to a collision or to a domain limit). There is always a marginal prob of a node of leading to a good extension. In this way the prob. completeness is maintained.
Definition at line 4007 of file rrt.c. References TRRTSampleInfo::blocked, and Trrt::si. Referenced by AtlasRRTValidateSample(). ◆ GetRRTTopology()
Gives access to the topology for each variable stored in the RRT
Definition at line 4025 of file rrt.c. References Trrt::tp. Referenced by InitAtlasRRT(), and LoadAtlasRRT(). ◆ GetRRTWeights()
Returns the weights for the distances between samples. Such weights are eventually used for nearest-neightbour determination (see WEIGTHED_NN) and for determining the distance to the goal (see WEIGTHED_REACHED_GOAL).
Definition at line 4030 of file rrt.c. References Trrt::weight. Referenced by InitAtlasRRT(), and LoadAtlasRRT(). ◆ CostToRoot()
Computes the cost from a node to the root by adding the individual cost of the node-parent relations. During re-wires the cost of a node can change without being reflected in the cost associated with this node due to changes in the branch including the node that are not yet propagated. This function computes the true cost with full propagation given the current tree.
Definition at line 4035 of file rrt.c. References TRRTSampleInfo::costp, NO_UINT, TRRTSampleInfo::parent, and Trrt::si. Referenced by AtlasRRTstarCloseIteration(), RRTstarCloseIteration(), and UpdateCostToRoot(). ◆ UpdateCostToRoot()
Updates the cost from a node to the root by adding the individual cost of the node-parent relations.
Definition at line 4050 of file rrt.c. References TRRTSampleInfo::cost, CostToRoot(), and Trrt::si. ◆ UpdateTree()
Checks the tree including the current node and assigns it to this tree. During re-wires the actual tree of a node can change due to changes along the branch including this tree that are not propagated to it. This functions makes sure there is no inconsistency at least for the selected node.
Definition at line 4055 of file rrt.c. References Error(), GOAL2START, Trrt::mode, NO_UINT, TRRTSampleInfo::parent, Trrt::si, START2GOAL, TRRTSampleInfo::tree, and TWO_TREES_WITH_SWAP. Referenced by PlotRRT(). ◆ UpdateCostAndTree()
A combination of UpdateCostToRoot and UpdateTree defined to gain some efficiency.
Definition at line 4084 of file rrt.c. References TRRTSampleInfo::cost, TRRTSampleInfo::costp, GOAL2START, NO_UINT, TRRTSampleInfo::parent, Trrt::si, START2GOAL, and TRRTSampleInfo::tree. Referenced by AtlasRRTstar(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), ReWireRRTstar(), RRTstar(), UpdateBiRRTSteps(), WireAtlasRRTstar(), and WireRRTstar(). ◆ StepsToRoot()
Computes the number of steps from a node to the root.
Definition at line 4111 of file rrt.c. References NO_UINT, TRRTSampleInfo::parent, and Trrt::si. Referenced by SmoothPathInAtlasRRT(). ◆ PlotRRT()
Plots a 3d projection of a RRT defined on a manifold. Although the ambient space can have arbitrary dimension we project it on 3 dimensions. Only 2D manifolds plots can be properly visualized. The output plot can be visualized using geomview.
Definition at line 4126 of file rrt.c. References Close3dObject(), ClosePlot3d(), CORRECT_POINT_PLOT, CS_WD_REGENERATE_ORIGINAL_POINT, CT_CUT_X, CT_CUT_Y, CT_CUT_Z, CT_DELTA, DeleteColor(), FALSE, GetParameter(), GOAL2START, InitPlot3d(), M_PI, Trrt::mode, NEW, NewColor(), TRRTSampleInfo::ns, Trrt::ns, ONE_TREE, TRRTSampleInfo::parent, TRRTSampleInfo::path, PlotSphere(), PlotVect3d(), TRRTSampleInfo::samples, Trrt::si, START2GOAL, StartNew3dObject(), TRRTSampleInfo::tree, TWO_TREES_WITH_SWAP, UpdateTree(), and Trrt::w. Referenced by main(), and PlotAtlasRRT(). ◆ SaveRRTNodes()
Stores the nodes of the RRT in the form of boxes.
Definition at line 4350 of file rrt.c. References CreateFileName(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, DeleteFileName(), Error(), GetFileFullName(), Trrt::ns, TRRTSampleInfo::samples, Trrt::si, SOL_EXT, SOL_WITH_DUMMIES_EXT, and Trrt::w. Referenced by main(). ◆ SaveRRTCosts()
Stores the cost associated with the nodes of the RRT. This is typically used when dealing with molecules. In this case the cost is the energy.
Definition at line 4401 of file rrt.c. References TRRTSampleInfo::cost, COST_EXT, CreateFileName(), DeleteFileName(), Error(), GetFileFullName(), INF, Trrt::ns, and Trrt::si. Referenced by main(). ◆ RRTMemSize()
Returns the approximated memory used (in bytes) by a given RRT.
Definition at line 4440 of file rrt.c. References Trrt::graph, Trrt::m, TRRTSampleInfo::nn, Trrt::ns, and Trrt::si. Referenced by AtlasRRTMemSize(), and main(). ◆ SaveRRT()Stores all the information in the RRT in a file.
Definition at line 4457 of file rrt.c. References TRRTSampleInfo::actions, TRRTSampleInfo::blocked, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, Trrt::da, Trrt::dd, TRRTSampleInfo::ddr, Trrt::delta, Error(), TRRTSampleInfo::g, GetFileFullName(), Trrt::graph, Trrt::k, TRRTSampleInfo::m, Trrt::m, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, Trrt::n, Trrt::nFail, TRRTSampleInfo::nn, TRRTSampleInfo::ns, Trrt::ns, TRRTSampleInfo::parent, TRRTSampleInfo::path, TRRTSampleInfo::samples, Trrt::si, Trrt::temperature, TRRTSampleInfo::time, TRRTSampleInfo::times, TRRTSampleInfo::tree, and TRRTSampleInfo::u. Referenced by main(), and SaveAtlasRRT(). ◆ LoadRRT()
Construct a RRT from the information previously stored in a file by SaveRRT. The user information stored in the node (see GetRRTNodeInfo and SetRRTNodeInfo) is lost when storing the RRT in a file. Right now we never store user info in the RRT nodes and, thus this limitation has no effect.
Definition at line 4534 of file rrt.c. References TRRTSampleInfo::actions, Trrt::ambient, TRRTSampleInfo::blocked, TRRTSampleInfo::cn, TRRTSampleInfo::cost, TRRTSampleInfo::costp, CS_WD_GENERATE_SIMP_INITIAL_BOX, Trrt::da, Trrt::dd, TRRTSampleInfo::ddr, Trrt::delta, dr(), Error(), FALSE, TRRTSampleInfo::g, GetBoxInterval(), GetBoxNIntervals(), GetDistanceWeightsFromBox(), GetFileFullName(), Trrt::graph, Trrt::k, LowerLimit(), TRRTSampleInfo::m, Trrt::m, Trrt::mode, Trrt::ms, TRRTSampleInfo::n, Trrt::n, Trrt::nCores, NEW, Trrt::nFail, TRRTSampleInfo::nn, TRRTSampleInfo::ns, Trrt::ns, Trrt::parallel, TRRTSampleInfo::parent, TRRTSampleInfo::path, RRT_NN_TOPOLOGY, TRRTSampleInfo::samples, SetRRTTopology(), Trrt::si, START2GOAL, Trrt::temperature, TRRTSampleInfo::time, TRRTSampleInfo::times, Trrt::tp, TRRTSampleInfo::tree, TWO_TREES, TRRTSampleInfo::u, UpperLimit(), TRRTSampleInfo::userInfo, Trrt::w, and Trrt::weight. Referenced by LoadAtlasRRT(), and main(). ◆ PrintRRTDefines()
Prints the defines in rrt.h This is used only for debug purposes.
Definition at line 4759 of file rrt.c. References EXPLORATION_RRT, GET_RRT_STATISTICS, HEURISTIC_RRT_STAR, PENALIZE_BLOCKED_NODES, RRT_NN_TOPOLOGY, RRT_PLOT_NODES, RRT_VERBOSE, RRTSTAR_SYMMETRIC_COST, RRTSTAR_UPDATE_COSTS, TEMPERATURE_INIT, WEIGTHED_NN, and WEIGTHED_REACHED_GOAL. Referenced by main(), and PrintAtlasRRTDefines(). ◆ DeleteRRT()
Deletes the information stored in the RRT.
Definition at line 4776 of file rrt.c. References TRRTSampleInfo::actions, Trrt::ambient, TRRTSampleInfo::cn, Trrt::da, DeleteBox(), DeleteSamples(), DeleteTrajectory(), Trrt::graph, Trrt::mode, TRRTSampleInfo::n, TRRTSampleInfo::ns, Trrt::ns, TRRTSampleInfo::path, TRRTSampleInfo::samples, Trrt::si, TRRTSampleInfo::times, Trrt::tp, TWO_TREES, TRRTSampleInfo::u, and Trrt::weight. Referenced by DeleteAtlasRRT(), and main(). |
Follow us!