geodex: A Library for Motion Planning on Riemannian Manifolds

Phone Thiha Kyaw; Ben Wei; Sepehr Samavi; Miguel Angel Rogel Garcia; Jonathan Kelly

cs.ROOct 6, 2026

geodex: A Library for Motion Planning on Riemannian Manifolds

Authors: Phone Thiha Kyaw, Ben Wei, Sepehr Samavi, Miguel Angel Rogel Garcia, Jonathan Kelly

Organizations: Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory at the University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Ontario, Canada, M3H 5T6

Abstract

Planning motions that respect the intrinsic geometry of a robot's configuration space, including its curvature and a configuration-dependent notion of cost, yields shorter, lower-energy, and more natural trajectories than planning under the ambient flat metric. Existing libraries for optimization on manifolds provide rich geometric primitives but do not plan around obstacles. While general-purpose motion planning libraries support many state spaces and custom distance functions, they do not yet treat a configuration-dependent Riemannian metric as the geometry that drives distance, interpolation, and geodesics. We present geodex, an open-source C++20 library with Python bindings. The library exposes the manifold, its Riemannian metric, the retraction, and the sampler as independent, interchangeable components through a single sampling-based motion planning interface. The same planner runs unchanged on canonical spaces $\mathbb{R}^n$ , $\mathbb{T}^n$ , $\mathbb{S}^n$ , matrix Lie groups such as $SO(2)$ , $SE(2)$ , $SO(3)$ , and $SE(3)$ , products of these spaces, and articulated-robot configuration spaces, each equipped with a user-defined Riemannian metric. We make geodex publicly available with documentation, tests, and a reproducible benchmark suite.

Figures & tables

Fig. 1 : In geodex , the metric comes first. A user builds a space from a manifold and three swappable policies: a metric, a retraction, and a sampler. We show a Franka manipulator moving a box between shelf compartments under the kinetic-energy metric, and a Clearpath Jackal planning nonholonomic motions on the Lie group $\mathrm{SE}(2)$ under a left-invariant metric. Both tasks use the same planner, and only the space argument changes.

	Planning			Geometry			Sampling		Interface
	Obstacles	Sampling- based	Asympt. optimal	Riem. metric	Geodesic paths	Multiple manifolds	Informed	Low- discrep.	C++	Python
OMPL [ 8 ]	✓	✓	✓	✗	✗	✓	✓	✗	✓	✓
MoveIt [ 9 ]	✓	✓	✓	✗	✗	✓	✓	✗	✓	✓
VAMP [ 10 ]	✓	✓	✓	✗	✗	✗	✓	✓	✓	✓
cuRoboV2 [ 12 ]	✓	✗	✗	✗	✗	✗	✗	✗	✓	✓
PyRoki [ 13 ]	✓	✗	✗	✗	✗	✗	✗	✗	✗	✓

TABLE I : Comparison of widely used motion-planning and geometry libraries. Planning libraries offer obstacle-aware, sampling-based search but treat geometry as a fixed ambient metric, whereas geometry libraries offer configuration-dependent Riemannian metrics and geodesics but do not plan around obstacles. geodex is the only library that provides both, combining collision-aware, asymptotically optimal sampling-based planning with a first-class, configuration-dependent Riemannian metric, informed search, low-discrepancy on-manifold sampling, and both C++ and Python interfaces.

Manifold	Dim	Topology	Retractions
Euclidean $\mathbb{R}^{n}$	$n$	flat, optional bounds	addition
Torus $\mathbb{T}^{n}$	$n$	periodic, wrapped	wrapped addition
Sphere $\mathbb{S}^{n}$	$n$	round, ambient $\mathbb{R}^{n+1}$	exp, projection †
$\mathrm{SO}(2)$	$1$	circle group $\cong\mathbb{S}^{1}$	group exp
$\mathrm{SO}(3)$	$3$	rotation group	left/right exp
$\mathrm{SE}(2)$	$3$	planar rigid group	left/right exp, Euler †

TABLE II : Manifolds and metrics provided by geodex with the retractions available for each. Every manifold models the RiemannianManifold concept and pairs with any compatible metric and sampler. A dagger (

\dagger

) marks a cheaper first-order retraction; all others are exact under the default Riemannian metric, and products compose block-wise with exact product distance. A configuration-dependent metric is a single user callback, and a weighted sum of metrics is again a metric.

