PATACON: GPU-Parallel Tangent-Bundle RRT-Connect for Constrained Motion Planning

Abstract

Fast whole-body motion planning enables continuous replanning for humanoid robots in dynamic environments while satisfying collision avoidance and kinematic constraints. However, constrained motion planning incurs substantial computational cost due to repeated constraint projection in addition to collision checking and nearest-neighbor search. Recently, GPU-based massively parallel planners have been proposed to reduce computational cost. Nevertheless, these planners still steer in the ambient configuration space, so projecting candidates that deviate from the manifold remains a significant source of overhead, particularly in highly constrained, high-dimensional problems. In this work, we introduce PATACON, a GPU-parallel bidirectional planner that explores tangent space to exploit the local manifold geometry induced by known constraint functions. Experiments on dual-arm closed-chain and humanoid whole-body constrained planning tasks show that PATACON outperforms the prior work as configuration-space dimension and constraint codimension grow, achieving up to an 11.25 times speedup. These results suggest that GPU-parallel tangent-space exploration brings constrained whole-body motion planning closer to real-time performance.

I Introduction

Dual-arm closed-chain and humanoid whole-body motion require maintaining task-dependent kinematic relationships while satisfying joint limits and avoiding collisions. These kinematic requirements can be formulated as nonlinear equality constraints, which define a lower-dimensional constraint manifold embedded in a high-dimensional configuration space [1]. In sampling-based motion planning, collision checking and nearest-neighbor queries are generally major sources of computational cost[2, 3]. Planning on a constraint manifold introduces additional overhead because constraint projection is performed repeatedly throughout tree expansion. Reducing this computational burden is particularly important in scenarios that require repeated replanning as the target, obstacles, or task conditions change, where low planning latency is essential for responsive execution.

A widely used approach generates candidate motions in the ambient configuration space and projects them onto the manifold as the tree grows [1]. cpRRTC [4] brought this approach to the GPU by assigning each thread block to steer from the nearest node toward a random sample, project the waypoints of the resulting straight-line segment in parallel, and perform collision checks with early termination. This reduces constrained planning to milliseconds, but the exploration scheme is unchanged; each extension still steers in the ambient space and relies on projection to satisfy the constraints.

Ambient-space steering, however, ignores the shape of the manifold. Kim et al. [5] showed that a target sampled in the ambient space can contribute little progress along the manifold once projected, and that the resulting large number of projections degrades both computational efficiency and the Voronoi bias of RRT. Because the constraint Jacobian provides the tangent space at any configuration, displacements can instead be restricted to it: Stilman [6] observed that tangent displacements are more likely to remain within the constraint tolerance, and TB-RRT [5] grows trees on tangent spaces bounded by the local curvature, outperforming projection-based planners with larger gains on more complex problems. However, TB-RRT expands one node at a time, incurs additional preprocessing and bookkeeping for its tangent spaces, and relies on lazy projection. Therefore, the feasibility checked on tangent spaces must be re-evaluated after projection.

We propose PATACON (PArallel TAngent-bundle RRT-CONnect), a GPU-parallel bi-directional planner that brings tangent-space-guided exploration to massively parallel tree expansion, as illustrated in Fig. 1. Unlike TB-RRT, PATACON projects and validates every candidate before tree insertion, so every node is feasible. Its efficiency has two sources: tangent-space candidates are more likely to remain within the constraint tolerance [6] and avoid ambient targets whose projections make little progress along the manifold [5], and a single sample and nearest-neighbor query yield multiple candidate edges that are validated concurrently on the GPU, which amortizes the preprocessing and bookkeeping of tangent spaces over multiple edges.

Our main contributions are as follows:

1. Tangent-space-guided GPU-parallel extension. PATACON replaces the ambient-space steering of existing GPU-parallel planners with extension along local tangent spaces, so that candidates follow the manifold geometry before projection.
2. Shared tangent-space banks. Tangent spaces are maintained in tree-specific banks shared across thread blocks, together with the correspondence between projected manifold states and their tangent-space coordinates, so that multiple blocks can explore different regions of the manifold concurrently.
3. Speculative multi-edge expansion. From one sample and one nearest-neighbor query, PATACON generates consecutive candidate edges along a tangent direction, validates them concurrently, and inserts the longest valid prefix, unlike the step-by-step CONNECT heuristic of RRT-Connect [7].

