Just in time Informed Trees: Manipulability-Aware Asymptotically Optimized Motion Planning

TL;DR

The paper tackles high-dimensional robotic motion planning with manipulability and self-collision concerns by proposing Just-in-Time Informed Trees (JIT*), an anytime optimal planner that extends EIT* with two modules: Just-in-Time for adaptive edge connectivity and sampling, and Motion Performance for manipulability-aware optimization. JIT* uses a bidirectional lazy search framework guided by an informed random geometric graph, densifying sampling specifically in bottleneck regions and adaptively expanding edges to accelerate initial feasible solutions while preserving asymptotic optimality. The manipulability-focused cost term, null-space refinements, and self-collision bias sampling together reduce singularities and inter-arm interference, demonstrated across $\mathbb{R}^4$ to $\mathbb{R}^{16}$ and in single- and dual-arm real-robot tasks. Experiments show JIT* achieving faster initial solutions, lower path costs, higher success rates, and improved manipulability compared to state-of-the-art planners, indicating meaningful practical impact for real-time, safe, high-dimensional manipulation. The work provides theoretical guarantees of probabilistic completeness and almost-sure asymptotic optimality and discusses limitations and avenues for future enhancement such as handling dynamic environments and leveraging hardware acceleration.

Abstract

In high-dimensional robotic path planning, traditional sampling-based methods often struggle to efficiently identify both feasible and optimal paths in complex, multi-obstacle environments. This challenge is intensified in robotic manipulators, where the risk of kinematic singularities and self-collisions further complicates motion efficiency and safety. To address these issues, we introduce the Just-in-Time Informed Trees (JIT*) algorithm, an enhancement over Effort Informed Trees (EIT*), designed to improve path planning through two core modules: the Just-in-Time module and the Motion Performance module. The Just-in-Time module includes "Just-in-Time Edge," which dynamically refines edge connectivity, and "Just-in-Time Sample," which adjusts sampling density in bottleneck areas to enable faster initial path discovery. The Motion Performance module balances manipulability and trajectory cost through dynamic switching, optimizing motion control while reducing the risk of singularities. Comparative analysis shows that JIT* consistently outperforms traditional sampling-based planners across $\mathbb{R}^4$ to $\mathbb{R}^{16}$ dimensions. Its effectiveness is further demonstrated in single-arm and dual-arm manipulation tasks, with experimental results available in a video at https://youtu.be/nL1BMHpMR7c.

Figures (4)

• Figure 1: System diagram of the proposed path planning method. After setting the task's start and end points, our algorithm optimizes the dynamic heuristic costs across each edge. It comprises two main modules: the Just-in-Time module, which enhances trajectory efficiency through Edge and Sample components, and the Motion Performance module, which optimizes manipulability and prevents self-collisions, ensuring the trajectory aligns with the manipulator’s dynamic constraints.
• Figure 2: The 2D experimental scenarios.
• Figure 3: Experimental results for Just-in-time verification. MaxTime is the planner's maximum allotted planning time. (a), (c), and (e) depict test benchmark narrow passage outcomes in $\mathbb{R}4$, $\mathbb{R}^8$, and $\mathbb{R}^{16}$, respectively. (b), (d), and (f) showcase random rectangle experiments in $\mathbb{R}^4$, $\mathbb{R}^8$, $\mathbb{R}^{16}$.
• Figure 4: Results of real robot experiments. Fig. (a) shows the GPIT start and end points, while Fig. (b) shows the optimized end configuration by our method with the same end-effector position but a different joint setup; dashed outlines indicate start positions, and solid outlines the end. Figs. (c) and (d) illustrate the DOPOT start and end points, and Figs. (e) and (f) display the PWT. Fig. (g) gives the minimal singularity value for the GPIT, with Figs. (h) and (j) showing minimal values for the right arm in DOPOT and PWT, and Figs. (i) and (k) for the left arm. The third row shows additional results: Figs. (l) and (m) show solution cost and success rate for GPIT, Figs. (n) and (o) for DOPOT, and Figs. (p) and (q) for PWT.