Fig. 3 : Low-discrepancy sampling on manifolds. Pseudo-random points on the unit square clump and leave gaps (left), while a Halton sequence of the same size spreads evenly (center). The measure-preserving from_unit_cube map then carries these same points onto the sphere, shown here as the equal-area map from the unit square to

\mathbb{S}^{2}

(right), so the points that are uniform in the square remain uniform on the manifold.

Fig. 4 : Shelf manipulation with a Franka robot. The arm moves a cracker box from the bottom compartment to the top one under the Euclidean metric (left) and under the kinetic-energy metric (right), with configurations overlaid along each execution. The two runs differ only in the metric; with the kinetic-energy metric, the arm turns its heavier joints less and its lighter joints more.

geodex , Euclidean metric (Ours)
			Length $\,\downarrow$		Energy $\,\downarrow$
Planner	Succ. $\,\uparrow$	Time	Euclid.	Kinetic	Median	Best
MoveIt2 RRT-Connect	80.0%	351 ms	3.521	3.174	1.503	0/6
MoveIt2 CHOMP	63.3%	116 ms	3.554	3.804	2.768	0/6
cuRoboV2 (GPU)	100.0%	185 ms	1.611	1.841	0.661	0/6
initial	100.0%	54 ms	4.656	3.434	–	–

TABLE III : Shelf manipulation over the six problems, five trials each. Success is the fraction of runs that return a valid path. Time is the planning time for the baselines and the budget for our anytime sampling-based planner. We measure length under both metrics, after shortcutting and smoothing. Energy is the median kinetic energy of the executed motion on the real robot, and Best counts how many of the six problems a planner solves with the least energy. cuRoboV2 runs on a GPU and the others on a single CPU core.

Fig. 5 : Map (roughly $1,600~\mathrm{m}^{2}$ ) used for nonholonomic navigation on $\mathrm{SE}(2)$ . White areas are traversable, and gray indicate obstacles. We also illustrate areas traversed by the robot in the real-robot experiments (Sec. V-B2 ). The narrow areas are inside a cluttered office space, and the far areas a publicly accessible atrium space in a university building.

Planner	Succ. $\,\uparrow$		Coll. $\,\downarrow$		Time $\,\downarrow$		Len. $\mathbb{R}^{2}\,\downarrow$		Len. $\mathrm{SE}(2)\,\downarrow$		Smooth. $\,\downarrow$		Succ. $\,\uparrow$		Coll. $\,\downarrow$		Time $\,\downarrow$		Len. $\mathbb{R}^{2}\,\downarrow$		Len. $\mathrm{SE}(2)\,\downarrow$		Smooth. $\,\downarrow$
	Far scenarios												Narrow scenarios
Theta*	100.00 /	100.00	0.00/	0.00	363/	87	38.75 /	38.82	54.37/	48.15	0.602/	0.381	100.00/	100.00	30.67 /	27.67	24 /	9	6.52 /	6.48	15.99/	14.40	1.453/	1.151
Smac-Lattice	88.48/	88.48	0.50 /	0.00	179/	55	40.02/	39.32	51.49/	51.66	0.319/	0.308	73.00/	73.50	0.00/	1.50	34/	15	6.95/	7.06	14.94/	14.50	0.834/	0.848
Smac-HybA*	100.00 /	100.00	0.00/	0.00	101/	26	39.45/	39.30	50.60/	49.02	0.444/	0.396	94.50/	94.50	1.00 /	2.00	32/	9	7.03/	6.79	13.73/	13.26	0.837/	0.829
geodex (Ours)	100.00 /	100.00	0.00/	0.00	72 /	48	41.79/	41.82	47.00 /	47.15	0.219 /	0.223	100.00 /	100.00	0.00/	0.00	53/	37	7.12/	7.18	11.64 /	11.78	0.612 /	0.614

TABLE IV : Planning results on the building map for the far (left) and narrow (right) scenarios, with each cell giving 2 cm / 5 cm resolution. Succ.: % of queries returning a path. Coll.: % of those paths in collision. Other columns are medians over queries solved by all planners, with time in ms,