We evaluate PATACON on eight benchmarks ranging from 7-DoF manipulation to 35-DoF humanoid whole-body planning. Although cpRRTC is faster on low-dimensional problems, PATACON shows increasing advantages with configuration-space dimension and constraint restrictiveness, reaching a 11.25\times speedup with a 100% success rate.

II Related Work

Sampling-based constrained motion planners enforce nonlinear equality constraints through numerical projection or local approximations of the constraint manifold [1]. Projection-based methods range from randomized gradient descent for closed chains [8] to Jacobian-based tangent-space sampling and first-order retraction [6] and CBiRRT [9] with Task Space Regions [10], whereas AtlasRRT [11] and TB-RRT [5] grow trees on local charts or tangent spaces, and Kingston et al. [12] unify these mechanisms. These methods, however, are primarily sequential.

Learning-based planners reduce online search through learned latent spaces [13, 14, 15] or constraint-aware samplers [16], but require offline data generation and training; we instead accelerate planning directly from the given constraint functions.

Parallelization reduces the wall-clock cost of planning through GPU collision checking [17, 18], multiple or shared trees on CPUs [19, 20, 21], SIMD vectorization [22, 23], and GPU trajectory optimization [24]. pRRTC [25] and BatchRRT [26] run RRT-style tree growth on the GPU, and cpRRTC [4] extends pRRTC to constrained planning by projecting motion-segment waypoints in parallel. However, the steering methods remain in the ambient space, so parallelism lowers the cost of each projection but not the off-manifold motion to be projected.

Simply adding tangent-space steering to such planners is insufficient, because tangent-space management and local nearest-neighbor search add per-extension overhead. PATACON instead exploits the fact that tangent-space candidates stay close to the manifold: one sample and nearest-neighbor query yield several consecutive candidate edges, which are validated in parallel on the GPU to amortize this overhead.

III Problem Definition

Let \mathcal{C}\subseteq\mathbb{R}^{n} denote the joint-feasible robot configuration space, where q\in\mathcal{C} represents a robot configuration. Task-dependent kinematic constraints are represented by a differentiable nonlinear equality-constraint function h:\mathcal{C}\rightarrow\mathbb{R}^{m}, which defines the constraint manifold as

\mathcal{M}=\{q\in\mathcal{C}\mid h(q)=0\}.

At a regular configuration q in the interior of \mathcal{C}, we assume that the constraint Jacobian J_{h}(q)=\partial h(q)/\partial q has full row rank m. The constraint manifold is then locally of dimension d=n-m, and its tangent space is given by

T_{q}\mathcal{M}=\{v\in\mathbb{R}^{n}\mid J_{h}(q)v=0\}.

Equivalently, let N(q)\in\mathbb{R}^{n\times d} denote an orthonormal basis of the null space of J_{h}(q), such that J_{h}(q)N(q)=0 and N(q)^{\top}N(q)=I_{d}. This basis provides the local tangent-space representation used by the planner.

Let \mathcal{C}_{\mathrm{free}}\subseteq\mathcal{C} denote the set of configurations that are free of both self-collision and environmental collision. The feasible portion of the constraint manifold is then

\mathcal{M}_{\mathrm{free}}=\mathcal{M}\cap\mathcal{C}_{\mathrm{free}}.

Given feasible start and goal configurations q_{\mathrm{start}},q_{\mathrm{goal}}\in\mathcal{M}_{\mathrm{free}}, the constrained motion-planning problem considered in this work is to find a continuous path

\sigma:[0,1]\rightarrow\mathcal{M}_{\mathrm{free}},

such that

\sigma(0)=q_{\mathrm{start}},\qquad\sigma(1)=q_{\mathrm{goal}}.

Thus, every configuration along the resulting path must satisfy the nonlinear kinematic constraints, joint limits, and collision-avoidance requirements.

IV GPU-Parallel Tangent-Bundle RRT-Connect

PATACON restructures tangent-space-guided RRT-Connect for GPU-parallel execution through two algorithmic components: Tangent-Guided Sampling (TGS) and Tangent-Guided Multi-Edge Projection (TGMP). TGS guides exploration using local tangent-space geometry, while TGMP converts each TGS exploration decision into a multi-step expansion that generates multiple consecutive candidate edges. PATACON combines concurrent exploration across multiple tangent-space regions with GPU-parallel processing of the candidate edges generated by TGMP.

