AORRTC: Optimal RRT-Connect Planning

• The paper introduces AORRTC, a sampling-based motion planning algorithm that extends RRT-Connect via the AO-x meta-algorithm to achieve almost-sure optimality as computation time increases.
• It employs cost-bounded tree construction, augmented state spaces, and SIMD acceleration to rapidly find initial solutions while continually refining path quality.
• Empirical evaluations demonstrate that AORRTC provides fast initial planning and robust anytime improvement in high-DoF robotic systems, ensuring probabilistic completeness and convergence.

Asymptotically Optimal RRT-Connect (AORRTC) is a sampling-based motion planning algorithm that extends the classic RRT-Connect to achieve almost-surely asymptotic optimality (ASAO). By leveraging the AO-x meta-algorithm, AORRTC finds initial feasible solutions with the same rapidity as RRT-Connect but exhibits anytime optimality convergence, improving solution quality continually as computation time increases. The method is proven to be probabilistically complete and asymptotically optimal; it has demonstrated superior performance in high-degree-of-freedom settings and real-world robotic manipulation, particularly when accelerated with SIMD techniques (Wilson et al., 15 May 2025, Sabbadini et al., 24 Oct 2025).

1. Algorithmic Foundation and Theoretical Guarantees

AORRTC builds upon RRT-Connect’s bidirectional tree growth and single-query, greedy connection strategy. Classic RRT-Connect is fast but does not guarantee improvement over time; its solutions are often suboptimal. AORRTC is constructed by applying the AO-x meta-algorithm, which transforms any probabilistically complete satisficing planner into an ASAO planner by searching in an augmented space: each state is paired with a cost-to-come dimension.

Given a motion planning problem in configuration space $X$ with a cost function $c(\cdot)$, AO-x operates as follows:

• The planner iteratively seeks solutions below the current best cost $c_{\min}$.
• Each iteration is a motion planning query, where path cost is restricted to $c < c_{\min}$.
• Probabilistic completeness ensures eventual solution discovery, and continual restriction of cost bounds yields optimality: $\lim_{i\to\infty} P(c(\sigma_i) \to c^*) = 1$, where $c^*$ is the global optimum (Wilson et al., 15 May 2025).

AORRTC is thus almost-surely asymptotically optimal under mild technical conditions—namely, Lipschitz continuity of cost/dynamics and completeness of the underlying satisficing planner. This property establishes convergence to the optimal path as the number of samples grows, distinguishing AORRTC from RRT-Connect.

2. Structural Modifications Relative to RRT-Connect

AORRTC fundamentally augments RRT-Connect as follows:

• Augmented state space: Each node is a tuple $(x, c)$.
• Cost-bounded tree construction: Both trees grow only via edges such that accumulated path cost remains below the current $c_{\min}$.
• Parent resampling (partial): Upon node addition, the cost is resampled in a lower cost range to possibly assign a better parent, although full rewiring as in RRT* is avoided.
• Modified nearest neighbor search: Searches are performed in the augmented $(n+1)$-dimensional space, using joint configuration and cost proximity:

$$x_{\text{near}} = \arg\min_{v \in V_T} \{ \|x-x_v\|^2 + |c-c_v|^2 : c_v+\hat{c}(x_v, x) < c \}$$

• Connect operation: The greedy extend/connect step is cost-bounded to enforce admissibility of the resultant path.

Path simplification such as shortcutting or BSpline smoothing is performed on every discovered solution, further accelerating convergence rates when the triangle inequality and Euclidean geometry apply (Paulin et al., 2017).

3. Sampling Strategies and Informed Sets

AORRTC’s convergence rate and computational efficiency are strongly enhanced by informed sampling techniques:

• Direct informed sampling (prolate hyperspheroids): After solution discovery, samples are drawn exclusively from the prolate hyperspheroid defined by the start and goal, and the current best path cost (Gammell et al., 2017). Uniform sampling within this set (PH) avoids the curse of dimensionality inherent to rejection approaches, i.e., the probability of improvement does not decay factorially with dimension.
• Multi-ellipsoid and convex hull approximations: Sampling can be further focused using unions of local ellipsoids covering each subpath segment or tight convex hulls about the current solution trajectory. These approximations preserve asymptotic optimality when augmented by occasional global PH sampling (Kříž et al., 20 Mar 2025).

