Detailed DescriptionImplementation of the AtlasRRT functions.
Definition in file atlasrrt.c.
Macro Definition Documentation
Adds all nodes generated in the branch extension. Definition at line 31 of file atlasrrt.c. Referenced by AddBranchToAtlasRRT(), AtlasRRT(), and AtlasTRRT().
Adds anly the last generated node during the branch extension. Definition at line 38 of file atlasrrt.c. Referenced by AddBranchToAtlasRRT(), and AddStepToAtlasRRTstar().
Adds anly the last generated node during the branch extension if it is not already in the tree. Definition at line 46 of file atlasrrt.c. Referenced by AddBranchToAtlasRRT(), and AddStepToAtlasRRTstar().
Do not add any node during the branch extension. Definition at line 53 of file atlasrrt.c. Referenced by AtlasBiRRTstar(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar(). Function Documentation
To be called everty time we start a new atlasRRT branch. In the context of RRT* this must be interpreted as the number of possible edges (number of sample pairs whose connections is attempted).
Definition at line 367 of file atlasrrt.c. References TAtlasRRTStatistics::nBranch. Referenced by AtlasRRT(), and WireAtlasRRTstar().
To be called every time we managed to add a non-emtpy branch to the AtlasRRT. In the context of RRT* this must be interpreted as the number of edges (number of sample pairs whose connections is valid).
Definition at line 372 of file atlasrrt.c. References TAtlasRRTStatistics::nNoEmptyBranch. Referenced by AtlasRRT(), and WireAtlasRRTstar().
New attempt to connect the two trees. This is only updated in bi-directional Atlas RRTs.
Definition at line 377 of file atlasrrt.c. References TAtlasRRTStatistics::nTreeConnection. Referenced by AtlasRRT().
New attempt to connect the two trees that resulted in a non empty branch. This is only updated in bi-directional Atlas RRTs.
Definition at line 382 of file atlasrrt.c. References TAtlasRRTStatistics::nNoEmptyTreeConnection. Referenced by AtlasRRT().
New distance from q_rand to the tree. These distances are accumulated and then we compute the average using the number of attempts for branch extension.
Definition at line 387 of file atlasrrt.c. References TAtlasRRTStatistics::dQrand. Referenced by AtlasRRT().
To be called everytime we add a new step to a branch.
Definition at line 392 of file atlasrrt.c. References TAtlasRRTStatistics::nStep. Referenced by NewTemptativeSample().
To be called when we create a new chart due to convergence issues in the current chart.
Definition at line 397 of file atlasrrt.c. References TAtlasRRTStatistics::nNoConvergent. Referenced by NewTemptativeSample().
To be called every time the random sample is reached.
Definition at line 402 of file atlasrrt.c. References TAtlasRRTStatistics::nQrandReached. Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().
To be called everty time a branch is stopped since it leaves the given domain.
Definition at line 407 of file atlasrrt.c. References TAtlasRRTStatistics::nNotInDomain. Referenced by NewTemptativeSample().
To be called every time a branch extension is stopped due to a collision.
Definition at line 412 of file atlasrrt.c. References TAtlasRRTStatistics::nCollision. Referenced by NewTemptativeSample().
To be called every time a branch extension is stopped because the branch is already too long.
Definition at line 417 of file atlasrrt.c. References TAtlasRRTStatistics::nTooLong. Referenced by AddBranchToAtlasRRT().
To be called every time a branch extension is stopped because the brach is scaping the ball defined with q_near, q_rand.
Definition at line 422 of file atlasrrt.c. References TAtlasRRTStatistics::nTooFar. Referenced by AddBranchToAtlasRRT().
To be called every time a branch extension is stopped to avoid the overlap with another branch.
Definition at line 427 of file atlasrrt.c. References TAtlasRRTStatistics::nOverlap. Referenced by NewTemptativeSample().
To be called every time the attempt to create a chart fails. This triggers the creation of a new chart in the precedent sample (where the chart was successfully created before).
Definition at line 437 of file atlasrrt.c. References TAtlasRRTStatistics::nErrorNewChart. Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().
To be called every time we create a new chart since a node reached the border of the current chart (i.e., it leaves the ball defining the maximum chart validity area).
Definition at line 432 of file atlasrrt.c. References TAtlasRRTStatistics::nOutOfChart. Referenced by NewTemptativeSample().
Called when there is a large curvature along a branch, and thus, a new chart has to be created. In this case the new chart has too large curvature error with respect to the current chart.
Definition at line 442 of file atlasrrt.c. References TAtlasRRTStatistics::nLargeCurvature. Referenced by NewTemptativeSample().
Called every time we create a new chart as it is the a cheap test that do not require the creation of temporary chart
Definition at line 447 of file atlasrrt.c. References TAtlasRRTStatistics::nDirLargeCurvature. Referenced by NewTemptativeSample().
To be called everytime we have to reduce the default size between two nodes on a atlasRRT branch to be able to generate a valid new node. The default step size is given by the DELTA parameter.
Definition at line 452 of file atlasrrt.c. References TAtlasRRTStatistics::nStepReduction. Referenced by AddBranchToAtlasRRT().
To be called every time we actually add a node (or sample) to the atlasRRT.
Definition at line 457 of file atlasrrt.c. References TAtlasRRTStatistics::nSample. Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().
To be called every time we actualy add a chart to the atlasRRT.
Definition at line 462 of file atlasrrt.c. References TAtlasRRTStatistics::nChart. Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().
When creating a new chart it should intersect with its parent chart. If this does not happen we have to record it. Definition at line 467 of file atlasrrt.c. References TAtlasRRTStatistics::nNoConnect. Referenced by AddBranchToAtlasRRT().
Number of times we try to create a char at a singular regions. Note that singularities are zero volume areas but due to the numerical tolerances they have some thickness and, thus,the probability of having points on this almost singular regions is not null. A point on a singularity blocks the extension of branches. If the number of times this happens is large, we could fail to solve a problem because we can not cross singular regions. A typical solution is to decrease parameter 'epsilon'.
Definition at line 472 of file atlasrrt.c. References TAtlasRRTStatistics::nSingular. Referenced by AddBranchToAtlasRRT().
Counts the number of random samples generated. Definition at line 477 of file atlasrrt.c. References TAtlasRRTStatistics::nRandom. Referenced by AtlasRRTSample().
Counts the number os random samples rejected. Definition at line 482 of file atlasrrt.c. References TAtlasRRTStatistics::nRejections. Referenced by AtlasRRTSample(), and AtlasRRTValidateSample().
New collision check. Definition at line 487 of file atlasrrt.c. References TAtlasRRTStatistics::nCollisionChecks. Referenced by NewTemptativeSample().
Init the Atlas RRT statistics.
Definition at line 325 of file atlasrrt.c. References TAtlasRRTStatistics::dQrand, TAtlasRRTStatistics::n, TAtlasRRTStatistics::nBranch, TAtlasRRTStatistics::nChart, TAtlasRRTStatistics::nCollision, TAtlasRRTStatistics::nCollisionChecks, TAtlasRRTStatistics::nDirLargeCurvature, TAtlasRRTStatistics::nErrorNewChart, TAtlasRRTStatistics::nLargeCurvature, TAtlasRRTStatistics::nNoConnect, TAtlasRRTStatistics::nNoConvergent, TAtlasRRTStatistics::nNoEmptyBranch, TAtlasRRTStatistics::nNoEmptyTreeConnection, TAtlasRRTStatistics::nNotInDomain, TAtlasRRTStatistics::nOutOfChart, TAtlasRRTStatistics::nOverlap, TAtlasRRTStatistics::nQrandReached, TAtlasRRTStatistics::nRandom, TAtlasRRTStatistics::nRejections, TAtlasRRTStatistics::nSample, TAtlasRRTStatistics::nSingular, TAtlasRRTStatistics::nStep, TAtlasRRTStatistics::nStepReduction, TAtlasRRTStatistics::nTooFar, TAtlasRRTStatistics::nTooLong, and TAtlasRRTStatistics::nTreeConnection. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and main().
Accumulates two sets of Atlas RRT statistics. This is used to accumulate statistics when executing a long sequence of experiments.
Definition at line 492 of file atlasrrt.c. References TAtlasRRTStatistics::dQrand, TAtlasRRTStatistics::n, TAtlasRRTStatistics::nBranch, TAtlasRRTStatistics::nChart, TAtlasRRTStatistics::nCollision, TAtlasRRTStatistics::nCollisionChecks, TAtlasRRTStatistics::nDirLargeCurvature, TAtlasRRTStatistics::nErrorNewChart, TAtlasRRTStatistics::nLargeCurvature, TAtlasRRTStatistics::nNoConnect, TAtlasRRTStatistics::nNoConvergent, TAtlasRRTStatistics::nNoEmptyBranch, TAtlasRRTStatistics::nNoEmptyTreeConnection, TAtlasRRTStatistics::nNotInDomain, TAtlasRRTStatistics::nOutOfChart, TAtlasRRTStatistics::nOverlap, TAtlasRRTStatistics::nQrandReached, TAtlasRRTStatistics::nRandom, TAtlasRRTStatistics::nRejections, TAtlasRRTStatistics::nSample, TAtlasRRTStatistics::nSingular, TAtlasRRTStatistics::nStep, TAtlasRRTStatistics::nStepReduction, TAtlasRRTStatistics::nTooFar, TAtlasRRTStatistics::nTooLong, and TAtlasRRTStatistics::nTreeConnection. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().
Prints a report about the collected atlasRRT statistics.
Definition at line 538 of file atlasrrt.c. References Tatlasrrt::atlas, TAtlasRRTStatistics::dQrand, DynamicDomainRRT(), GetAtlasChart(), GetChartSamplingRadius(), GetDynamicDomainRadius(), INF, TAtlasRRTStatistics::n, TAtlasRRTStatistics::nBranch, Tatlasrrt::nc, TAtlasRRTStatistics::nChart, TAtlasRRTStatistics::nCollision, TAtlasRRTStatistics::nCollisionChecks, TAtlasRRTStatistics::nDirLargeCurvature, TAtlasRRTStatistics::nErrorNewChart, TAtlasRRTStatistics::nLargeCurvature, TAtlasRRTStatistics::nNoConnect, TAtlasRRTStatistics::nNoConvergent, TAtlasRRTStatistics::nNoEmptyBranch, TAtlasRRTStatistics::nNoEmptyTreeConnection, TAtlasRRTStatistics::nNotInDomain, TAtlasRRTStatistics::nOutOfChart, TAtlasRRTStatistics::nOverlap, TAtlasRRTStatistics::nQrandReached, TAtlasRRTStatistics::nRandom, TAtlasRRTStatistics::nRejections, Tatlasrrt::ns, TAtlasRRTStatistics::nSample, TAtlasRRTStatistics::nSingular, TAtlasRRTStatistics::nStep, TAtlasRRTStatistics::nStepReduction, TAtlasRRTStatistics::nTooFar, TAtlasRRTStatistics::nTooLong, TAtlasRRTStatistics::nTreeConnection, and Tatlasrrt::rrt. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and main().
Deletes the atlasRRT statistics object.
Definition at line 745 of file atlasrrt.c. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and main().
Add a new branch to the AtlasRRT. The branch is extened until the given goal is reached or until a problem in the extension occurs (obstacles, convergence problems, singularities, etc.). Note that this function is also used to check if a given sample can be connected to the tree but without actually adding a branch to the tree. In this case, though the charts necessaries to connect the tree and the given sample are generated if not ye defined (i.e., while we do not add samples we might add charts. This extension-without-addition is basically used for RRT*. Please, see ConnectSamplesChart for a simpler version of this method.
Definition at line 1300 of file atlasrrt.c. References AccumulateVector(), ADD_ALL, ADD_LAST, ADD_LAST_NO_REP, AddChart2AtlasRRT(), AddSample2AtlasRRT(), AddSample2Samples(), AdjustDynamicDomain(), Tatlasrrt::atlas, TSampleInfo::c, CT_DELTA, CT_EPSILON, CT_MAX_NODES_RRT, CT_SAMPLING, DifferenceVectorTopology(), Distance(), DistanceTopology(), Error(), FALSE, TSampleInfo::generateChart, GetAtlasChart(), GetParameter(), GetRRTNN(), GetRRTNodeTree(), TSampleInfo::id, InitSamples(), Tatlasrrt::k, TSampleInfo::lsc, Tatlasrrt::m, Manifold2Chart(), NEW, NewAtlasRRTBlockBySingularity(), NewAtlasRRTChart(), NewAtlasRRTInitChartError(), NewAtlasRRTNoConnectToParent(), NewAtlasRRTQrandReached(), NewAtlasRRTSample(), NewAtlasRRTStepReduction(), NewAtlasRRTTooFar(), NewAtlasRRTTooLong(), NewTemptativeSample(), NO_UINT, Tatlasrrt::ns, TSampleInfo::pc, PointTowardRandSample(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, Tatlasrrt::tp, TRUE, and Warning(). Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), AtlasRRT(), AtlasTRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().
Generates a new sample interpolating in between q_near and q_rand and projecting to the manifold. This is used to extend a AtlasRRT branch. This function consideres the possible transition to a neighbouring chart of the new sample. If this transition occurs, the random point is also translated to the new chart.
Definition at line 1702 of file atlasrrt.c. References Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Tatlasrrt::ce, Chart2Manifold(), Tatlasrrt::ci, CloseCharts(), CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_SAMPLING, DeleteChart(), DetermineChartNeighbour(), Distance(), DistanceTopology(), Tatlasrrt::e, EnlargeChart(), FALSE, GetAtlasChart(), GetParameter(), TSampleInfo::id, InitChart(), InsideChartPolytope(), Tatlasrrt::J, Tatlasrrt::k, Tatlasrrt::m, Manifold2Chart(), NEW, NewAtlasRRTCollision(), NewAtlasRRTCollisionCheck(), NewAtlasRRTDirLargeCurvature(), NewAtlasRRTInitChartError(), NewAtlasRRTLargeCurvature(), NewAtlasRRTNoConvergentStep(), NewAtlasRRTNotInDomain(), NewAtlasRRTOutOfChart(), NewAtlasRRTOverlap(), NewAtlasRRTQrandReached(), NewAtlasRRTStep(), Norm(), PointInBoxTopology(), PointTowardRandSample(), Tatlasrrt::r, Tatlasrrt::rrt, TSampleInfo::s, SumVectorScale(), Tatlasrrt::tp, TransitionTestRRT(), TChartInfo::tree, TRUE, Tatlasrrt::w, and Warning(). Referenced by AddBranchToAtlasRRT().
Adds a sample to the AtlasRRT connected to a given sample already in the tree.
Definition at line 1967 of file atlasrrt.c. References AddChart2Tree(), AddNodeToRRT(), Tatlasrrt::atlas, TSampleInfo::c, Tatlasrrt::ci, DistanceTopology(), Error(), TSampleInfo::generateChart, GetAtlasChart(), GetRRTNode(), TSampleInfo::id, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, Manifold2Chart(), MEM_DUP, Tatlasrrt::ms, NEW, Tatlasrrt::ns, TSampleInfo::pc, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, and Tatlasrrt::tp. Referenced by AddBranchToAtlasRRT().
Used when a sample triggers the generation of a new chart.
Definition at line 2104 of file atlasrrt.c. References AddChart2Tree(), AddTrustedChart2Atlas(), Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, ChartNeighbourID(), ChartNumNeighbours(), Tatlasrrt::ci, EnlargeChart(), Error(), FALSE, ForceChartCut(), TSampleInfo::generateChart, GetAtlasChart(), GetAtlasNumCharts(), TSampleInfo::id, INIT_NUM_CHARTS, TChartInfo::it, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Manifold2Chart(), Tatlasrrt::mc, MEM_EXPAND, Tatlasrrt::nc, NEW, NO_UINT, TSampleInfo::pc, PopulateWithSamples(), TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, Tatlasrrt::tp, TChartInfo::tree, and TRUE. Referenced by AddBranchToAtlasRRT(), and InitAtlasRRT().
After generating a new chart (with identifier id) some of the samples assigned to their neighbours have to be assigned to the new chart.
Definition at line 2267 of file atlasrrt.c. References AddChart2Tree(), Tatlasrrt::atlas, TSampleInfo::c, ChartNeighbourID(), ChartNumNeighbours(), Tatlasrrt::ci, CT_EPSILON, DetermineChartNeighbour(), EnlargeChart(), GetAtlasChart(), GetParameter(), GetRRTNodeTree(), Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Manifold2Chart(), NEW, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, and Tatlasrrt::tp. Referenced by AddChart2AtlasRRT().
The tree must not contain the chart
Definition at line 2038 of file atlasrrt.c. References Tatlasrrt::birrt, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, Tatlasrrt::mct1, Tatlasrrt::mct2, MEM_DUP, Tatlasrrt::nct1, Tatlasrrt::nct2, START2GOAL, and TChartInfo::tree. Referenced by AddChart2AtlasRRT(), AddSample2AtlasRRT(), and PopulateWithSamples().
Determines a unitary vector on the current chart that points towards the random sample. Additionally, project the random sample to the current chart.
Definition at line 2060 of file atlasrrt.c. References Tatlasrrt::atlas, DifferenceVector(), FALSE, GetAtlasChart(), Tatlasrrt::k, Local2Global(), Manifold2Chart(), Norm(), Normalize(), SumVectorScale(), TANGENT_SAMPLING, and TRUE. Referenced by AddBranchToAtlasRRT(), and NewTemptativeSample().
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 2357 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), AddSample2Samples(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, DeleteSamples(), FALSE, GetRRTParent(), InitSamples(), NO_UINT, ReverseSamples(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TRUE, Tatlasrrt::w, and Warning(). Referenced by AtlasRRTstar().
Defines a path (a collection of close enough samples) from a set of steps defined as nodes in a AtlasRRT.
Definition at line 2424 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), AddSample2Samples(), CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CT_EPSILON, DeleteSamples(), FALSE, GetParameter(), GetRRTNodeCost(), GetVectorElement(), InitSamples(), NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TRUE, VectorSize(), Tatlasrrt::w, and Warning(). Referenced by AtlasBiRRTstar().
Local optimiziation of the length of the path to a node. Tries to re-wire the tree to minimize the path from the root to a given node.
Definition at line 2503 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), FALSE, GetRRTNodeCost(), GetRRTParent(), NEW, NO_UINT, randomMax(), Tatlasrrt::rrt, TSampleInfo::s, SetRRTNodeCost(), SetRRTParent(), Tatlasrrt::si, StepsToRoot(), and TRUE.
Tries to generate a node from i_near toward q_rand and, if possible, adds it to the AtlasRRT and updates the cost of the new node. Moreover, we check if the goal is actually reached. If so, we add another node to the tree representing the goal.
Definition at line 3561 of file atlasrrt.c. References ADD_LAST, ADD_LAST_NO_REP, AddBranchToAtlasRRT(), CT_EPSILON, DistanceTopology(), Error(), FALSE, GetParameter(), GetRRTNodeCost(), GetRRTParent(), Tatlasrrt::m, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, SetRRTNodeCost(), Tatlasrrt::si, Tatlasrrt::tp, and TRUE. Referenced by AtlasBiRRTstar(), and AtlasRRTstar().
Selects the best parent among all neighbouring nodes for a node just added to the tree.
Definition at line 3623 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), AddEdgeToRRT(), AddElement2Heap(), Tatlasrrt::ambient, ConnectSamplesChart(), CT_EPSILON, Error(), FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, IsRRTGraph(), Tatlasrrt::k, Tatlasrrt::m, NEW, NewAtlasRRTBranch(), NewAtlasRRTNoEmptyBranch(), NewDoublePair(), NO_UINT, Tatlasrrt::parallel, Tatlasrrt::rrt, TSampleInfo::s, SetRRTCostAndParent(), Tatlasrrt::si, Tatlasrrt::tp, TRUE, TWO_TREES_WITH_SWAP, UpdateCostAndTree(), and Tatlasrrt::w. Referenced by AtlasBiRRTstar(), and AtlasRRTstar().
Uses the last added node to the AtlasRRT to try to reduce the cost of the neighbouring nodes.
Definition at line 3764 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), ChangeBiRRTSteps(), CT_EPSILON, FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, SetRRTCostAndParent(), Tatlasrrt::si, TRUE, TWO_TREES_WITH_SWAP, and UpdateCostAndTree(). Referenced by AtlasBiRRTstar(), and AtlasRRTstar().
Prints information about the AtlasRRT* iteration.
Definition at line 3843 of file atlasrrt.c. References ATLASRRT_VERBOSE, CostToRoot(), GetRRTNodeCost(), Tatlasrrt::nc, NO_UINT, Tatlasrrt::ns, and Tatlasrrt::rrt. Referenced by AtlasRRTstar().
Prints information about the Bi-directional AtlasRRT* iteration.
Definition at line 3878 of file atlasrrt.c. References ATLASRRT_VERBOSE, INF, Tatlasrrt::nc, and Tatlasrrt::ns. Referenced by AtlasBiRRTstar().
Saves the information relating a given sample to charts in an AtlasRRT.
Definition at line 2808 of file atlasrrt.c. References TSampleInfo::c, TSampleInfo::generateChart, TSampleInfo::id, Tatlasrrt::k, TSampleInfo::lsc, TSampleInfo::pc, and TSampleInfo::t. Referenced by SaveAtlasRRT().
Saves the information relating a given chart to the samples it contains
Definition at line 2822 of file atlasrrt.c. References TChartInfo::it, TChartInfo::lc, and TChartInfo::tree. Referenced by SaveAtlasRRT().
Reads the information relating a given sample to charts in an AtlasRRT.
Definition at line 2829 of file atlasrrt.c. References TSampleInfo::c, TSampleInfo::generateChart, GetRRTNode(), TSampleInfo::id, Tatlasrrt::k, TSampleInfo::lsc, NEW, TSampleInfo::pc, Tatlasrrt::rrt, TSampleInfo::s, and TSampleInfo::t. Referenced by LoadAtlasRRT().
Reads the information relating a given chart to the samples it contains
Definition at line 2844 of file atlasrrt.c. References TChartInfo::it, TChartInfo::lc, and TChartInfo::tree. Referenced by LoadAtlasRRT().
Small debug utility to plot the position of q_rand.
Definition at line 2652 of file atlasrrt.c. References Tatlasrrt::atlas, Close3dObject(), ClosePlot3d(), CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, DeleteColor(), DeleteFileName(), FALSE, GetAtlasChart(), GetChartCenter(), GetFileFullName(), InitPlot3d(), Local2Global(), Tatlasrrt::m, NEW, NewColor(), PLOT3D_EXT, PlotVect3d(), TSampleInfo::s, Tatlasrrt::si, StartNew3dObject(), Tatlasrrt::tp, and Tatlasrrt::w.
Graphical representation of the attempt to connect two trees.
Definition at line 2749 of file atlasrrt.c. References Close3dObject(), ClosePlot3d(), CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, DeleteColor(), DeleteFileName(), FALSE, GetFileFullName(), InitPlot3d(), NewColor(), PLOT3D_EXT, PlotVect3d(), TSampleInfo::s, Tatlasrrt::si, StartNew3dObject(), and Tatlasrrt::w.
Selects a random point in the set of charts reached by a given tree.
Definition at line 2562 of file atlasrrt.c. References Tatlasrrt::atlas, Tatlasrrt::birrt, BOTHTREES, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, GetAtlasChart(), GetAtlasNumCharts(), GetChartSamplingRadius(), Tatlasrrt::k, Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, randomMax(), RandomPointInAtlas(), RandomPointInChart(), randomWithDistribution(), START2GOAL, and Tatlasrrt::tp. Referenced by AtlasRRTSample().
Defines a Atlas-RRT with a single sample, the root.
Definition at line 2855 of file atlasrrt.c. References AddChart2AtlasRRT(), Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, CT_CE, CT_E, CT_N_DOF, CT_R, Tatlasrrt::e, Error(), EXPLORATION_RRT, FALSE, TSampleInfo::generateChart, GetAtlasNumCharts(), GetBoxNIntervals(), GetParameter(), GetRRTNode(), GetRRTNumNodes(), GetRRTTopology(), GOAL2START, TSampleInfo::id, INIT_NUM_CHARTS, INIT_NUM_SAMPLES_RRT, InitAtlasFromPoint(), InitRRT(), TChartInfo::it, Tatlasrrt::J, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, Tatlasrrt::mc, Tatlasrrt::mct1, Tatlasrrt::mct2, Tatlasrrt::ms, Tatlasrrt::nc, Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, NO_UINT, Tatlasrrt::ns, ONE_TREE, Tatlasrrt::parallel, TSampleInfo::pc, Tatlasrrt::r, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, TSampleInfo::t, Tatlasrrt::tp, TChartInfo::tree, TRUE, and Tatlasrrt::w. Referenced by main().
Generates a random sample to expand the AtlasRRT. 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 2993 of file atlasrrt.c. References AMBIENT_SAMPLING, Tatlasrrt::atlas, Tatlasrrt::birrt, CS_WD_SIMP_INEQUALITIES_HOLD, DecreaseChartSamplingRadius(), Error(), FALSE, GetAtlasChart(), IncreaseChartSamplingRadius(), Tatlasrrt::k, KDTREE_SAMPLING, Tatlasrrt::m, NewAtlasRRTRandomSample(), NewAtlasRRTSampleRejection(), NO_UINT, Norm(), Tatlasrrt::r, randomDouble(), RandomPointInAtlasTree(), Tatlasrrt::rrt, RRTSample(), TANGENT_SAMPLING, and Tatlasrrt::w. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().
Checks if a given sample is valid to expand the AtlasRRT.
Definition at line 3081 of file atlasrrt.c. References TSampleInfo::c, DistanceTopology(), GetRRTNN(), GetRRTNNInNeighbourChart(), HEURISTIC_RRT_STAR, InDynamicDomain(), INF, Tatlasrrt::m, NewAtlasRRTSampleRejection(), NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, Tatlasrrt::tp, and TRUE. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), and AtlasTRRT().
Adds as many branches as necessary to the Atlas-RRT until a targed configuration is reached (approached at a small distance).
Definition at line 3133 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), ADD_ALL, AddBranchToAtlasRRT(), Tatlasrrt::ambient, Tatlasrrt::atlas, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, 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_DETECT_BIFURCATIONS, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DecreaseChartSamplingRadius(), DeleteAtlasRRTStatistics(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetAtlasChart(), GetElapsedTime(), GetParameter(), GetRRTNNInBranch(), GOAL2START, IncreaseChartSamplingRadius(), INF, InitAtlasRRTStatistics(), InitStatistics(), Tatlasrrt::k, Tatlasrrt::m, MOV_AVG_DOWN, MOV_AVG_UP, Tatlasrrt::nc, NEW, NewAtlasRRTBranch(), NewAtlasRRTChart(), NewAtlasRRTDistanceQrand(), NewAtlasRRTNoEmptyBranch(), NewAtlasRRTNoEmptyTreeConnection(), NewAtlasRRTSample(), NewAtlasRRTTreeConnection(), NO_UINT, Tatlasrrt::ns, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, TANGENT_SAMPLING, Tatlasrrt::tp, TRUE, and Tatlasrrt::w. Referenced by main().
Adds as many branches as necessary to the Atlas-TRRT until a targed configuration is reached (approached at a small distance). The difference with respect AtlasRRT 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 3399 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), ADD_ALL, AddBranchToAtlasRRT(), Tatlasrrt::ambient, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, 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_DETECT_BIFURCATIONS, CT_EPSILON, CT_MAX_NODES_RRT, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteStatistics(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNode(), GetTRRTTemperature(), INF, InitAtlasRRTStatistics(), InitStatistics(), Tatlasrrt::k, Tatlasrrt::m, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), Tatlasrrt::rrt, SetRRTNodeCost(), START2GOAL, Tatlasrrt::tp, TRUE, and Tatlasrrt::w. Referenced by main().
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. This is the atlas-based version of RRTstar. The main difference is that the atlas is used to sample new nodes and to determine paths between samples. 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 3908 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), AddStepToAtlasRRTstar(), Tatlasrrt::ambient, AtlasBiRRTstar(), AtlasRRTSample(), AtlasRRTstarCloseIteration(), AtlasRRTValidateSample(), Tatlasrrt::birrt, CopyDoublePair(), 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_DELTA, CT_EPSILON, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteDoublePair(), DeleteHeap(), DeleteStatistics(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNNInBall(), GetRRTNodeCost(), HEURISTIC_RRT_STAR, INF, InitAtlasRRTStatistics(), InitHeap(), InitStatistics(), IsRRTGraph(), Tatlasrrt::k, LessThanDoublePair(), Tatlasrrt::m, Tatlasrrt::ms, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, Tatlasrrt::parallel, PointInBoxTopology(), PrintAtlasRRTStatistics(), ReconstructAtlasRRTPath(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, Tatlasrrt::tp, TRUE, UpdateCostAndTree(), Tatlasrrt::w, and WireAtlasRRTstar(). Referenced by AtlasBiRRTstar(), and main().
The same as AtlasRRTstar but here a bi-directional RRT is built.
Definition at line 4148 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), ADD_NONE, AddBranchToAtlasRRT(), AddEdgeToRRT(), AddStepToAtlasRRTstar(), AtlasBiRRTstarCloseIteration(), AtlasRRTSample(), AtlasRRTstar(), AtlasRRTValidateSample(), Tatlasrrt::birrt, BOTHTREES, ChangeBiRRTSteps(), CopyDoublePair(), CopyRRTStep(), CT_DELTA, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteDoublePair(), DeleteHeap(), DeleteRRTStep(), DeleteStatistics(), DeleteVector(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GetRRTNNInBall(), GetRRTNodeCost(), GetRRTNodeTree(), GOAL2START, HEURISTIC_RRT_STAR, INF, InitAtlasRRTStatistics(), InitHeap(), InitStatistics(), InitVector(), IsRRTGraph(), Tatlasrrt::k, LessThanDoublePair(), Tatlasrrt::m, Tatlasrrt::ms, Tatlasrrt::nc, NEW, NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, Tatlasrrt::parallel, PrintAtlasRRTStatistics(), RecursiveReWireRRTstar(), ReWireAtlasRRTstar(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, Steps2PathinAtlasRRT(), Tatlasrrt::tp, TRUE, UpdateBiRRTSteps(), and WireAtlasRRTstar(). Referenced by AtlasRRTstar().
Returns the number of nodes (samples) in the AtlasRRT.
Definition at line 4400 of file atlasrrt.c. References Tatlasrrt::ns. Referenced by main().
Returns the number of charts in the AtlasRRT.
Definition at line 4405 of file atlasrrt.c. References Tatlasrrt::nc. Referenced by main().
Selects the nearest neighbour from the random point in the chart of Id ChartId, or in one of its neighbour charts.
Definition at line 4411 of file atlasrrt.c. References Tatlasrrt::atlas, ChartNeighbourID(), ChartNumNeighbours(), GetAtlasChart(), GetRRTNNInChart(), INF, and NO_UINT. Referenced by AtlasRRTValidateSample().
Selects the nearest neighbour from the random point in the chart of Id ChartId.
Definition at line 4443 of file atlasrrt.c. References Tatlasrrt::birrt, Tatlasrrt::ci, DistanceTopologyMin(), GetRRTNodeTree(), TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, NO_UINT, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, and Tatlasrrt::tp. Referenced by GetRRTNNInNeighbourChart().
Pots a projection of a Atlas-RRT. Plots a 3d projection of an atlasRRT defined on a manifold. That is it plots the atlas, the RRT and some information about the relation between the two structures. Although the ambien space can have arbitrary dimension we project it on 3 dimensions. The output plot can be visualized using geomview.
Definition at line 4470 of file atlasrrt.c. References Tatlasrrt::atlas, Tatlasrrt::birrt, TSampleInfo::c, Close3dObject(), ClosePlot3d(), CreateFileName(), CS_WD_REGENERATE_ORIGINAL_POINT, DeleteColor(), DeleteFileName(), FALSE, GetAtlasChart(), GetFileFullName(), GetRRTNodeTree(), GetRRTParent(), GOAL2START, InitPlot3d(), Tatlasrrt::k, Local2Global(), Tatlasrrt::m, NEW, NewColor(), Tatlasrrt::ns, PLOT3D_EXT, PLOT_AS_POLYGONS, PlotAtlas(), PlotRRT(), PlotVect3d(), Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, START2GOAL, StartNew3dObject(), TSampleInfo::t, and Tatlasrrt::w. Referenced by main().
Returns the approximated memory used (in bytes) by a given atlasRRT.
Definition at line 4677 of file atlasrrt.c. References Tatlasrrt::atlas, AtlasMemSize(), Tatlasrrt::rrt, and RRTMemSize(). Referenced by main().
Stores the atlasRRT as a normal atlas and a RRT than can be latter read/plotted with the usual programs. Note that this does not save all the information stored in the AtlasRRT but only that necessary for plotting.
Definition at line 4687 of file atlasrrt.c. References Tatlasrrt::atlas, ATLAS_EXT, ATLAS_RRT_EXT, Tatlasrrt::birrt, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CreateFileName(), DeleteFileName(), Tatlasrrt::e, Error(), GetFileFullName(), Tatlasrrt::k, Tatlasrrt::m, Tatlasrrt::mc, Tatlasrrt::mct1, Tatlasrrt::mct2, Tatlasrrt::ms, Tatlasrrt::nc, Tatlasrrt::nct1, Tatlasrrt::nct2, Tatlasrrt::ns, Tatlasrrt::r, Tatlasrrt::rrt, RRT_EXT, SaveAtlas(), SaveAtlasRRTChartInfo(), SaveAtlasRRTSampleInfo(), SaveRRT(), and Tatlasrrt::si. Referenced by main().
Construct an atlasRRT from the information previously stored in a file by SaveAtlasRRT.
Definition at line 4744 of file atlasrrt.c. References Tatlasrrt::ambient, Tatlasrrt::atlas, ATLAS_EXT, ATLAS_RRT_EXT, Tatlasrrt::birrt, Tatlasrrt::ce, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, CreateFileName(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, DeleteFileName(), Tatlasrrt::e, Error(), FALSE, GetFileFullName(), GetRRTTopology(), Tatlasrrt::J, Tatlasrrt::k, LoadAtlas(), LoadAtlasRRTChartInfo(), LoadAtlasRRTSampleInfo(), LoadRRT(), Tatlasrrt::m, Tatlasrrt::mc, Tatlasrrt::mct1, Tatlasrrt::mct2, Tatlasrrt::ms, Tatlasrrt::nc, Tatlasrrt::nct1, Tatlasrrt::nct2, NEW, Tatlasrrt::ns, Tatlasrrt::parallel, Tatlasrrt::r, Tatlasrrt::rrt, RRT_EXT, Tatlasrrt::si, Tatlasrrt::tp, and Tatlasrrt::w. Referenced by main().
Deletes the information stored in the RRT.
Definition at line 4817 of file atlasrrt.c. References Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, Tatlasrrt::chartsAtTree1, Tatlasrrt::chartsAtTree2, Tatlasrrt::ci, DeleteAtlas(), DeleteBox(), DeleteJacobian(), DeleteRRT(), Tatlasrrt::J, Tatlasrrt::nc, Tatlasrrt::ns, Tatlasrrt::rrt, Tatlasrrt::si, and TSampleInfo::t. Referenced by main(). |
Follow us!