In each exploration iteration, PATACON expands the selected tree through the TGS–TGMP pipeline, projects and validates the resulting candidate edges in parallel, and inserts only the longest consecutively valid prefix. A successful EXTEND is followed by a target-directed CONNECT toward the opposite tree. The following subsections describe these stages in order. Fig. 2 illustrates the GPU execution structure, while Algorithm 1 summarizes the overall planning procedure.

IV-A Tangent-Guided Sampling for Concurrent Exploration

TGS determines the local region and expansion state used for each tree-expansion attempt. For a tangent-space root q_{r}\in\mathcal{M}, PATACON uses the orthonormal null-space basis N(q_{r}) defined in Sec. III. The corresponding tangent-space projector is

P_{\mathcal{T}_{r}}=N(q_{r})N(q_{r})^{\top}.

\mathcal{T}_{r}=\left\{q_{r}+N(q_{r})c\mid c\in\mathbb{R}^{d}\right\}.

Sampling within \mathcal{T}_{r} guides exploration toward directions consistent with the local constraint geometry, reducing unnecessary exploration away from the manifold before projection. This geometric guidance, however, introduces additional computation for tangent-space management, selection, and local nearest-neighbor search, which motivates the multi-step expansion of TGMP to amortize this overhead. Each tree node stores its projected manifold configuration q_{i}\in\mathcal{M}_{\mathrm{free}}, parent index, associated tangent-space index \tau(q_{i}), and local tangent-space representation \bar{q}_{i}\in\mathcal{T}_{\tau(q_{i})}. The projected configuration q_{i} is used for feasibility and path validation, whereas \bar{q}_{i} is used for tangent-space exploration and tangent-space-local nearest-neighbor search.

PATACON maintains separate tangent-space banks in GPU global memory for the start and goal trees, with each bank shared across the CUDA blocks exploring the corresponding tree. At initialization, tangent spaces rooted at q_{\mathrm{start}} and q_{\mathrm{goal}} are inserted into their respective banks. A newly generated tangent space becomes selectable only after its root node and associated tangent-space data have been fully initialized.

At each exploration iteration, a CUDA block selects the grow tree T_{\mathrm{grow}} and opposite tree T_{\mathrm{opp}} according to their current sizes. When the two trees have comparable sizes, the grow tree is alternated; when their size difference exceeds a prescribed threshold, the smaller tree is preferentially expanded until the imbalance is reduced, following the balanced bidirectional search principle [27].

After selecting the grow tree, each CUDA block dynamically selects a ready tangent space from the corresponding bank. The initial bank index varies with the block and planning iteration, after which the bank is scanned cyclically until a ready tangent space is found. Therefore, tangent spaces are dynamically selected rather than statically assigned to individual CUDA blocks.

Following TB-RRT [5], PATACON applies backtracking prevention when sampling from a newly created tangent space. Let q_{p} denote the root of its parent tangent space and q_{r} the root of the new tangent space. The displacement q_{r}-q_{p} is projected onto the new tangent space to obtain a forward direction d_{r}. If the sampled tangent-space displacement has a negative inner product with d_{r}, its direction is reversed. This restricts sampling to the forward half-space of the tangent space and prevents the tree from backtracking toward previously explored regions.

Within the selected tangent space \mathcal{T}_{r}, a block-local Halton sample [28, 29] determines a normalized combination of tangent-basis directions and a displacement that remains within the joint-space sampling bounds, producing a tangent-space sample \bar{q}_{\mathrm{rand}}. Let V(T,r) denote the set of configurations in tree T associated with tangent space \mathcal{T}_{r}. The expansion node is then selected by a nearest-neighbor query over V(T_{\mathrm{grow}},r),

q_{\mathrm{near}}=\argminB_{q_{i}\in V(T_{\mathrm{grow}},r)}\operatorname{dist}\left(\bar{q}_{i},\bar{q}_{\mathrm{rand}}\right),

where \bar{q}_{i} denotes the tangent-space representation of q_{i} in \mathcal{T}_{r}, and \operatorname{dist}(q_{a},q_{b})=\|q_{a}-q_{b}\|_{2}. This nearest-neighbor search is parallelized within each CUDA block using thread-level distance evaluation followed by block-level reduction. The sample \bar{q}_{\mathrm{rand}} and the tangent-space representation \bar{q}_{\mathrm{near}} of the selected node q_{\mathrm{near}} are then passed to TGMP for subsequent expansion.

IV-B Tangent-Guided Multi-Edge Projection

