atlasrrt.c File Reference 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(), AtlasTRRT(), InitBranchConfig(), and New_AddBranchToAtlasRRT().
Adds anly the last generated node during the branch extension. Definition at line 38 of file atlasrrt.c. Referenced by AddBranchToAtlasRRT(), AddStepToAtlasRRTstar(), and New_AddBranchToAtlasRRT().
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(), AddStepToAtlasRRTstar(), and New_AddBranchToAtlasRRT().
Do not add any node during the branch extension. Definition at line 53 of file atlasrrt.c. Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), InitBranchConfig(), New_AddBranchToAtlasRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().
Extends a AtlasRRT towards a random sample. All the generated samples are added to the RRT. Definition at line 66 of file atlasrrt.c. Referenced by AtlasRRT(), AtlasTRRT(), and InitBranchConfig().
Extends a AtlasRRT towards a node already in a RRT (i.e., tries to connect a the two RRTs in a bi-directinal RRT). All the generated samples are added to the RRT. Definition at line 76 of file atlasrrt.c. Referenced by AtlasRRT(), and InitBranchConfig().
Checks if it is possible to connect two samples on the manifold. Definition at line 83 of file atlasrrt.c. Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), InitBranchConfig(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), and WireAtlasRRTstar().
Checks if it is possible to connect two samples on the manifold but without checking for collisions (the connection is already known to be collision free). Definition at line 92 of file atlasrrt.c. Referenced by InitBranchConfig(), ReconstructAtlasRRTPath(), and Steps2PathinAtlasRRT(). Function Documentation
Initializes a branch configuration with one of the predefined pre-sets. Note that the particular algorithms may use one of the pre-sets and then adjust several of the fields at its convenience.
Definition at line 1733 of file atlasrrt.c. References ADD_ALL, ADD_NONE, TAtlasRRTBranchConfig::addMode, TAtlasRRTBranchConfig::checkCollisions, CONNECT_RRTs, CONNECT_SAFE, CONNECT_SAMPLES, TAtlasRRTBranchConfig::costData, TAtlasRRTBranchConfig::costF, Error(), TAtlasRRTBranchConfig::explorationSample, EXTEND_RRT, FALSE, TAtlasRRTBranchConfig::maxLength, TAtlasRRTBranchConfig::onManifold, TAtlasRRTBranchConfig::revisitCharts, and TRUE. Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), AtlasRRT(), AtlasTRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().
Sets the values of the status structure to start a branch creation.
Definition at line 1634 of file atlasrrt.c. References TAtlasRRTBranchStatus::blocked, TSampleInfo::c, TAtlasRRTBranchStatus::collision, CopyID(), TAtlasRRTBranchStatus::cost, TAtlasRRTBranchStatus::delta, TAtlasRRTBranchStatus::deltaParam, TAtlasRRTBranchStatus::desiredLength, DistanceTopology(), TAtlasRRTBranchStatus::distGoal, TAtlasRRTBranchStatus::distOrigin, TAtlasRRTBranchStatus::distQrand, FALSE, GetRRTNodeTree(), TSampleInfo::id, INF, InitVector(), Tatlasrrt::k, TAtlasRRTBranchStatus::lastID, TAtlasRRTBranchStatus::length, Tatlasrrt::m, TAtlasRRTBranchStatus::maxDistOrigin, TAtlasRRTBranchConfig::maxLength, TAtlasRRTBranchStatus::maxLength, TAtlasRRTBranchStatus::nCreatedCharts, NEW, NewVectorElement(), TAtlasRRTBranchConfig::onManifold, TAtlasRRTBranchStatus::reachedGoal, TAtlasRRTBranchStatus::reachedQrand, TAtlasRRTBranchStatus::reachedTmpQrand, Tatlasrrt::rrt, TSampleInfo::s, TAtlasRRTBranchStatus::stepLength, TAtlasRRTBranchStatus::tooFar, Tatlasrrt::tp, TAtlasRRTBranchStatus::traversedCharts, TAtlasRRTBranchStatus::tree, TRUE, and TAtlasRRTBranchStatus::validChart. Referenced by New_AddBranchToAtlasRRT().
Tests if the branch can be further extended.
Definition at line 1723 of file atlasrrt.c. References TAtlasRRTBranchStatus::blocked, TAtlasRRTBranchStatus::collision, TAtlasRRTBranchStatus::length, TAtlasRRTBranchStatus::maxLength, TAtlasRRTBranchStatus::reachedGoal, TAtlasRRTBranchStatus::reachedQrand, TAtlasRRTBranchStatus::reachedTmpQrand, and TAtlasRRTBranchStatus::tooFar. Referenced by New_AddBranchToAtlasRRT().
Releases the memory allocated by InitBranchStatus.
Definition at line 1717 of file atlasrrt.c. References DeleteVector(), TAtlasRRTBranchStatus::deltaParam, and TAtlasRRTBranchStatus::traversedCharts. Referenced by New_AddBranchToAtlasRRT().
Allocates space for a parametrized point.
Definition at line 1782 of file atlasrrt.c. References TSampleInfo::c, TSampleInfo::generateChart, TSampleInfo::id, TSampleInfo::lsc, NEW, NO_UINT, TSampleInfo::pc, TSampleInfo::s, and TSampleInfo::t. Referenced by New_AddBranchToAtlasRRT().
Copy constructor.
Definition at line 1793 of file atlasrrt.c. References TSampleInfo::c, TSampleInfo::generateChart, TSampleInfo::id, TSampleInfo::lsc, TSampleInfo::pc, TSampleInfo::s, and TSampleInfo::t. Referenced by AddChart2AtlasRRT(), New_AddBranchToAtlasRRT(), and SetSampleInfoFromNode().
Copies the informaion of an AtlasRRT node. Note that this does not allocate space.
Definition at line 1805 of file atlasrrt.c. References Tatlasrrt::k, Tatlasrrt::m, SetSampleInfo(), and Tatlasrrt::si. Referenced by New_AddBranchToAtlasRRT().
Destructor.
Definition at line 1810 of file atlasrrt.c. References TSampleInfo::s, and TSampleInfo::t. Referenced by New_AddBranchToAtlasRRT().
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 595 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 600 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 605 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 610 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 615 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 620 of file atlasrrt.c. References TAtlasRRTStatistics::nStep. Referenced by New_NewTemptativeSample(), and NewTemptativeSample().
To be called when we create a new chart due to convergence issues in the current chart.
Definition at line 625 of file atlasrrt.c. References TAtlasRRTStatistics::nNoConvergent. Referenced by New_NewTemptativeSample(), and NewTemptativeSample().
To be called every time the random sample is reached.
Definition at line 630 of file atlasrrt.c. References TAtlasRRTStatistics::nQrandReached. Referenced by AddBranchToAtlasRRT(), New_AddBranchToAtlasRRT(), New_NewTemptativeSample(), and NewTemptativeSample().
To be called everty time a branch is stopped since it leaves the given domain.
Definition at line 635 of file atlasrrt.c. References TAtlasRRTStatistics::nNotInDomain. Referenced by New_NewTemptativeSample(), and NewTemptativeSample().
To be called every time a branch extension is stopped due to a collision.
Definition at line 640 of file atlasrrt.c. References TAtlasRRTStatistics::nCollision. Referenced by New_NewTemptativeSample(), and NewTemptativeSample().
To be called every time a branch extension is stopped because the branch is already too long.
Definition at line 645 of file atlasrrt.c. References TAtlasRRTStatistics::nTooLong. Referenced by AddBranchToAtlasRRT(), and New_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 650 of file atlasrrt.c. References TAtlasRRTStatistics::nTooFar. Referenced by AddBranchToAtlasRRT(), and New_AddBranchToAtlasRRT().
To be called every time a branch extension is stopped to avoid the overlap with another branch.
Definition at line 655 of file atlasrrt.c. References TAtlasRRTStatistics::nOverlap. Referenced by New_NewTemptativeSample(), and 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 665 of file atlasrrt.c. References TAtlasRRTStatistics::nErrorNewChart. Referenced by AddBranchToAtlasRRT(), New_AddBranchToAtlasRRT(), New_NewTemptativeSample(), 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 660 of file atlasrrt.c. References TAtlasRRTStatistics::nOutOfChart. Referenced by New_NewTemptativeSample(), and 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 670 of file atlasrrt.c. References TAtlasRRTStatistics::nLargeCurvature. Referenced by New_NewTemptativeSample(), and 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 675 of file atlasrrt.c. References TAtlasRRTStatistics::nDirLargeCurvature. Referenced by New_NewTemptativeSample(), and 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 680 of file atlasrrt.c. References TAtlasRRTStatistics::nStepReduction. Referenced by AddBranchToAtlasRRT(), and New_AddBranchToAtlasRRT().
To be called every time we actually add a node (or sample) to the atlasRRT.
Definition at line 685 of file atlasrrt.c. References TAtlasRRTStatistics::nSample. Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and New_AddBranchToAtlasRRT().
To be called every time we actualy add a chart to the atlasRRT.
Definition at line 690 of file atlasrrt.c. References TAtlasRRTStatistics::nChart. Referenced by AddBranchToAtlasRRT(), AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and New_AddBranchToAtlasRRT().
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 695 of file atlasrrt.c. References TAtlasRRTStatistics::nNoConnect. Referenced by AddBranchToAtlasRRT(), and New_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 700 of file atlasrrt.c. References TAtlasRRTStatistics::nSingular. Referenced by AddBranchToAtlasRRT(), and New_AddBranchToAtlasRRT().
Counts the number of random samples generated. Definition at line 705 of file atlasrrt.c. References TAtlasRRTStatistics::nRandom. Referenced by AtlasRRTSample().
Counts the number os random samples rejected. Definition at line 710 of file atlasrrt.c. References TAtlasRRTStatistics::nRejections. Referenced by AtlasRRTSample(), and AtlasRRTValidateSample().
New collision check. Definition at line 715 of file atlasrrt.c. References TAtlasRRTStatistics::nCollisionChecks. Referenced by New_NewTemptativeSample(), and NewTemptativeSample().
Init the Atlas RRT statistics.
Definition at line 553 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 720 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 766 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 973 of file atlasrrt.c. Referenced by AtlasBiRRTstar(), AtlasRRT(), AtlasRRTstar(), AtlasTRRT(), and main().
New version of AddBranchToAtlasRRT. 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*.
Definition at line 1816 of file atlasrrt.c. References ADD_ALL, ADD_LAST, ADD_LAST_NO_REP, ADD_NONE, AddChart2AtlasRRT(), TAtlasRRTBranchConfig::addMode, AddSample2AtlasRRT(), AddSample2Samples(), AdjustDynamicDomain(), TAtlasRRTBranchStatus::blocked, TSampleInfo::c, canContinueBranch(), TAtlasRRTBranchStatus::collision, TAtlasRRTBranchStatus::cost, CT_DELTA, CT_EPSILON, CT_MAX_NODES_RRT, CT_SAMPLING, DeleteBranchStatus(), DeleteSampleInfo(), TAtlasRRTBranchStatus::delta, Distance(), DistanceTopology(), TAtlasRRTBranchStatus::distGoal, TAtlasRRTBranchStatus::distOrigin, TAtlasRRTBranchStatus::distQrand, Error(), FALSE, TSampleInfo::generateChart, GetParameter(), GetRRTNN(), TSampleInfo::id, InitBranchStatus(), InitSampleInfo(), InitSamples(), Tatlasrrt::k, TAtlasRRTBranchStatus::lastID, TAtlasRRTBranchStatus::length, Tatlasrrt::m, TAtlasRRTBranchStatus::maxDistOrigin, TAtlasRRTBranchStatus::maxLength, TAtlasRRTBranchStatus::nCreatedCharts, New_NewTemptativeSample(), New_PointTowardRandSample(), NewAtlasRRTBlockBySingularity(), NewAtlasRRTChart(), NewAtlasRRTInitChartError(), NewAtlasRRTNoConnectToParent(), NewAtlasRRTQrandReached(), NewAtlasRRTSample(), NewAtlasRRTStepReduction(), NewAtlasRRTTooFar(), NewAtlasRRTTooLong(), NewVectorElement(), NO_UINT, Norm(), Tatlasrrt::ns, TAtlasRRTBranchConfig::onManifold, TAtlasRRTBranchStatus::reachedGoal, TAtlasRRTBranchStatus::reachedQrand, TAtlasRRTBranchStatus::reachedTmpQrand, Tatlasrrt::rrt, TSampleInfo::s, SetSampleInfo(), SetSampleInfoFromNode(), Tatlasrrt::si, TAtlasRRTBranchStatus::stepLength, TSampleInfo::t, TAtlasRRTBranchStatus::tooFar, Tatlasrrt::tp, TAtlasRRTBranchStatus::traversedCharts, TAtlasRRTBranchStatus::tree, TRUE, TAtlasRRTBranchStatus::validChart, and Warning(). Referenced by AddStepToAtlasRRTstar(), AtlasBiRRTstar(), AtlasRRT(), AtlasTRRT(), ReconstructAtlasRRTPath(), ReWireAtlasRRTstar(), SmoothPathInAtlasRRT(), Steps2PathinAtlasRRT(), and WireAtlasRRTstar().
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 2478 of file atlasrrt.c. References AccumulateVector(), ADD_ALL, ADD_LAST, ADD_LAST_NO_REP, ADD_NONE, 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, INF, 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 2115 of file atlasrrt.c. References Tatlasrrt::ambient, Tatlasrrt::atlas, Tatlasrrt::birrt, TAtlasRRTBranchStatus::blocked, TSampleInfo::c, Tatlasrrt::ce, Chart2Manifold(), TAtlasRRTBranchConfig::checkCollisions, Tatlasrrt::ci, CloseCharts(), CmpID(), TAtlasRRTBranchStatus::collision, TAtlasRRTBranchStatus::cost, TAtlasRRTBranchConfig::costData, TAtlasRRTBranchConfig::costF, CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_EPSILON, CT_SAMPLING, DeleteChart(), TAtlasRRTBranchStatus::delta, TAtlasRRTBranchStatus::deltaParam, DetermineChartNeighbour(), Distance(), DistanceTopology(), Tatlasrrt::e, ElementInVector(), EnlargeChart(), TAtlasRRTBranchConfig::explorationSample, FALSE, TSampleInfo::generateChart, GetAtlasChart(), GetParameter(), TSampleInfo::id, InitChart(), InsideChartPolytope(), Tatlasrrt::J, Tatlasrrt::k, Tatlasrrt::m, Manifold2Chart(), TAtlasRRTBranchStatus::nCreatedCharts, New_PointTowardRandSample(), NewAtlasRRTCollision(), NewAtlasRRTCollisionCheck(), NewAtlasRRTDirLargeCurvature(), NewAtlasRRTInitChartError(), NewAtlasRRTLargeCurvature(), NewAtlasRRTNoConvergentStep(), NewAtlasRRTNotInDomain(), NewAtlasRRTOutOfChart(), NewAtlasRRTOverlap(), NewAtlasRRTQrandReached(), NewAtlasRRTStep(), NewVectorElement(), Norm(), TAtlasRRTBranchConfig::onManifold, PointInBoxTopology(), Tatlasrrt::r, TAtlasRRTBranchConfig::revisitCharts, Tatlasrrt::rrt, TSampleInfo::s, SumVectorScale(), TSampleInfo::t, Tatlasrrt::tp, TransitionTestRRT(), TAtlasRRTBranchStatus::traversedCharts, TChartInfo::tree, TRUE, TAtlasRRTBranchStatus::validChart, Tatlasrrt::w, and Warning(). Referenced by New_AddBranchToAtlasRRT().
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 2881 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 3146 of file atlasrrt.c. References AddChart2Tree(), AddNodeToRRT(), TSampleInfo::c, Tatlasrrt::ci, DistanceTopology(), Error(), TSampleInfo::generateChart, GetRRTNode(), TSampleInfo::id, Tatlasrrt::k, TChartInfo::lc, TSampleInfo::lsc, Tatlasrrt::m, MEM_DUP, Tatlasrrt::ms, NEW, Tatlasrrt::ns, TSampleInfo::pc, Tatlasrrt::rrt, TSampleInfo::s, Tatlasrrt::si, TSampleInfo::t, and Tatlasrrt::tp. Referenced by AddBranchToAtlasRRT(), and New_AddBranchToAtlasRRT().
Used when a sample triggers the generation of a new chart. We can generate a chart on:
Definition at line 3284 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, Tatlasrrt::m, Manifold2Chart(), Tatlasrrt::mc, MEM_EXPAND, Tatlasrrt::nc, NEW, NO_UINT, TSampleInfo::pc, PopulateWithSamples(), TSampleInfo::s, SetSampleInfo(), Tatlasrrt::si, SingularChart(), TSampleInfo::t, Tatlasrrt::tp, TChartInfo::tree, TRUE, and Warning(). Referenced by AddBranchToAtlasRRT(), InitAtlasRRT(), and New_AddBranchToAtlasRRT().
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 3463 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 3218 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. Replacement of PointTowardRandSample.
Definition at line 2425 of file atlasrrt.c. References Tatlasrrt::atlas, TSampleInfo::c, TAtlasRRTBranchStatus::deltaParam, DifferenceVector(), FALSE, GetAtlasChart(), Tatlasrrt::k, Local2Global(), Manifold2Chart(), TAtlasRRTBranchStatus::maxLength, Norm(), Normalize(), TAtlasRRTBranchConfig::onManifold, TSampleInfo::s, SumVectorScale(), TSampleInfo::t, TANGENT_SAMPLING, Tatlasrrt::tp, and TRUE. Referenced by New_AddBranchToAtlasRRT(), and New_NewTemptativeSample().
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 3240 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 3553 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), AddSample2Samples(), CONNECT_SAFE, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, DeleteSamples(), FALSE, GetRRTParent(), InitBranchConfig(), InitSamples(), New_AddBranchToAtlasRRT(), NO_UINT, TAtlasRRTBranchStatus::reachedQrand, 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 3637 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), AddSample2Samples(), CONNECT_SAFE, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CT_DELTA, CT_EPSILON, DeleteSamples(), TAtlasRRTBranchStatus::distQrand, FALSE, GetParameter(), GetRRTNodeCost(), GetVectorElement(), InitBranchConfig(), InitSamples(), New_AddBranchToAtlasRRT(), 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 3745 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), CONNECT_SAMPLES, FALSE, GetRRTNodeCost(), GetRRTParent(), InitBranchConfig(), TAtlasRRTBranchConfig::maxLength, NEW, New_AddBranchToAtlasRRT(), NO_UINT, randomMax(), TAtlasRRTBranchStatus::reachedQrand, 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 4872 of file atlasrrt.c. References ADD_LAST, ADD_LAST_NO_REP, AddBranchToAtlasRRT(), TAtlasRRTBranchConfig::addMode, CONNECT_SAMPLES, CT_EPSILON, DistanceTopology(), TAtlasRRTBranchStatus::distGoal, Error(), FALSE, GetParameter(), GetRRTNodeCost(), GetRRTParent(), InitBranchConfig(), TAtlasRRTBranchStatus::lastID, Tatlasrrt::m, New_AddBranchToAtlasRRT(), NO_UINT, TAtlasRRTBranchConfig::onManifold, TAtlasRRTBranchStatus::reachedGoal, TAtlasRRTBranchStatus::reachedQrand, 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 4968 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), AddEdgeToRRT(), AddElement2Heap(), Tatlasrrt::ambient, CONNECT_SAMPLES, ConnectSamplesChart(), CT_EPSILON, Error(), FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, InitBranchConfig(), IsRRTGraph(), Tatlasrrt::J, Tatlasrrt::k, Tatlasrrt::m, TAtlasRRTBranchConfig::maxLength, NEW, New_AddBranchToAtlasRRT(), NewAtlasRRTBranch(), NewAtlasRRTNoEmptyBranch(), NewDoublePair(), NO_UINT, Tatlasrrt::parallel, TAtlasRRTBranchStatus::reachedQrand, 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 5127 of file atlasrrt.c. References ADD_NONE, AddBranchToAtlasRRT(), ChangeBiRRTSteps(), CONNECT_SAMPLES, CT_EPSILON, FALSE, GetParameter(), GetRRTMode(), GetRRTNodeCost(), GetRRTNodeCostFromParent(), GetRRTNodeTree(), GetRRTParent(), INF, InitBranchConfig(), TAtlasRRTBranchConfig::maxLength, New_AddBranchToAtlasRRT(), NO_UINT, TAtlasRRTBranchStatus::reachedQrand, 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 5220 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 5255 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 4067 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 4081 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 4088 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 4103 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 3911 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 4008 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 3821 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 4114 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 4252 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 4340 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 4392 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), ADD_ALL, AddBranchToAtlasRRT(), Tatlasrrt::ambient, Tatlasrrt::atlas, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, CONNECT_RRTs, 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, TAtlasRRTBranchConfig::explorationSample, EXTEND_RRT, FALSE, GetAtlasChart(), GetElapsedTime(), GetParameter(), GetRRTNNInBranch(), GOAL2START, IncreaseChartSamplingRadius(), INF, InitAtlasRRTStatistics(), InitBranchConfig(), InitStatistics(), Tatlasrrt::k, TAtlasRRTBranchStatus::lastID, Tatlasrrt::m, MOV_AVG_DOWN, MOV_AVG_UP, Tatlasrrt::nc, NEW, New_AddBranchToAtlasRRT(), NewAtlasRRTBranch(), NewAtlasRRTChart(), NewAtlasRRTDistanceQrand(), NewAtlasRRTNoEmptyBranch(), NewAtlasRRTNoEmptyTreeConnection(), NewAtlasRRTSample(), NewAtlasRRTTreeConnection(), NO_UINT, Tatlasrrt::ns, TAtlasRRTBranchConfig::onManifold, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), TAtlasRRTBranchStatus::reachedGoal, 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 4688 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), ADD_ALL, AddBranchToAtlasRRT(), Tatlasrrt::ambient, AtlasRRTSample(), AtlasRRTValidateSample(), Tatlasrrt::birrt, TAtlasRRTBranchConfig::costData, TAtlasRRTBranchConfig::costF, 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, TAtlasRRTBranchConfig::explorationSample, EXTEND_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNode(), GetTRRTTemperature(), INF, InitAtlasRRTStatistics(), InitBranchConfig(), InitStatistics(), Tatlasrrt::k, TAtlasRRTBranchStatus::lastID, Tatlasrrt::m, Tatlasrrt::nc, NEW, New_AddBranchToAtlasRRT(), NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, TAtlasRRTBranchConfig::onManifold, PathStart2GoalInRRT(), PointInBoxTopology(), PrintAtlasRRTStatistics(), TAtlasRRTBranchStatus::reachedGoal, 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 5285 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(), 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 5523 of file atlasrrt.c. References AccumulateAtlasRRTStatistics(), ADD_NONE, AddBranchToAtlasRRT(), AddEdgeToRRT(), AddStepToAtlasRRTstar(), AtlasBiRRTstarCloseIteration(), AtlasRRTSample(), AtlasRRTstar(), AtlasRRTValidateSample(), Tatlasrrt::birrt, BOTHTREES, ChangeBiRRTSteps(), CONNECT_SAMPLES, CopyDoublePair(), CopyRRTStep(), CT_DELTA, CT_GAMMA, CT_MAX_PLANNING_ITERATIONS, CT_MAX_PLANNING_TIME, CT_SAMPLING, DeleteAtlasRRTStatistics(), DeleteHeap(), DeleteStatistics(), DeleteVector(), DistanceTopology(), Error(), EXPLORATION_RRT, FALSE, GetElapsedTime(), GetParameter(), GetRRTNN(), GetRRTNNInBall(), GetRRTNodeCost(), GetRRTNodeTree(), GOAL2START, HEURISTIC_RRT_STAR, INF, InitAtlasRRTStatistics(), InitBranchConfig(), InitHeap(), InitStatistics(), InitVector(), IsRRTGraph(), Tatlasrrt::k, LessThanDoublePair(), Tatlasrrt::m, TAtlasRRTBranchConfig::maxLength, Tatlasrrt::ms, Tatlasrrt::nc, NEW, New_AddBranchToAtlasRRT(), NewAtlasRRTChart(), NewAtlasRRTSample(), NO_UINT, Tatlasrrt::ns, Tatlasrrt::parallel, PrintAtlasRRTStatistics(), TAtlasRRTBranchStatus::reachedQrand, 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 5789 of file atlasrrt.c. References Tatlasrrt::ns. Referenced by main().
Returns the number of charts in the AtlasRRT.
Definition at line 5794 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 5800 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 5832 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 5859 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 6066 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 6076 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 6133 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 6206 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!