Geometry-Aware Sampling-Based Motion Planning on Riemannian Manifolds

In many robot motion planning problems, task objectives and physical constraints induce non-Euclidean geometry on the configuration space, yet many planners operate using Euclidean distances that ignore this structure. We address the problem of planning collision-free motions that minimize length under configuration-dependent Riemannian metrics, corresponding to geodesics on the configuration manifold. Conventional numerical methods for computing such paths do not scale well to high-dimensional systems, while sampling-based planners trade scalability for geometric fidelity. To bridge this gap, we propose a sampling-based motion planning framework that operates directly on Riemannian manifolds. We introduce a computationally efficient midpoint-based approximation of the Riemannian geodesic distance and prove that it matches the true Riemannian distance with third-order accuracy. Building on this approximation, we design a local planner that traces the manifold using first-order retractions guided by Riemannian natural gradients. Experiments on a two-link planar arm and a 7-DoF Franka manipulator under a kinetic-energy metric, as well as on rigid-body planning in $\mathrm{SE}(2)$ with non-holonomic motion constraints, demonstrate that our approach consistently produces lower-cost trajectories than Euclidean-based planners and classical numerical geodesic-solver baselines.

💡 Research Summary

The paper addresses a fundamental gap in robot motion planning: most sampling‑based planners (e.g., RRT, RRT*) compute distances and interpolate motions using the ambient Euclidean metric, even when the robot’s configuration space is a curved Riemannian manifold induced by task constraints, dynamics, or Lie‑group structure. Ignoring this intrinsic geometry leads to sub‑optimal or even infeasible trajectories, especially when the cost to be minimized (energy, effort, manipulability) is naturally expressed by a configuration‑dependent Riemannian metric.

To close this gap, the authors propose a geometry‑aware sampling‑based planning framework that works directly on Riemannian manifolds. The core contributions are twofold. First, they introduce a midpoint‑based approximation of the true Riemannian geodesic distance. Given two configurations qₓ and q_y, the geodesic midpoint is defined as q_mid = exp_{qₓ}(½·log_{qₓ}(q_y)). The metric is evaluated only at this midpoint, and the distance is approximated by the norm of the difference of the two tangent vectors in the tangent space at q_mid. The authors prove (Theorem 1) that the error of this approximation scales as O(‖qₓ–q_y‖³), i.e., third‑order accuracy, and that it becomes exact as the points approach each other. This approximation requires a single metric evaluation per distance query, making it cheap enough for repeated nearest‑neighbor searches and tree rewiring in high‑dimensional planners.

Second, they design a geometry‑aware local planner that replaces the straight‑line interpolation of classic RRT* with a gradient‑based step guided by the Riemannian natural gradient (∇_R L = G⁻¹∇L). The planner proceeds as follows: (i) select a target direction using the midpoint distance, (ii) take a small step along the natural gradient of the path‑length functional, and (iii) map the resulting tangent vector back onto the manifold via a first‑order retraction (an approximation of the exponential map). This process respects the manifold’s curvature at every extension, ensuring that the generated edges are close to true geodesics under the given metric.

The framework is integrated into standard asymptotically optimal planners (RRT* and its anytime variants) without sacrificing probabilistic completeness or convergence guarantees.

Experimental validation is performed on three benchmarks: (1) a two‑link planar arm (4 DoF) with a kinetic‑energy metric, (2) a 7‑DoF Franka manipulator operating in a cluttered environment under the same metric, and (3) a non‑holonomic vehicle modeled in SE(2) where the metric encodes the non‑holonomic constraint. In all cases, the geometry‑aware planner consistently produces trajectories with lower Riemannian length (i.e., lower energy or effort) than Euclidean‑based RRT* and than a classical numerical geodesic solver that directly integrates the geodesic ODE. Notably, the proposed method achieves comparable or slightly faster planning times because the midpoint distance avoids solving a boundary‑value problem at each query.

In summary, the paper delivers a practical, theoretically grounded solution for high‑dimensional motion planning on manifolds: a cheap, third‑order accurate distance estimator and a natural‑gradient‑driven local planner that together enable sampling‑based algorithms to optimize directly with respect to intrinsic Riemannian costs. The approach opens avenues for future work on more complex, task‑specific metrics (e.g., barrier‑function‑derived metrics) and on extending the theoretical analysis of asymptotic optimality under manifold curvature.