TGMP converts the expansion generated by TGS into a multi-step tree expansion. Instead of generating only one candidate edge per sampling and nearest-neighbor operation, TGMP reuses the selected tangent-space context and expansion direction to generate multiple consecutive candidates within a single EXTEND call.

For exploratory expansion, the tangent-space sample \bar{q}_{\mathrm{rand}} and the local representation \bar{q}_{\mathrm{near}} of the selected expansion node define the displacement

v_{\mathrm{ext}}=\bar{q}_{\mathrm{rand}}-\bar{q}_{\mathrm{near}}.

If \|v_{\mathrm{ext}}\|_{2}\leq\epsilon_{\mathrm{dir}}, EXTEND returns Trapped. Otherwise, the displacement is normalized as

u=\frac{v_{\mathrm{ext}}}{\|v_{\mathrm{ext}}\|_{2}},

and generates the k-th tangent-space candidate as

\bar{q}_{k}=\bar{q}_{\mathrm{near}}+k\xi u,\qquad k=1,\ldots,K,

where \xi denotes the extension step size and K\leq K_{\max}. The ordered sequence Q_{\mathrm{cand}}=(\bar{q}_{1},\ldots,\bar{q}_{K}) represents a multi-step expansion along the common tangent-space direction and defines the candidate edges processed in the subsequent projection and validation stage.

E_{M}(\bar{q}_{k})=\|h(\bar{q}_{k})\|_{2}.

Because the affine tangent space provides only a local linear approximation of the nonlinear constraint manifold, a generated candidate \bar{q}_{k} does not necessarily satisfy the nonlinear constraints. PATACON evaluates its constraint residual as

Candidate generation terminates when K_{\max} candidates have been generated or immediately after appending the first candidate satisfying

E_{M}(\bar{q}_{k})>\epsilon_{M}.

The threshold-crossing candidate remains in Q_{\mathrm{cand}} and is passed to the projection and validation stage. The residual E_{M} is used only as a practical criterion for determining when the current tangent-space approximation should be renewed and is not interpreted as the geometric distance from \bar{q}_{k} to the constraint manifold. Moreover, generating a threshold-crossing candidate alone does not establish its feasibility or immediately create a new tangent space.

TGMP itself defines the multi-step expansion independently of the execution schedule used for candidate-edge validation. Therefore, the candidate edges generated by TGMP can be processed sequentially, as in the +\mathrm{TGMP} ablation variant, or concurrently on the GPU, as in the full PATACON planner.

IV-C GPU-Parallel Edge Projection and Validation

PATACON parallelizes the projection and validation of the multiple candidate edges generated by TGMP. Different candidate edges are assigned to separate cooperative thread groups and processed concurrently, whereas the waypoint projection within each individual edge follows the original sequential projection order.

For each candidate edge, intermediate waypoints are generated according to the edge-discretization resolution G. Starting from the edge source, the waypoints are projected sequentially toward the constraint manifold so that each projection step follows the preceding projected waypoint. Collision checking and feasibility validation are performed for the projected edge, while multiple candidate edges undergo this procedure concurrently on the GPU.

An edge is considered valid only when its sequential waypoint projection converges within the prescribed projection tolerances and all projected configurations satisfy the joint limits and collision constraints. During target-directed CONNECT, the target-progress condition described in Sec. IV-D is additionally applied.

Although candidate edges are processed concurrently, their validation results are consumed in the original TGMP expansion order. Only the longest consecutively valid prefix beginning with the first candidate edge is inserted into the tree. If the first candidate edge is invalid, EXTEND returns Trapped; otherwise, the valid prefix is inserted and EXTEND returns Advanced with the final inserted configuration q_{\mathrm{end}}.

Tangent-space renewal is determined separately from valid-prefix insertion. A new tangent space is created at q_{\mathrm{end}} only when the entire TGMP candidate batch is accepted and the final tangent-space candidate satisfies

E_{M}(\bar{q}_{\mathrm{end}})>\epsilon_{M}.

The boundary node then becomes the root of the new tangent space with \bar{q}_{i}=q_{i}; otherwise, the inserted nodes remain associated with their original tangent space. Consequently, full-batch acceptance is required only for tangent-space renewal, whereas any nonempty valid prefix is sufficient to trigger the subsequent CONNECT attempt.

IV-D Target-Directed Connection and Bridge Validation

After an exploratory EXTEND returns Advanced, PATACON selects the closest configuration in the opposite tree as a fixed target for the current CONNECT attempt,

