Induced Riemannian Metrics for Motion Planning with Constraints

Phone Thiha Kyaw¹, Thomas Cohn², Miguel Angel Rogel Garcia¹, Jonathan Kelly¹

¹Space & Terrestrial Autonomous Robotic Systems Laboratory, University of Toronto Institute for Aerospace Studies
²Robot Locomotion Group, MIT Computer Science and Artificial Intelligence Laboratory

arXiv PDF Video Interactive 3D BibTeX

Two Franka arms holding a book between their grippers, shown at several instants as they carry it from the bottom compartment of a white bookcase to the top compartment. — Two Franka arms executing a constraint-satisfying motion plan under the kinetic-energy Riemannian metric to transfer a book between shelf compartments in our bimanual experimental setup.

Task and loop-closure constraints restrict a robot's motion to a curved, lower-dimensional submanifold of its configuration space. We show that the implicit and explicit representations of this submanifold induce the same Riemannian metric for any metric on the configuration space, so a planner can choose its metric and its representation independently. We then plan with the induced metric in a sampling-based planner and a trajectory optimizer, and run the resulting motions on two Franka arms that carry a book between the compartments of a shelf.

One geometry

The restricted metric of the implicit representation and the pullback metric of the explicit representation are related by an invertible change of basis.

Any ambient metric

The equivalence holds for the constant Euclidean metric and for any configuration-dependent metric, such as the kinetic-energy metric, whose metric tensor is the robot's mass matrix.

One length, two solvers

A sampling-based planner and a trajectory optimizer both minimize the same induced Riemannian length on the constraint submanifold.

Video

Two Franka arms under a loop-closure constraint transfer a book between shelf compartments. The video shows each of the six tasks under the three metrics, from two synchronized camera views.

Abstract

In constrained motion planning problems, task and loop-closure constraints restrict a robot's motion to a curved, lower-dimensional submanifold of its configuration space. Planners measure path length with a metric, which sets the cost of moving in each direction. Under the Euclidean metric, this cost is the same everywhere, whereas under a general Riemannian metric, such as the kinetic-energy metric, the cost can vary with direction and configuration. Existing methods often describe the submanifold either implicitly, as a constraint level set, or explicitly, through a parameterization. The implicit representation is typically combined with the Euclidean metric of the configuration space, and the explicit representation with the parameter domain, so the path length that a planner minimizes depends on the representation. Instead, we measure path length with the induced metric, which the submanifold inherits from a Riemannian metric on the configuration space. The implicit and explicit representations yield the same induced metric, expressed in different coordinates, and hence the same geometry. This result holds for any Riemannian metric on the configuration space, not only the Euclidean one. The choice of metric is therefore independent of the choice of representation. Using this result, we extend planning under a Riemannian metric from unconstrained spaces to constraint submanifolds by applying the induced metric in both a sampling-based planner and a trajectory optimizer. For an explicit representation, the induced metric also accounts for the distortion that the parameterization introduces. In experiments on a bimanual manipulation setup with two Franka arms under end-effector task constraints, we compare the Euclidean and kinetic-energy metrics.

Induced geometry of constraint submanifolds

A smooth constraint map \(h : \mathcal{M} \to \mathbb{R}^{k}\) encodes the task and loop-closure constraints, and the configurations that satisfy it form the zero level set \(\mathcal{Q} = \{ q \in \mathcal{M} : h(q) = \mathbf{0} \}\). Restricting the ambient metric \(\mat{G}_q\) to the tangent spaces of \(\mathcal{Q}\) makes \(\mathcal{Q}\) a Riemannian manifold in its own right, and the resulting induced metric determines the cost of feasible motion. We derive this metric twice, first from the implicit constraint level set and then from an explicit parameterization.

The parameterization \(\varphi\) maps a square grid on the parameter domain \(\mathcal{U}\) onto the curved submanifold \(\mathcal{Q}\), where it stretches unevenly. At \(q = \varphi(u)\), the columns \(\partial_1\varphi, \partial_2\varphi\) of \(\mat{\Phi} = \mathrm{D}\varphi(u)\) (dashed) and the columns \(b_1, b_2\) of an orthonormal basis \(\mat{B}\) (solid) span the same tangent space \(\mathcal{T}_q\mathcal{Q}\). The ellipse collects the tangent vectors of unit length under the induced metric, and both bases describe it. The pullback metric \(g(u)\) and \(\mat{R}^{\mathsf{T}} \mat{G}_{\mathcal{Q}}(q) \mat{R}\) above agree for every \(u\) and for either ambient metric of this toy example.