These techniques ensure that computational effort is only spent in regions that can improve the current solution, yielding linear convergence rates in high-dimensional spaces, minimizing wasted samples, and enabling effective graph pruning.

4. Computational Performance and Empirical Results

Experimental evaluations on 7-DoF (Panda) and 8-DoF (Fetch) arms, using the MotionBenchMaker dataset and hardware implementations (OMPL, VAMP), show:

• Initial solution time: AORRTC finds initial solutions as fast as classic RRT-Connect (0.1–0.3 ms, VAMP), outperforming tested AO planners (BIT*, AIT*, RRT*) by factors of 3–15x (Wilson et al., 15 May 2025).
• Anytime improvement: AORRTC improves solutions in an anytime fashion—converging to near-optimal path lengths quickly, while other planners require more time to even approach feasibility.
• Success rate: AORRTC consistently solves all or nearly all benchmark problems, including difficult high-dimensional scenarios; others fail on hard problem instances, especially within limited compute budgets.
• SIMD acceleration: Vectorized collision checking and parallel edge evaluation allow sub-millisecond plans on high-DoF setups. The algorithm is especially efficient due to minimal queueing and lack of expensive rewiring.

For dynamic, real-world robotics, AORRTC provides real-time replanning capability without plan-reuse. It can repeatedly replan from scratch upon obstacle/environment change, outperforming incremental planners like RRT-X, and producing globally consistent, high-quality paths (Sabbadini et al., 24 Oct 2025).

5. Convergence Properties and Limitations

Empirical and theoretical evidence confirms that:

• Linear convergence is achievable when using direct informed sampling, even in 14-DOF settings (Gammell et al., 2017).
• Graph pruning is effective: nodes outside the informed set and with no descendants inside can be efficiently removed, reducing the computational cost of nearest-neighbour searches.
• Shortcutting and local optimization can further accelerate solution improvement when path cost satisfies the triangle inequality (Paulin et al., 2017). In non-Euclidean or complex metrics, shortcutting might require caution.

A plausible implication is that the fundamental efficiency of AORRTC derives from the retention of the greedy connection strategy and the absence of full rewiring, in contrast to RRT*’s cost-intensive optimality mechanisms.

6. Relationships to Other ASAO Planners and Bidirectional Techniques

AORRTC, via AO-x, generalizes to other bidirectional planners. Critically:

• Asymmetric bidirectional search (as in AIT*) and informed heuristics further enhance initial solution speed and robustness, especially in expensive edge environments (Strub et al., 2020).
• Space-time bidirectional RRT approaches, like ST-RRT*, provide bidirectional optimality under dynamic and time-constrained constraints. AORRTC does not natively support velocity or explicit time constraints, but its structure is extensible to these domains (Grothe et al., 2022).
• Theoretical underpinning from random geometric graph connectivity affirms that bidirectional planners, with proper connection radius scaling and local reconnection, can guarantee optimality with marginal extra computational cost (Karaman et al., 2010).

7. Tabular Comparison of Planner Properties

Planner	Initial Solution Speed	Anytime Optimality	Computational Cost	Graph Structure
RRT-Connect	Fast	No	Low	Bidirectional, Tree
RRT*	Slow	Yes	Higher	Monodirectional, Rewiring
BIT* / AIT*	Moderate	Yes	Batch/Heuristic	Batch, Heuristic
AORRTC	Fast	Yes	Low	Bidirectional, Cost-bounded

8. Applicability, Impact, and Open Directions

The adoption of AORRTC in both simulation and hardware has demonstrated that ASAO planners can deliver both practical responsiveness and eventual optimality, closing the long-standing gap between fast feasibilistic planners and theoretically optimal methods. Efficient sampling strategies, local optimization integration, and hardware acceleration collectively yield planners capable of sub-100ms query times and robust path improvement for complex manipulation scenarios and dynamic environments (Wilson et al., 15 May 2025, Sabbadini et al., 24 Oct 2025).

AORRTC’s framework is extensible to kinodynamic, time-dependent, and multi-objective domains, provided informed sets and cost-bounded strategies are properly generalized. Future research may focus on:

• Integration of learned or data-driven heuristics for even faster practical convergence.
• Advanced local optimization methods synergized with bidirectional ASAO frameworks.
• Hybrid planning strategies for temporally dynamic and constrained environments.

The combination of bidirectional search, informed sampling, local optimization, and computational efficiency marks AORRTC as a canonical approach for modern motion planning in robotics and related domains.