q_{\mathrm{tar}}=\argminB_{q_{j}\in V(T_{\mathrm{opp}})}\operatorname{dist}(q_{j},q_{\mathrm{end}}).

Unlike the tangent-space-local nearest-neighbor search used by TGS, this target search considers all configurations in the opposite tree. Starting from q_{\mathrm{cur}}=q_{\mathrm{end}}, PATACON repeatedly applies EXTEND to T_{\mathrm{grow}} toward the same fixed target q_{\mathrm{tar}}. During a CONNECT attempt, the opposite tree provides the target configuration but is not itself expanded by that CONNECT operation.

At each CONNECT step, let \mathcal{T}_{r} denote the tangent space associated with q_{\mathrm{cur}}. The ambient displacement toward the fixed target is projected onto the tangent vector space as

v_{\mathrm{con}}=P_{\mathcal{T}_{r}}(q_{\mathrm{tar}}-q_{\mathrm{cur}}).

The resulting v_{\mathrm{con}} is passed to the same TGMP-based EXTEND procedure used during exploratory expansion. Thus, CONNECT also generates a multi-step candidate sequence and processes the resulting candidate edges using the GPU-parallel edge projection and validation procedure described in Sec. IV-C.

Because numerical projection can modify the motion implied by the tangent-space direction, satisfying the projection and collision constraints alone does not guarantee progress toward the fixed target. Therefore, each candidate edge considered during CONNECT must additionally satisfy

\operatorname{dist}(q_{k},q_{\mathrm{tar}})<\operatorname{dist}(q_{k-1},q_{\mathrm{tar}})-\epsilon_{\mathrm{prog}},

where \epsilon_{\mathrm{prog}}>0 denotes the minimum required reduction in distance to the target. If an edge fails this condition, that edge is rejected and valid-prefix insertion terminates before it, even if its projection and collision checks succeed.

After each successful EXTEND, q_{\mathrm{cur}} is updated to the returned q_{\mathrm{end}}, and PATACON evaluates

\operatorname{dist}(q_{\mathrm{cur}},q_{\mathrm{tar}})\leq\epsilon_{\mathrm{con}}.

When this condition is satisfied, the remaining bridge between q_{\mathrm{cur}} and q_{\mathrm{tar}} is projected and validated using the same sequential waypoint projection and collision-validation procedure. The two trees are considered connected only if this final bridge validation succeeds; otherwise, the CONNECT attempt continues until EXTEND returns Trapped or the maximum number of EXTEND calls J_{\max} is reached.

Algorithm 3 summarizes the target-proximity test, final bridge validation, and path construction.

V Experiments

We conducted three sets of experiments to evaluate PATACON. First, we compare planner performance across regimes spanning increasingly restrictive constraints and higher-dimensional configuration spaces. Second, we perform a cumulative ablation study by progressively introducing Tangent-Guided Sampling (TGS) and Tangent-Guided Multi-Edge Projection (TGMP) to cpRRTC. Finally, we evaluate solution-quality improvement under a common anytime optimization framework.

V-A Experimental Environment

We evaluate PATACON primarily against two representative constrained motion planners: McVAMP [23] and cpRRTC [4], representing CPU SIMD-accelerated manifold-constrained planning and GPU-parallel projection-based RRT-Connect, respectively. We additionally include TB-RRT [5] as a tangent-space-based constrained planner on the single-arm Franka benchmark.

For McVAMP, we use its publicly available implementation. For cpRRTC, we implement the planner by extending the publicly available pRRTC codebase according to the algorithm described in the cpRRTC paper. For TB-RRT, we use the implementation provided by OMPL.

Within each benchmark regime, all planners use the same robot model, environment, start–goal pairs, joint limits, constraint definitions, feasibility tolerances, and planning timeout. For each regime, we randomly sample 100 feasible start–goal joint-state pairs, which are shared across all evaluated planners.

All experiments were conducted on a workstation equipped with an AMD Ryzen 7 9800X3D CPU, an NVIDIA GeForce RTX 5090 GPU, and 64 GB of system memory. Unless otherwise specified, PATACON used N_{\mathrm{blk}}=512, K_{\max}=4, \epsilon_{M}=0.1, and an edge-discretization resolution of G=16. One-time robot, environment, and device initialization was excluded from the timing measurements, while all computations required for each planning query were included.