Implicit representation

The implicit representation describes \(\mathcal{Q}\) as the zero level set of the constraint map. Its tangent space is the kernel of the constraint Jacobian \(\mat{H} = \mathrm{D}h(q)\), and projection with the damped pseudoinverse returns ambient configurations to \(\mathcal{Q}\). With an orthonormal basis \(\mat{B} \in \mathbb{R}^{n \times d}\) of \(\ker \mat{H}\), the induced metric is

\[ \mat{G}_{\mathcal{Q}}(q) = \mat{B}^{\mathsf{T}} \mat{G}_q \mat{B} \in \mathbb{S}^{d}_{++}. \]

Explicit representation

The explicit representation describes \(\mathcal{Q}\) as the image of a parameterization \(\varphi : \mathcal{U} \subseteq \mathbb{R}^{d} \to \mathcal{Q}\), which satisfies the constraint by construction, so sampling \(\mathcal{U}\) yields feasible configurations without projection. Pulling the ambient metric back through the differential \(\mat{\Phi} = \mathrm{D}\varphi(u)\) gives

\[ g(u) = \mat{\Phi}^{\mathsf{T}} \mat{G}_q \mat{\Phi}, \qquad q = \varphi(u). \]

Theorem (Equivalence of induced metrics). Let \(q = \varphi(u) \in \mathcal{Q}\), let \(\mat{B}\) be an orthonormal basis of \(\ker \mat{H}\), and let \(\mat{R} = \mat{B}^{\mathsf{T}} \mat{\Phi}\). Then \(\mat{R}\) is invertible and the pullback metric and the induced metric are equivalent,

\[ g(u) = \mat{R}^{\mathsf{T}} \mat{G}_{\mathcal{Q}}(q) \, \mat{R}. \]

The two therefore define the same inner product on \(\mathcal{T}_q\mathcal{Q}\), and hence the same lengths, angles, and geodesics on \(\mathcal{Q}\).

Since the ambient metric enters both constructions in the same place, the theorem holds for any positive definite ambient metric, not only the Euclidean one. Treating the parameter domain as flat instead discards the pullback metric and distorts distances.

Planning with the induced metric

Existing motion planning algorithms can use the induced metric without any change to their search strategies.

Sampling-based planning

We plan with anytime planner G-RRT* from our previous work [1], using our matrix-valued lower bound heuristic for informed sampling [2]. Each edge cost is the midpoint approximation from our WAFR paper [3], which evaluates the induced metric once and agrees with the induced distance to third order,

\[ \hat{c}(q_x, q_y) = \big\| \mathcal{R}^{-1}_{q_{\text{mid}}}(q_y) - \mathcal{R}^{-1}_{q_{\text{mid}}}(q_x) \big\|_{q_{\text{mid}}}. \]

Trajectory optimization

The trajectory optimizer represents the path as a B-spline with control points \(P_i\) in the parameter domain, using Drake's KinematicTrajectoryOptimization. It minimizes the induced Riemannian length through a discretized path energy that evaluates the metric at the lifted midpoint of each chord \(\Delta_i = \varphi(P_i) - \varphi(P_{i-1})\),

\[ E(P) = \lambda \sum_{i=1}^{N} \Delta_i^{\mathsf{T}} \, \mat{G}_{\varphi(\bar{P}_i)} \, \Delta_i. \]

Bimanual book transfers

Two Franka arms hold a book and move it between the bottom (B), middle (M) and top (T) compartments of a shelf, which gives six problems. The explicit representation uses the analytic inverse kinematics parameterization of Cohn et al. [4], whose differential is available in closed form [5], and we compare three metrics on its parameter domain.

flat parameter domain treated as flat Euclidean Euclidean metric of the configuration space, pulled back to the parameter domain kinetic-energy kinetic-energy metric of the robot, pulled back to the parameter domain

Execution on the real bimanual setup

We seed trajectory optimization with a Euclidean-metric plan from G-RRT* [1] and time-parameterize each motion with TOPP-RA [6]. The left arm runs under joint position control and the right arm under compliance control.

Bottom to top0.0s

Real · front view

Bars show the median over five trials and fill as each motion plays. White outlines mark the lowest path length and the lowest kinetic energy.

The Euclidean metric gives the shortest joint-space paths and the fastest plans, while the kinetic-energy metric gives the lowest integrated kinetic energy on every task, with plans that take 17% to 30% longer than the Euclidean ones.