\mathbb{R}^{2}

length in m,

\mathrm{SE}(2)

length under the plugin metric, and smoothness in rad/m.

Panel of Figure 9 — Fig. 6 : Narrow scenarios on the Jackal. Left of each panel: the global costmap at the time of planning, the start (solid) and goal (dashed) footprints, and one exemplary executed trajectory per planner. Right: planning time and time to goal (bars are the mean, whiskers span the full range, dots are individual runs), and the mean with one standard deviation of speed and cumulative steering effort $\int|\dot{\omega}|\,dt$ against normalized progress along the executed path. geodex plans $2.5$ to $3\times$ faster, arrives first, holds a higher cruise speed, and accumulates less steering effort, because it optimizes on $\mathrm{SE}(2)$ rather than a Euclidean surrogate.

Panel of Figure 10 — Fig. 7 : Far scenarios on the Jackal. Over these routes, the planning-time gap between geodex and the baselines widens to $3$ to $7\times$ , and the baselines shed speed at every corner while geodex holds its cruise speed, accumulating roughly half the steering effort. The baselines were run once per scenario.

Explore similar work

RIT*: Riemannian Informed Trees for Cost-Adaptive Optimal Motion Planning
Aug 1, 2026Muhayy Ud Din, Ahmed Nadar, Jan Rosell +1Motion Planning Obstacle Avoidance
Center for Autonomous Robotic Systems, Khalifa University (KU-CARS) · Institute of Industrial and Control Engineering, Universitat Politècnica de Catalunya, Barcelona, Spain
Induced Riemannian Metrics for Motion Planning with Constraints
Sep 22, 2026Phone Thiha Kyaw, Thomas Cohn, Miguel Angel Rogel Garcia +1Motion Planning Riemannian Manifolds
Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory at the University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Ontario, Canada, M3H 5T6
Learning from Demonstrations over Riemannian Manifolds using Neural ODEs: An Extended Abstract
Jun 3, 2026Diana Cuervo Espinosa, Mahathi Anand, Angela P. SchoelligGeodesic Riemannian Manifolds
Chair of Robotics and System Intelligence, Technical University of Munich, Germany · Learning Systems and Robotics Lab, Technical University of Munich, Germany

Aug 1, 2026, cs.ROJ/K move · Enter open · S save

RIT*: Riemannian Informed Trees for Cost-Adaptive Optimal Motion Planning

Muhayy Ud Din, Ahmed Nadar, Jan Rosell, Irfan Hussain

Center for Autonomous Robotic Systems, Khalifa University (KU-CARS) · Institute of Industrial and Control Engineering, Universitat Politècnica de Catalunya, Barcelona, Spain

Motion Planning Obstacle Avoidance Motion Anisotropic

Open paper AskPDF arXiv

We present Riemannian Informed Trees (RIT*), a planning framework that replaces Euclidean primitives in batch-informed search with their Riemannian counterparts. RIT* constructs a tighter, cost-consistent informed set, performs a nearest-neighbour search under an anisotropic distance metric, and evaluates edge costs efficiently via a cascading scheme. We further introduce a Collision-Adaptive Metric Refinement (CARM), which learns an obstacle-proximity cost field online from collision feedback, reducing the reliance on prior metric design in practical settings. Experiments across environments from 2-D to 14-D show that RIT* is competitive in low-dimensional and spatially constant-metric settings and produces substantially lower-cost solutions when the metric varies spatially in high-dimensional configuration spaces. Performance gains scale with anisotropy and dimension, reaching up to 13.0% improvement in median initial cost over BIT* in the 3-D anisotropic benchmark, up to 9.0% in median final cost over BIT* in 6-DOF manipulation, and 24.8-63.5% in a 14-DOF bimanual planning problem, where Euclidean-informed baselines degrade. Videos and code can be found here: https://muhayyuddin.github.io/ritstar/

Abstract

Explore similar work

RIT*: Riemannian Informed Trees for Cost-Adaptive Optimal Motion Planning

Induced Riemannian Metrics for Motion Planning with Constraints

Learning from Demonstrations over Riemannian Manifolds using Neural ODEs: An Extended Abstract