The benchmark robots span configuration spaces of different dimensions: (A) 7-DoF for the Franka Panda, (B) 14-DoF for the dual Franka, (C) 15-DoF for the fixed-base FFW-SG2, (D) 18-DoF for the mobile-base FFW-SG2, and (E) 35-DoF for the Unitree G1. We denote the constraints as follows: BK is a 6D bimanual kinematic-chain equality constraint on the relative end-effector pose; OR is a 2D orientation equality constraint that aligns the z-axes while leaving rotation about the aligned axis unconstrained; and FT is a 12D equality constraint that fixes the poses of both feet (6D per foot). CoM constrains the planar (x,y) center of mass to lie within the support region using inequality constraints.

V-B Planning Performance across Constraint Regimes

Fig. 5 compares the planning time and success rate across the benchmark regimes. Each trial is limited to 500 s, and the RRT extension range is set to \xi=0.4 where applicable. TB-RRT is evaluated only on the single-arm Franka benchmark because preliminary evaluations showed substantially larger planning times on higher-dimensional problems. McVAMP is evaluated on the Franka and dual-Franka benchmarks, but is omitted from the higher-dimensional FFW-SG2 and Unitree G1 benchmarks due to similarly high computational cost. We therefore focus on cpRRTC and PATACON in these higher-dimensional regimes.

On the Franka benchmarks, cpRRTC and PATACON show comparable planning performance, with their relative advantage varying across the constraint settings. In contrast, PATACON consistently achieves lower planning latency across the higher-dimensional FFW-SG2 and Unitree G1 benchmarks. Its relative advantage becomes more pronounced in the more restrictive constraint settings, particularly when the axis constraint is introduced. This trend is most evident on the fixed-base FFW-SG2 benchmark with bimanual-pose and axis constraints, where the mean planning latency corresponds to an equivalent planning throughput of approximately 38.5 solutions per second for PATACON, compared with 3.4 for cpRRTC. A similar tendency is observed on the Unitree G1 benchmarks, where PATACON is faster both with and without the axis constraint, while the performance gap increases after the axis constraint is added. These results suggest that the benefit of tangent-space-guided parallel exploration becomes more pronounced in higher-dimensional and more restrictive constrained planning regimes.

V-C Ablation Study

Starting from cpRRTC, +TGS introduces the tangent-guided exploration described in Sec. IV-A. +TGMP further generates multiple consecutive candidate edges within a single EXTEND call, but processes them edge by edge: each candidate edge is projected and collision-validated before the next edge is processed. PATACON retains this tangent-guided multi-edge expansion, but projects and validates multiple candidate edges concurrently on the GPU while preventing backtracking toward previously explored tangent-space regions.

TGS alone increases planning time because constructing and maintaining tangent-space information introduces additional overhead despite providing geometry-aware exploration and a restricted nearest-neighbor search. TGMP reduces this overhead by reusing the same tangent-space context across multiple consecutive expansion steps, thereby amortizing the cost of tangent-space construction.

PATACON further reduces planning time by projecting and validating multiple candidate edges in parallel on the GPU. In addition, the backtracking-prevention strategy biases expansion away from the parent tangent-space region, reducing redundant exploration of already visited regions and promoting progress toward unexplored portions of the constraint manifold. Overall, the results show that tangent-guided exploration becomes most effective when combined with multi-edge expansion, GPU-parallel edge processing, and directed exploration that avoids backtracking.

V-D Anytime Planning Performance

The previous experiments demonstrate that PATACON can rapidly obtain a feasible solution. This low first-solution latency leaves additional computation time that can be exploited for further solution refinement. We therefore embed PATACON in an AORRTC-based anytime optimization framework [30] and evaluate whether the remaining computation budget can be used to progressively improve solution quality on the G1 whole-body benchmark.

The resulting anytime planner is denoted AO-PATACON. AO-PATACON does not continue expanding the existing trees after finding the first feasible solution. Instead, it uses the cost of that solution as an upper bound, reinitializes the trees, and restarts the search. Whenever a lower-cost solution is found, the cost bound is tightened and the search is restarted again. This process is repeated throughout the 10-s optimization budget.

The best-so-far solution cost at optimization time t is defined as

C^{*}(t)=\min_{\tau\leq t}C(\sigma_{\tau}),

where \sigma_{\tau} denotes a feasible solution obtained at time \tau, and the configuration-space path length is used as the solution cost.