Show the full table of hardware results

Explore the motions in 3D

Three panels of the two arms in front of the bookcase, each overlaying intermediate configurations of one motion from the bottom compartment to the top one, tinted orange, blue and green from left to right. — Trajectory-optimizer motions for B→T, with the white curve tracing the book's path. The flat metric gives the longest motion (orange), the induced Euclidean metric takes the shortest route (blue), and under the kinetic-energy metric, the heavier joints of both arms barely move (green).

The viewer below replays the planned motions that the arms executed, side by side and timed to match the videos above. Drag to orbit, scroll to zoom, and use the scene tree in the playback bar to show or hide a metric.

Rendered in the browser with viser.

Simulation results

For each problem, we run 50 trials of G-RRT* [1] with a 10 s budget, seed the trajectory optimizer with each solution, and report the median.

Both representations measure the same length

We measure every solution path twice, with the pullback metric and with the induced metric of the implicit representation, and the two lengths match. The eigenvalue ratio of the pullback metric shows that the parameterization is far from an isometry, so treating the parameter domain as flat optimizes the wrong functional.

	Euclidean		kinetic-energy
Planned under	explicit	implicit	explicit	implicit
flat	28.80	28.80	17.70	17.70
Euclidean	27.25	27.25	18.01	18.01
kinetic-energy	32.93	32.93	15.77	15.77
Total	88.97	88.97	51.49	51.49
Eigenvalue ratio	56		5.2 × 10²

Induced length summed over the six problems, with totals computed before rounding. Bold marks the shortest path under each measure, and the last row gives the median eigenvalue ratio over 10,000 sampled configurations.

Pulling the metric back shortens the physical motion

In the explicit representation, the Euclidean and kinetic-energy variants of both solvers give the shortest motion under their own metric on every problem, ahead of the flat metric.

A matching initial guess speeds up the optimizer

In each column, the optimizer converges fastest from the guess planned under the same metric, and the flat guess is the slowest under both induced metrics. The optimized motions are the same from every guess.

	Time (s) per objective
Initial guess	flat	Euclidean	kinetic-energy
flat	9.2	55.2	92.8
Euclidean	9.9	41.8	80.5
kinetic-energy	10.9	44.3	78.8

Median solve time over 50 trials. Rows name the metric of the initial guess and columns the metric that the optimizer minimizes.

Citation

@misc{kyaw2026induced,
  title         = {Induced Riemannian Metrics for Motion Planning with Constraints},
  author        = {Kyaw, Phone Thiha and Cohn, Thomas and
                   Rogel Garcia, Miguel Angel and Kelly, Jonathan},
  year          = {2026},
  eprint        = {2609.25695},
  archivePrefix = {arXiv},
  primaryClass  = {cs.RO},
  url           = {https://arxiv.org/abs/2609.25695}
}

Acknowledgements

This work was supported in part by the Canada Research Chairs Program, the Natural Sciences and Engineering Research Council of Canada (NSERC) through Discovery Grant no. RGPIN-2023-05036, the National Science Foundation Graduate Research Fellowship Program under Grant No. 2141064, and the Secretaría de Ciencia, Humanidades, Tecnología e Innovación (SECIHTI), Mexico, under CVU number 1066015. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the National Science Foundation.

References

P. T. Kyaw, A. V. Le, R. E. Mohan, and J. Kelly. Greedy heuristics for sampling-based motion planning in high-dimensional state spaces. 2024. arXiv:2405.03411
P. T. Kyaw and J. Kelly. Direct informed sampling on Riemannian manifolds via Loewner order lower bounds. 2026. arXiv:2606.02879
P. T. Kyaw and J. Kelly. Geometry-aware sampling-based motion planning on Riemannian manifolds. In Proceedings of the 17th World Symposium on the Algorithmic Foundations of Robotics (WAFR), 2026. arXiv:2602.00992
T. Cohn, S. Shaw, M. Simchowitz, and R. Tedrake. Constrained bimanual planning with analytic inverse kinematics. 2024. arXiv:2309.08770
T. Cohn, S. Shaw, H. Biggie, T. Manderson, N. Roy, and R. Tedrake. Planning along differentiable charts of constraint manifolds with general-purpose IK solvers. 2026. arXiv:2609.10905
H. Pham and Q.-C. Pham. A new approach to time-optimal path parameterization based on reachability analysis. IEEE Transactions on Robotics, 34(3), 2018. arXiv:1707.07239