To quantify the improvement obtained after the first feasible solution, we define the relative cost reduction as

R_{\mathrm{cost}}=\frac{C_{\mathrm{init}}-C^{*}(T_{\mathrm{AO}})}{C_{\mathrm{init}}}\times 100,

where C_{\mathrm{init}} denotes the cost of the first feasible solution.

As shown in Fig. 6, AO-PATACON reaches a 100% success rate during the early stage of planning and subsequently continues to improve the quality of the incumbent solution within the remaining optimization budget. The best-so-far path cost decreases in a stepwise manner as lower-cost feasible paths are discovered, with the largest reductions occurring shortly after the first feasible solutions are obtained. The magnitude of subsequent improvements gradually decreases, indicating convergence toward lower-cost solutions as additional computation time is provided.

These results show that, because PATACON finds the first feasible solution quickly, a larger portion of the fixed planning budget can be devoted to subsequent solution refinement. AO-PATACON can then use the remaining computation time to progressively improve path quality in an anytime manner.

VI Conclusion

In this work, we presented PATACON, a GPU-parallel tangent-bundle RRT-Connect framework for constrained motion planning. PATACON integrates tangent-space-guided exploration with GPU-parallel tree expansion and validation, enabling efficient exploration of constrained configuration spaces. Experimental results across dual-arm and whole-body planning problems show that PATACON becomes increasingly advantageous as the constraints become more restrictive and the planning problem becomes more challenging. These results demonstrate the effectiveness of combining local tangent-space guidance with massively parallel GPU execution for high-dimensional constrained motion planning.

Acknowledgments

We used ChatGPT (OpenAI) to refine the language of the manuscript and Codex (OpenAI) to assist in writing portions of the code used in the experiments reported in Section V.

[Correction of Benchmark Results]

After the initial submission, we found that a computational task had not terminated and was still running on the same GPU during benchmarking. This caused resource contention and led to overestimated cpRRTC planning times in the Franka OR (2D) and Dual Franka BK+OR (8D) benchmarks. The originally reported planning times of 2.11\pm 2.18 ms and 8.94\pm 4.90 ms were corrected to 0.52\pm 0.06 ms and 5.55\pm 0.66 ms, respectively, after re-evaluation without concurrent GPU workloads. The corrected results, reflected in Table I and Fig. 5, show that cpRRTC achieves lower mean planning times than PATACON in both benchmarks.

References

1. [1] Z. Kingston, M. Moll, and L. E. Kavraki (2018) Sampling-based methods for motion planning with constraints. Annual Review of Control, Robotics, and Autonomous Systems 1, pp. 159–185. External Links: Document
2. [2] A. Yershova and S. M. LaValle (2007) Improving motion-planning algorithms by efficient nearest-neighbor searching. IEEE Transactions on Robotics 23 (1), pp. 151–157. External Links: Document
3. [3] M. Kleinbort, O. Salzman, and D. Halperin (2020) Collision detection or nearest-neighbor search? on the computational bottleneck in sampling-based motion planning. In Algorithmic Foundations of Robotics XII, pp. 624–639.
4. [4] J. Hu, J. Wang, and H. I. Christensen (2025) cpRRTC: GPU-parallel RRT-connect for constrained motion planning. External Links: 2505.06791
5. [5] B. Kim, T. T. Um, C. Suh, and F. C. Park (2016) Tangent bundle RRT: a randomized algorithm for constrained motion planning. Robotica 34 (1), pp. 202–225. External Links: Document
6. [6] M. Stilman (2010) Global manipulation planning in robot joint space with task constraints. IEEE Transactions on Robotics 26 (3), pp. 576–584. External Links: Document
7. [7] J. J. Kuffner and S. M. LaValle (2000) RRT-connect: an efficient approach to single-query path planning. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Vol. 2, pp. 995–1001. External Links: Document
8. [8] J. H. Yakey, S. M. LaValle, and L. E. Kavraki (2001) Randomized path planning for linkages with closed kinematic chains. IEEE Transactions on Robotics and Automation 17 (6), pp. 951–958.
9. [9] D. Berenson, S. S. Srinivasa, D. Ferguson, and J. J. Kuffner (2009) Manipulation planning on constraint manifolds. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 625–632. External Links: Document
10. [10] D. Berenson, S. S. Srinivasa, and J. J. Kuffner (2011) Task space regions: a framework for pose-constrained manipulation planning. The International Journal of Robotics Research 30 (12), pp. 1435–1460. External Links: Document
11. [11] L. Jaillet and J. M. Porta (2013) Path planning under kinematic constraints by rapidly exploring manifolds. IEEE Transactions on Robotics 29 (1), pp. 105–117. External Links: Document
12. [12] Z. Kingston, M. Moll, and L. E. Kavraki (2019) Exploring implicit spaces for constrained sampling-based planning. The International Journal of Robotics Research 38 (10–11), pp. 1151–1178. External Links: Document
13. [13] B. Ichter and M. Pavone (2019) Robot motion planning in learned latent spaces. IEEE Robotics and Automation Letters 4 (3), pp. 2407–2414. External Links: Document
14. [14] G. Sutanto, I. R. Fernández, P. Englert, R. K. Ramachandran, and G. Sukhatme (2021) Learning equality constraints for motion planning on manifolds. In Conference on Robot Learning, pp. 2292–2305.
15. [15] S. Park, S. Jeon, and J. Park (2024) A constrained motion planning method exploiting learned latent space for high-dimensional state and constraint spaces. IEEE/ASME Transactions on Mechatronics 29 (4), pp. 3001–3009. External Links: Document
16. [16] A. H. Qureshi, J. Dong, A. Baig, and M. C. Yip (2022) Constrained motion planning networks X. IEEE Transactions on Robotics 38 (2), pp. 868–886. External Links: Document
17. [17] J. Bialkowski, S. Karaman, and E. Frazzoli (2011) Massively parallelizing the RRT and the RRT*. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3513–3518. External Links: Document
18. [18] J. Pan and D. Manocha (2012) GPU-based parallel collision detection for fast motion planning. The International Journal of Robotics Research 31 (2), pp. 187–200.
19. [19] M. Otte and N. Correll (2013) C-FOREST: parallel shortest-path planning with superlinear speedup. IEEE Transactions on Robotics 29 (3), pp. 798–806. External Links: Document
20. [20] J. Ichnowski and R. Alterovitz (2012) Parallel sampling-based motion planning with superlinear speedup. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1206–1212.
21. [21] S. A. Jacobs, K. Manavi, J. Burgos, J. Denny, S. Thomas, and N. M. Amato (2012) A scalable method for parallelizing sampling-based motion planning algorithms. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 2529–2536.
22. [22] W. Thomason, Z. Kingston, and L. E. Kavraki (2024) Motions in microseconds via vectorized sampling-based planning. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 8749–8756. External Links: Document
23. [23] S. R. Iyer, I. Chang, A. Z. Liu, Y. Gu, and Z. Kingston (2026) Vectorizing projection in manifold-constrained motion planning for real-time whole-body control. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Note: To Appear
24. [24] B. Sundaralingam, S. K. S. Hari, A. Fishman, C. Garrett, K. Van Wyk, V. Blukis, A. Millane, H. Oleynikova, A. Handa, F. Ramos, N. Ratliff, and D. Fox (2023) CuRobo: parallelized collision-free robot motion generation. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 8112–8119. External Links: Document
25. [25] C. H. Huang, P. Jadhav, B. Plancher, and Z. Kingston (2026) pRRTC: GPU-parallel RRT-connect for fast, consistent, and low-cost motion planning. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
26. [26] J. Liu, M. Li, Z. Liu, and T. Sun (2026) BatchRRT: a fully-batched reformulation of RRT with GPU-accelerated SDF collision checking for real-time human-robot collision avoidance. IEEE Robotics and Automation Letters 11 (5), pp. 6384–6391. External Links: Document
27. [27] J. J. Kuffner and S. M. LaValle (2005) An efficient approach to path planning using balanced bidirectional RRT search. Technical report Robotics Institute, Carnegie Mellon University, Pittsburgh, PA.
28. [28] J. H. Halton (1960) On the efficiency of certain quasi-random sequences of points in evaluating multi-dimensional integrals. Numerische Mathematik 2 (1), pp. 84–90. External Links: Document
29. [29] M. S. Branicky, S. M. LaValle, K. Olson, and L. Yang (2001) Quasi-randomized path planning. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Vol. 2, pp. 1481–1487. External Links: Document
30. [30] T. S. Wilson, W. Thomason, Z. Kingston, and J. D. Gammell (2025) AORRTC: almost-surely asymptotically optimal planning with RRT-connect. IEEE Robotics and Automation Letters. Note: Accepted September 2025