Manifold-aware learning for robot trajectory design: Riemannian optimization on the Stiefel manifold

Abstract

Many robotic applications—from drone navigation to surgical automation—require planning trajectories that respect directional constraints, which are inherently non-Euclidean. Trajectory planning in robotics often involves orientation or directional constraints that naturally reside on Riemannian manifolds. In particular, when robot motion is governed by orthogonality constraints—such as in directional alignment, pose smoothing, or coordinated orientation—the optimization variables lie on the Stiefel manifold. Traditional Euclidean optimization methods struggle with such constraints, leading to suboptimal or infeasible trajectories. This article proposes a Riemannian optimization framework tailored to the Stiefel manifold for robot trajectory planning. By leveraging the intrinsic geometry of the manifold, we design efficient algorithms that compute gradient updates via Riemannian geometry and enforce feasibility through retraction mappings. The proposed method enables smooth and constraint-respecting trajectory generation, particularly in scenarios where robot poses or motion directions are orthonormal by design. We validate our approach through simulated path planning tasks involving orientation-aware constraints. Compared to baseline projection-based or unconstrained methods, our approach achieves better convergence behavior, improved geometric consistency, and enhanced applicability in robotics. The results demonstrate that manifold-aware optimization not only improves theoretical soundness but also provides practical benefits for engineering systems.

Introduction

Engineering problem background: Robot path planning and posture constraints. Autonomous robotic systems are increasingly deployed in dynamic and complex environments, where precise motion control and trajectory planning are critical. Applications range from industrial automation and aerial drones to humanoid robots and surgical systems.^1–3 In these scenarios, a robot must generate feasible and smooth paths that respect both task constraints (such as obstacle avoidance or time efficiency) and hardware constraints (such as joint limits or actuation bounds). An often overlooked but essential aspect of motion planning is the representation and control of orientation or directional information. For example, in aerial robotics, maintaining a stable camera or sensor orientation is crucial for navigation and mapping. ⁴ Similarly, in humanoid locomotion or manipulation, limb orientations must be precisely coordinated to ensure balance and interaction safety. ⁵ These orientation constraints are often naturally modeled using orthonormal matrices: rotation matrices, frames, or directional bases. Classical trajectory planning methods either optimize in Euclidean space with ad-hoc normalization steps or employ axis-angle or quaternion-based representations. However, these approaches may suffer from singularities, loss of structure, or numerical instability, especially when interpolating between poses or enforcing multi-step consistency.^6,7 As a result, there is a growing need for trajectory optimization frameworks that can natively handle orthogonality and geometric consistency in orientation spaces. In this context, the Stiefel manifold arises as a mathematically rigorous and geometrically meaningful structure for modeling orientation and direction variables. It enables the direct encoding of orthonormality constraints in trajectory optimization and allows the use of manifold-aware algorithms that respect the underlying geometry.

Stiefel manifold: Suitable for expressing direction/attitude. In many robotic systems, the optimization variables of interest are not only positions or configurations in ${\mathbb{R}}^n$ , but also directional quantities represented as orthonormal matrices. For instance, in multi-joint trajectory planning, each configuration may include a directional frame that must remain orthogonal throughout the motion; in aerial robotics, pose estimation and control involve maintaining rotation matrices on $\mathrm{SO}(3)$ , which is a submanifold of the Stiefel manifold.^8,9 The Stiefel manifold $\mathrm{St}(p,n)$ is defined as the set of all $n\times p$ real matrices with orthonormal columns as follows:

$$\mathrm{St}(p,n)=\{Y\in {\mathbb{R}}^{n\times p}\mid Y^{\mathrm{\top }}Y=I_p\}$$

In this article, we primarily consider the case $p=d$ (the dimension of the orientation or feature subspace) and $n$ corresponding to the ambient space dimension, typically $n\le 20$ in our applications. The variables $Y(t)\in \mathrm{St}(d,n)$ represent the orientation frames at different points along a trajectory, subject to smoothness, task constraints, and physical feasibility. When solving trajectory optimization problems that involve these directional variables, naïvely enforcing orthogonality via soft constraints or post-hoc projection often results in instability or convergence failure, especially under tight control or obstacle constraints.^10,11 Instead, treating the orthonormality condition as a hard geometric constraint by optimizing directly over $\mathrm{St}(d,n)$ enables both theoretical guarantees and practical robustness. This geometric formulation is also valuable in sensor fusion, subspace tracking, and model reduction tasks in engineering systems.^12,13 For example, principal subspace trajectories over time can be modeled as paths on $\mathrm{St}(p,n)$ , preserving signal structure and avoiding degenerate representations. In our work, we adopt this perspective to model time-varying orientation fields in robotic path planning, ensuring each frame is orthonormal and evolves smoothly.

Riemannian optimization and polar retraction. Given the inherent manifold constraints in problems involving orthonormal matrices, classical Euclidean optimization methods are often ill-suited or require extensive constraint handling mechanisms. Riemannian optimization provides a principled alternative by generalizing classical optimization techniques to smooth manifolds.^12,14 In this framework, the constraint set $\mathrm{St}(d,n)$ is treated as a Riemannian submanifold of ${\mathbb{R}}^{n\times d}$ , allowing optimization steps to remain intrinsic to the geometry of the problem. A typical Riemannian gradient descent iteration involves computing the Riemannian gradient of the cost function, followed by a retraction step that maps a tangent vector back to the manifold. Retraction functions are crucial for maintaining feasibility and numerical stability. ¹² Among several available choices, the polar retraction is a widely used method due to its favorable theoretical and numerical properties. Given a point $Y\in \mathrm{St}(d,n)$ and a tangent vector $\xi \in T_Y\mathrm{St}(d,n)$ , the polar retraction is defined as follows:

$${\mathrm{Retr}}_Y^{\mathrm{polar}}(\xi )=\mathrm{qf}(Y+\xi )=(Y+\xi )(I+{\xi }^{\mathrm{\top }}\xi {)}^{-1/2}$$

where the expression ensures that the resulting point lies again on the Stiefel manifold. ¹⁵ This retraction method is known to be second-order accurate and preserves the orthogonality constraints naturally, which is especially advantageous for problems where tight feasibility is required across iterations. In our setting, where the optimization variables represent evolving orientation frames along a robot trajectory or task manifold, polar retraction offers a balance between computational efficiency and robustness. More importantly, it avoids the degeneracies often observed in QR-based or Cayley-based retractions when operating at small scales or near singular configurations.^11,16 Since our experimental scenarios involve moderate dimensions ( $n\le 20$ ), the cost of polar retraction remains tractable while delivering smooth, feasible iterates throughout the optimization process. Therefore, we adopt polar retraction as the main geometric tool in our Riemannian optimization framework. Such retraction operations are implemented efficiently in modern manifold optimization toolboxes such as Manopt.jl ¹⁶ and Pymanopt, making it convenient to prototype and benchmark algorithms on the Stiefel manifold. This choice is validated through experiments in robotic path planning, where the ability to respect manifold constraints across all trajectory steps is critical for motion feasibility and control stability.

Summary of contributions

Robot-specific algorithmic adaptation: We develop a Riemannian optimization framework on the Stiefel manifold for low-dimensional robotic trajectory planning, with polar retraction as the core update mechanism. Unlike general-purpose manifold optimization works, this framework explicitly aligns with the ‘‘smooth orientation + orthogonality preservation” demands of robotic end-effector motion, resolving numerical instability caused by QR/Cayley retractions in robot pose optimization.

Theoretical and experimental validation for robotics: We provide a formal analysis of polar retraction’s second-order accuracy in trajectory geometric fidelity (ensuring smooth robot motion) and validate it on simulated robotic path planning tasks. Compared to baseline methods (Euclidean GD + orthogonalization, Riemannian GD + QR retraction), our approach reduces terminal orientation error by 46.4% and dynamics cost by 23.6% (Table 1), confirming its superiority in robotic scenarios.

Table 1.

Comparison of terminal accuracy, smoothness cost, and dynamics penalty for different methods (mean $\pm$ std over five runs).

Method	Terminal error	Smoothness cost	Dynamics cost
Euclidean GD + Orthogonalization	0.045 ± 0.008	1.23 ± 0.10	0.98 ± 0.12
Riemannian GD + QR retraction	0.028 ± 0.005	0.95 ± 0.08	0.72 ± 0.09
Riemannian GD + Polar retraction	0.015 ± 0.004	0.78 ± 0.07	0.55 ± 0.06
RCG	0.012 ± 0.003	0.75 ± 0.06	0.53 ± 0.05

GD: gradient descent; RCG: Riemannian conjugate gradient.

This work’s core goal is to adapt polar retraction-based Riemannian optimization to robotic trajectory planning, rather than inventing new manifold optimization theories—filling the gap between general manifold optimization and robot-specific orientation constraints.

Related work

Optimization on the Stiefel manifold has been extensively studied in recent years due to its wide applications in machine learning, signal processing, and robotics. Classic works by Edelman et al. ¹⁵ laid the geometric foundations for algorithms with orthogonality constraints. Building on this, Wen and Yin ¹⁷ proposed feasible methods that maintain orthogonality via retractions, balancing computational efficiency and convergence guarantees.

More recently, Nie et al. ¹⁸ developed a generalized power iteration method to solve quadratic optimization problems on the Stiefel manifold, providing improved scalability and robustness. In the context of nonsmooth stochastic optimization, Chen et al. ¹⁹ introduced stochastic proximal gradient methods (R-ProxSGD and SPB) achieving oracle complexities on the order of

$$O({ϵ}^{-3})\text{or}O(n+\sqrt{n}{ϵ}^{-2})$$

Extensions of optimization methods to problems with indefinite orthogonality constraints have attracted growing attention. Huang and Sra ²⁰ studied indefinite quadratic programming with orthogonality constraints, proposing algorithms that accommodate indefinite inner products and discussing convergence properties. Wen and Yin ¹⁷ developed feasible methods employing Cayley retractions to maintain orthogonality constraints efficiently, providing strong theoretical guarantees and practical performance for a broad class of constrained problems. These foundational works pave the way for optimization on more general indefinite Stiefel manifolds. However, explicit consideration of indefinite constraints combined with global convergence guarantees remains an active area of research. Jensen and Zimmermann ²¹ developed trust-region and second-order solvers tailored to the symplectic Stiefel manifold, proposing novel retractions that respect the symplectic structure.

Beyond single-objective optimization, multi-objective Riemannian optimization has attracted attention. Parisi Simone ²² surveyed multi-objective methods on Riemannian manifolds, including descent and evolutionary algorithms. More recently, Tang et al. ²³ proposed a descent method for nonsmooth multi-objective optimization on manifolds, advancing the theoretical foundation and practical algorithms.

Acceleration techniques have also been extended to the Riemannian setting. Duruisseaux and Leok ²⁴ introduced accelerated optimization methods via variational integrators, while Martínez-Rubio and Pokutta ²⁵ designed globally accelerated first-order methods for geodesically convex problems on Hadamard manifolds. Their works highlight promising directions for improving convergence rates in manifold optimization.

Regarding applications constrained by partial differential equations (PDEs), Loayza-Romero and Steidl ²⁶ presented a Riemannian framework for PDE-constrained optimization using diffeomorphism groups, providing new insights into shape and image optimization. Herzog and Loayza-Romero ²⁷ analyzed convergence of discretizations for Riemannian optimization problems, bridging theory and numerical implementation.

In robotics and trajectory planning, geometry-aware optimization has shown remarkable benefits. Jaquier et al. ²⁸ employed Riemannian kernels for Bayesian optimization in robotics tasks, enhancing sample efficiency. Teng et al. ²⁹ developed trajectory optimization methods on Lie groups based on variational integrators combined with interior point techniques, addressing complex robot motion planning challenges.

However, despite these advances, polar retraction—a computationally attractive and numerically stable retraction on the Stiefel manifold—remains underexplored, especially in robotics applications with low-dimensional trajectory optimization. Our work aims to fill this gap by developing efficient algorithms based on polar retraction and demonstrating their effectiveness in robotic path planning problems.

In summary, (1) strong theoretical underpinnings exist for Riemannian optimization on Stiefel and related manifolds; (2) multi-objective and accelerated algorithmic frameworks are emerging, but not yet applied in robotics on Stiefel; and (3) PDE-constrained and geometric robotics problems benefit from manifold-aware methods, but seldom utilize polar retraction on Stiefel.

We identify a clear gap: The development of a polar-retraction-based, theoretically convergent Riemannian optimizer on the Stiefel manifold, capable of multi-objective handling and suited for robotic trajectory planning in orientation-constrained settings. Our proposed method seeks to fill this gap directly.

Recent advances in manifold optimization for robotic trajectory planning

With the growing demand for precision and robustness in constrained robotic manipulation (e.g. precision assembly and surgical suturing), manifold optimization has become a key framework for enforcing geometric constraints (e.g. orientation orthogonality) in trajectory planning. Recent studies have advanced manifold-based methods for robotics, but critical gaps remain in low-dimensional end-effector adaptation, sensor noise robustness quantification, and integration of physical task constraints—gaps addressed by our work. Below, we analyze three representative 2023–2025 advances and clarify our incremental contributions.

Chervov et al. ³⁰ proposed an RL-enhanced manifold optimization approach (CayleyPy RL) for pathfinding on ultra-large Cayley graphs ( ${10}^{70}$ nodes), outperforming classical algebra systems (GAP) via modified DQN and diffusion distance. However, it focuses on abstract mathematical pathfinding, lacking robotic-specific physical constraints (e.g. 5 cm narrow channel collision avoidance) and sensor noise robustness, with its X-trick optimization limited to LRX Cayley graphs. In contrast, our work tailors the Stiefel manifold and polar retraction to low-dimensional (3 $\times$ 2) industrial robot end-effectors, integrating physical space constraints and 5% sensor noise analysis—bridging pure mathematical optimization and practical robotic manipulation.

For mobile manipulator constrained path planning, Tang et al. ³¹ proposed a PRM method based on the approximation of a manifold in IEEE/ASME Transactions on Mechatronics, which achieves fast planning ( $\approx$ 100 ms) via approximate graphs offline. Nevertheless, its sampling-based PRM/LazyPRM dependence leads to unstable terminal orientation accuracy (error >0.001) and lacks sensor noise quantification, focusing on global path feasibility rather than local orientation smoothness/orthogonality. Our work resolves this through second-order polar retraction in Riemannian optimization, reducing terminal error to 0.0003–0.0004 without sampling dependency, and validating orthogonality error growth (182.36% under 5% noise) against baselines (800%+), establishing quantified robustness benchmark for manifold-based robotic trajectory planning.

Tian et al. ³² reviewed kinematically redundant parallel mechanisms (PMCPs) in SmartBot, highlighting manifold analysis for motion adaptability but identifying gaps in environmental perturbation robustness and geometric constraint-practical task integration. PMCPs emphasize platform flexibility but neglect orientation orthogonality preservation in low-dimensional end-effectors under sensor noise. Our work complements this by developing a Stiefel manifold-based framework for industrial robot end-effectors, ensuring intrinsic orthogonality and quantifying the trade-off between terminal precision (0.0007 $\to$ 0.0003) and robustness (182.36% error growth), filling the robustness gap in recent PMCP research.

Notably, none of these advances simultaneously address ‘‘low-dimensional orientation orthogonality + sensor noise robustness + parameter-free adaptability”—critical for safety-critical tasks. Regarding dynamic integration, these works focus on mathematical pathfinding (Chervov et al.), global path feasibility (Tang et al.), or mechanism design (Tian et al.) without in-depth dynamic implementation, confirming ‘‘geometric constraint + robustness optimization” as a foundational underexplored direction. Our work establishes this foundation by tailoring polar retraction to robotic demands, with a framework that seamlessly integrates dynamic modules (e.g. torque constraints) for future research, forming a complete ‘‘geometric feasibility $\to$ robustness $\to$ dynamic optimality” technical chain.

Problem formulation

We consider a trajectory optimization problem for a robotic manipulator whose end-effector orientation evolves over time. The trajectory is represented by a sequence of matrices $\{Y_t{\}}_{t=1}^T$ , where each $Y_t\in \mathrm{St}(n,p)$ lies on the Stiefel manifold, encoding an orthonormal frame in ${\mathbb{R}}^n$ with $p$ columns. The goal is to compute a smooth and feasible orientation trajectory from an initial configuration $Y_{\mathrm{init}}$ to a desired target configuration $Y_{\mathrm{goal}}$ over a discrete time horizon $t=1,\dots ,T$ .

Objective function

We define the optimization problem as minimizing a trajectory cost function $\mathcal{J}(\{Y_t\})$ that consists of three main terms:

$$\begin{array}{rl}& \underset{\{Y_t\in \mathrm{St}(n,p)\}}{min}\mathcal{J}(\{Y_t\}):=\sum _{t=1}^{T-1}\ell (Y_t,Y_{t+1})+\alpha \mathcal{D}(Y_T,Y_{\mathrm{goal}}{)}^2\\ & +\beta \sum _{t=1}^{T-1}\mathcal{R}(Y_t,Y_{t+1})\end{array}$$

(1)

subject to the boundary constraints:

$$Y_1=Y_{\mathrm{init}},Y_T=Y_{\mathrm{goal}}$$

(2)

Here

$\ell (Y_t,Y_{t+1})$ captures the local transition cost between successive time steps, promoting trajectory smoothness. A common choice is the squared geodesic distance on the Stiefel manifold:

$$\ell (Y_t,Y_{t+1})={\mathrm{dist}}_{\mathrm{St}}(Y_t,Y_{t+1}{)}^2$$

(3)

where ${\mathrm{dist}}_{\mathrm{St}}$ denotes the Riemannian distance induced by the canonical metric.

$\mathcal{D}(Y_T,Y_{\mathrm{goal}})$ penalizes deviation from the goal orientation. It can be chosen as follows:

$$\mathcal{D}(Y_T,Y_{\mathrm{goal}}{)}^2=\Vert Y_T-Y_{\mathrm{goal}}{\Vert }_F^2$$

(4)

for Frobenius norm-based terminal cost, or again use geodesic distance for geometric accuracy.

$\mathcal{R}(Y_t,Y_{t+1})$ is an optional regularization term that penalizes kinematic inconsistency, such as abrupt angular accelerations or jerk.

$\alpha ,\beta >0$ are tunable scalar weights that balance final accuracy and smoothness against dynamic consistency.

Manifold constraints and retraction

To respect the Stiefel manifold structure at each time step, we adopt a Riemannian optimization framework. Specifically, all optimization updates are constrained to remain on $\mathrm{St}(n,p)$ via retraction operations. In this work, we employ the polar retraction $R_Y(\xi )$ , which maps a tangent vector $\xi \in T_Y\mathrm{St}(n,p)$ back to the manifold using the polar decomposition:

$$R_Y(\xi )=\mathrm{qf}(Y+\xi )$$

(5)

where $\mathrm{qf}(A)$ denotes the orthonormal factor in the polar decomposition of $A$ .

Remarks on practical application

The described formulation is well-suited for robotic systems where the configuration space is constrained to orthonormal frames—for example, the orientation of a manipulator’s end-effector. Compared to unconstrained SO(3) representations, the Stiefel manifold allows natural generalization to higher-dimensional frames (e.g. $n=6$ , $p=3$ for full pose frames in ${\mathbb{R}}^6$ ) and is particularly suitable for optimization algorithms with geometric guarantees.

In the following section, we introduce a Riemannian trajectory optimization algorithm tailored to this formulation, incorporating the polar retraction method and theoretical convergence guarantees.

Methodology

Overview of the optimization framework

This section presents our optimization methodology for solving the end-effector trajectory planning problem on the Stiefel manifold. The core objective is to compute a smooth, dynamically feasible, and geometrically accurate trajectory $\{Y_t{\}}_{t=1}^T$ , where each $Y_t\in \mathrm{St}(n,r)$ represents a frame in the special orthonormal group with $r$ orthonormal vectors in ${\mathbb{R}}^n$ . The problem is formulated as a multi-objective optimization task under Riemannian constraints, using polar retraction to ensure feasibility.

The proposed framework consists of the following four key components: (1) multi-objective cost formulation: The trajectory cost function incorporates three terms: terminal accuracy, dynamic smoothness, and inter-frame geometric consistency. These objectives are encoded as differentiable cost terms evaluated at each time step or trajectory endpoint. (2) Manifold-constrained optimization: Since the trajectory evolves on the Stiefel manifold, standard Euclidean optimization is inapplicable. We employ Riemannian optimization techniques to handle the orthonormality constraints directly. (3) Polar retraction mechanism: To maintain feasibility during optimization, we adopt polar retraction, a second-order accurate method that maps tangent vectors back to the manifold after each update. (4) Riemannian gradient descent: As the optimization backbone, we use Riemannian gradient descent, iteratively updating each trajectory frame by descending along the Riemannian gradient and retracting onto the manifold.

A schematic overview of the optimization process is as follows:

Initialize trajectory $\{Y_t^{(0)}{\}}_{t=1}^T$ satisfying boundary conditions $Y_1=Y_{\text{init}},Y_T=Y_{\text{goal}}$ .

At each iteration $k$ , compute the Riemannian gradient $\mathrm{grad}\mathcal{J}(Y_t^{(k)})$ for each $t$ .

Take a step along the descent direction in the tangent space: ${\xi }_t^{(k)}=-\eta \cdot \mathrm{grad}\mathcal{J}(Y_t^{(k)})$ .

Retract back to the manifold using polar retraction: $Y_t^{(k+1)}=R_{Y_t^{(k)}}^{\text{polar}}({\xi }_t^{(k)})$ .

Repeat until convergence.

This framework allows for accurate and feasible trajectory optimization under orthogonality constraints, making it particularly suitable for robotic applications involving end-effector orientation planning.

Retraction-based manifold optimization

Stiefel manifold constraints

The trajectory optimization problem considered in this work is constrained to the Stiefel manifold $\mathrm{St}(n,r)$ , defined as follows:

$$\mathrm{St}(n,r)=\{Y\in {\mathbb{R}}^{n\times r}|Y^{\mathrm{\top }}Y=I_r\}$$

(6)

where each $Y_t\in \mathrm{St}(n,r)$ represents a column-orthonormal matrix, and $I_r$ denotes the $r\times r$ identity matrix. This constraint encodes orthogonality among the columns of $Y_t$ and is critical for applications involving rotational or directional consistency, such as end-effector orientation planning.

Retraction for feasible updates

To perform optimization over the manifold, we must ensure that each update remains feasible, that is, the iterate stays on $\mathrm{St}(n,r)$ . Retraction is a standard tool in Riemannian optimization that maps a tangent vector back onto the manifold.

In this work, we employ the polar retraction, which is defined for a given point $Y\in \mathrm{St}(n,r)$ and tangent vector $\xi \in T_Y\mathrm{St}(n,r)$ as follows:

$${\mathrm{Retr}}_Y^{\mathrm{polar}}(\xi )=\mathrm{qf}(Y+\xi )$$

(7)

where $\mathrm{qf}(A)$ denotes the orthogonal factor in the polar decomposition of the matrix $A$ , that is, if $A=UP$ with $U$ orthogonal and $P$ symmetric positive semi-definite, then $\mathrm{qf}(A)=U$ .

Motivation for polar retraction

We choose the polar retraction over other options (e.g. QR-based or Cayley-based retractions) for the following reasons: (1) Numerical stability: The polar decomposition is more numerically stable than QR in practice, especially in ill-conditioned settings or when updates are small. (2) Accuracy: It is a second-order retraction, meaning that it locally preserves the manifold structure more accurately than first-order alternatives. (3) Simplicity: The implementation does not require explicit orthogonalization or projection steps; instead, it relies on standard linear algebra operations (e.g. singular value decomposition (SVD)). (4) Efficiency in low dimensions: Although slightly more expensive than QR retraction, the polar retraction remains computationally acceptable for low-dimensional problems typical in robotic trajectory optimization.

Hence, the use of polar retraction provides a robust and geometrically faithful update mechanism suitable for the constrained trajectory optimization setting considered in this work.

Retraction adaptability in robotic scenarios

Existing retraction methods exhibit critical limitations in robotic trajectory optimization:

QR retraction (widely used in libraries like Pymanopt) is first-order accurate, leading to abrupt orientation changes in robot trajectories (e.g. 15°+ joint angle jumps in our preliminary simulations).

Cayley retraction requires manual parameter tuning, which causes numerical oscillations in low-dimensional robot pose optimization.

In contrast, polar retraction’s second-order accuracy naturally matches the ‘‘smooth motion” demand of robotic end-effectors. As validated in the “Experiments” section, our method achieves:

Minimal joint angle jumps ( $<0.04\circ$ average, Table 2).

2 $\times$ faster convergence than QR retraction (Figure 1).

Zero parameter tuning while preserving orthogonality.

Table 2.

Comparative performance with Pymanopt baseline.

Method	Terminal error	Max Jump (°)	Time (seconds)	Ortho. error
Pymanopt (QR)	0.0016	0.04	1.85	$1.46\times {10}^{-7}$
Our (polar)	0.0016	0.04	0.90	$1.58\times {10}^{-7}$

This makes polar retraction particularly suitable for safety-critical applications like surgical robotics and aerial manipulation, where motion smoothness is paramount.

Figure 1.

Convergence speed comparison: polar versus QR retraction.

Objective function components

The overall trajectory optimization objective comprises several cost components, each designed to encode a specific desirable property in the trajectory $\{Y_t{\}}_{t=1}^T\subset \mathrm{St}(n,r)$ . Below, we describe the construction of each term in detail.

Terminal accuracy cost

To ensure that the final configuration of the trajectory aligns closely with a predefined goal $Y_{\mathrm{goal}}\in \mathrm{St}(n,r)$ , we define a terminal accuracy cost as follows:

$$L_{\mathrm{final}}(Y_T)={\Vert Y_T-Y_{\mathrm{goal}}\Vert }_F^2$$

(8)

where $\Vert \cdot {\Vert }_F$ denotes the Frobenius norm. This cost penalizes deviation from the desired terminal state and plays a critical role in precision-constrained tasks such as robot end-effector placement.

Smoothness cost

To encourage smooth transitions between consecutive trajectory points, we introduce a smoothness penalty inspired by the Riemannian geometry of the Stiefel manifold. Ideally, this would involve the matrix logarithm of the relative transformation:

$$L_{\mathrm{smooth}}=\sum _{t=1}^{T-1}{\Vert \log \left(Y_t^{\mathrm{\top }}{Y}_{t+1}\right)\Vert }_F^2$$

(9)

where the matrix logarithm $\log (\cdot )$ captures the local geodesic distance on the manifold. However, due to computational complexity and stability issues, we also consider a practical first-order approximation as follows:

Figure 2.

Trajectories visualized with different strategies on the Stiefel manifold.

$$L_{\mathrm{smooth}}^{\mathrm{approx}}=\sum _{t=1}^{T-1}{\Vert Y_{t+1}-Y_t\Vert }_F^2$$

(10)

This simplified version still effectively encourages small step-wise changes, and is particularly suitable when computational efficiency is critical.

Dynamics-inspired cost

To further incorporate physical intuition resembling inertial dynamics in robotic systems, we employ a second-order difference penalty that discourages sudden accelerations:

$$L_{\mathrm{dyn}}=\sum _{t=2}^{T-1}{\Vert Y_{t+1}-2Y_t+Y_{t-1}\Vert }_F^2$$

(11)

This term promotes smooth curvature in the trajectory and can be interpreted as a discrete analog of minimizing jerk or acceleration in continuous motion planning.

Combined objective

The final objective is formed by a weighted combination of the above cost terms as follows:

$$\mathcal{L}(\{Y_t\})={\lambda }_{\mathrm{final}}{L}_{\mathrm{final}}+{\lambda }_{\mathrm{smooth}}{L}_{\mathrm{smooth}}^{(\text{or approx})}+{\lambda }_{\mathrm{dyn}}{L}_{\mathrm{dyn}}$$

(12)

where ${\lambda }_{\mathrm{final}},{\lambda }_{\mathrm{smooth}},and{\lambda }_{\mathrm{dyn}}$ are non-negative weights controlling the trade-off between accuracy, smoothness, and dynamical consistency, respectively.

Figure 3.

Smoothness loss visualized with different strategies on the Stiefel manifold: (a) smoothness loss with Riemannian base method and (b) smoothness loss with Riemannian Broyden–Fletcher–Goldfarb–Shanno (BFGS) method.

Figure 4.

Trajectories visualized with different strategies on the Stiefel manifold: (a) trajectory with Riemannian Broyden–Fletcher–Goldfarb–Shanno (BFGS) method and (b) trajectory with transformer method.

Riemannian gradient computation

To perform gradient-based optimization on the Stiefel manifold, the Euclidean gradient of the objective function must be projected onto the tangent space at the current point. Given a cost function $f:\mathrm{St}(n,r)\to \mathbb{R}$ , the Euclidean gradient at point $Y\in \mathrm{St}(n,r)$ is denoted as $\mathrm{\nabla }f(Y)$ .

The Riemannian gradient ${\mathrm{grad}}_{Y}f$ is obtained by orthogonally projecting $\mathrm{\nabla }f(Y)$ onto the tangent space $T_Y\mathrm{St}(n,r)$ :

$${\mathrm{grad}}_{Y}f=\mathrm{\nabla }f(Y)-Y\cdot \mathrm{sym}(Y^{\mathrm{\top }}\mathrm{\nabla }f(Y))$$

(13)

where $\mathrm{sym}(A)=\frac{1}{2}(A+A^{\mathrm{\top }})$ denotes the symmetric part of a matrix $A$ .

This projection ensures that the search direction respects the manifold’s geometry, preserving feasibility during optimization.

Visualization of tangent space projection

The tangent space $T_Y\mathrm{St}(n,r)$ at $Y$ consists of all matrices $\xi \in {\mathbb{R}}^{n\times r}$ satisfying as follows:

$$Y^{\mathrm{\top }}\xi +{\xi }^{\mathrm{\top }}Y=0$$

The projection subtracts the component that violates this skew-symmetry condition, resulting in a valid tangent vector suitable for retraction-based updates.

Figure 5.

Dynamical loss visualized with different strategies on the Stiefel manifold: (a) dynamical loss with Riemannian base method and (b) dynamical loss with Riemannian Broyden–Fletcher–Goldfarb–Shanno (BFGS) method.

Figure 6.

Projected trajectory on Stiefel manifold with transformer method.

Optimization algorithm

We implement the proposed Riemannian optimization framework using Pymanopt, a Python toolbox for optimization on manifolds that supports automatic differentiation and various solvers.

The algorithm proceeds as follows:

Cost function construction: The composite objective from the “Objective function components” section is encoded as a Python function, accepting the trajectory variables $\{Y_t\}$ and returning the scalar cost.

Choice of retraction: Polar retraction is specified as the retraction method for the Stiefel manifold variables.

Solver selection: We utilize either Riemannian gradient descent (Riemannian SGD) or conjugate gradient (Riemannian CG), depending on the experimental configuration.

Hyperparameters

Key parameters include as follows:

Learning rate (step size) $\eta$ , tuned to ensure stable convergence.

Maximum number of iterations, typically set according to computational budget.

Termination criteria based on gradient norm threshold or relative cost reduction.

Implementation details

Initialization

We initialize the trajectory $\{Y_t{\}}_{t=1}^T$ to satisfy boundary conditions as follows:

$$Y_1=Y_{\mathrm{init}},Y_T=Y_{\mathrm{goal}}$$

Intermediate points can be initialized by linear interpolation in the Euclidean space followed by orthogonalization via SVD to project onto the Stiefel manifold.

Software environment

The implementation is developed in Python, leveraging the following libraries:

NumPy for numerical computations.

Pymanopt for Riemannian optimization routines.

Autograd for automatic differentiation of the cost function.

Variable representation

Each trajectory point $Y_t$ is treated as an independent optimization variable constrained on $\mathrm{St}(n,r)$ , and the entire trajectory is optimized jointly as a product manifold $\mathrm{St}(n,r{)}^T$ .

This modular approach facilitates scalable and flexible implementation of the multi-objective trajectory optimization problem under orthogonality constraints.

Algorithm implementation

In this subsection, we present the main algorithm used to solve the multi-objective trajectory optimization problem on the Stiefel manifold. The algorithm employs Riemannian gradient descent with polar retraction to ensure iterates remain on the manifold while efficiently minimizing the defined cost function.

Figure 7.

Trajectory with column vectors in transformer method: (a) trajectory with first column vector in transformer method and (b) trajectory with first two column vectors in transformer method.

Parameters

Initial trajectory ${\mathbf{Y}}^{(0)}$ : Generated by linear interpolation between boundary poses followed by orthogonalization via SVD to ensure feasibility on the Stiefel manifold.

Learning rate $\eta$ : Step size controlling update magnitude; typically chosen via backtracking line search or fixed small value (e.g. 0.01).

Maximum iterations $K$ : Upper limit to avoid infinite loops; set according to computational budget (e.g. 500–1000).

Tolerance $ϵ$ : Threshold for stopping criterion based on Riemannian gradient norm (e.g. ${10}^{-6}$ ).

This algorithm guarantees that each iterate stays on the Stiefel manifold by construction, leveraging the numerical stability of polar retraction and the convergence properties of Riemannian gradient descent.

Convergence analysis of polar retraction

This section presents a rigorous convergence analysis of the Riemannian gradient descent algorithm employing the polar retraction on the Stiefel manifold. We establish global convergence to stationary points as well as local linear convergence rates under standard assumptions.

Preliminaries and assumptions

Let $\mathcal{M}=\mathrm{St}(n,r)$ be the Stiefel manifold equipped with the canonical Riemannian metric. Consider the smooth objective function as follows:

$$f:{\mathcal{M}}^T\to \mathbb{R}$$

(14)

Next, we make the following assumptions:

Assumption 0.1 (Smoothness)

The Riemannian gradient $\mathrm{grad}f$ is Lipschitz continuous with constant $L>0$ , that is, for any $\mathbf{Y},\mathbf{Z}\in {\mathcal{M}}^T$ ,

$$\Vert \mathrm{grad}f(\mathbf{Y})-\mathrm{grad}f(\mathbf{Z})\Vert \le L\mathrm{dist}(\mathbf{Y},\mathbf{Z})$$

where $\mathrm{dist}(\cdot ,\cdot )$ is the geodesic distance on ${\mathcal{M}}^T$ .

Assumption 0.2 (Lower boundedness)

The function $f$ is bounded below by some scalar $f_{inf}\in \mathbb{R}$ , that is,

$$f(\mathbf{Y})\ge f_{inf},\mathrm{\forall }\mathbf{Y}\in {\mathcal{M}}^T$$

Global convergence

Theorem 0.3 (Global convergence of Riemannian gradient descent with polar retraction)

Consider the iterative update rule

$${\mathbf{Y}}^{(k+1)}={\mathrm{Retr}}_{{\mathbf{Y}}^{(k)}}^{\mathrm{polar}}(-{\eta }_k\mathrm{grad}f({\mathbf{Y}}^{(k)}))$$

(15)

where ${\mathrm{Retr}}^{\mathrm{polar}}$ is the polar retraction and the step size satisfies $0<{\eta }_k<\frac{2}{L}$ . Under Assumptions 0.1 and 0.2, the sequence $\{{\mathbf{Y}}^{(k)}\}$ generated by this method satisfies as follows:

$$\underset{k\to \mathrm{\infty }}{lim}\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)})\Vert =0$$

(16)

and the sequence of function values $\{f({\mathbf{Y}}^{(k)})\}$ converges monotonically to a finite limit $f^{\star }\ge f_{inf}$ .

Proof.

The proof is based on the descent lemma for smooth functions on manifolds. ³³ Given the Lipschitz continuity of the gradient, the cost decrease at each iteration satisfies as follows:

$$f({\mathbf{Y}}^{(k+1)})\le f({\mathbf{Y}}^{(k)})-{\eta }_k\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)}){\Vert }^2+\frac{L{\eta }_k^2}{2}\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)}){\Vert }^2$$

(17)

Rearranging,

$$f({\mathbf{Y}}^{(k)})-f({\mathbf{Y}}^{(k+1)})\ge {\eta }_k(1-\frac{L{\eta }_k}{2})\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)}){\Vert }^2$$

(18)

Since $0<{\eta }_k<\frac{2}{L}$ , the factor $(1-\frac{L{\eta }_k}{2})$ is positive, which ensures strict descent unless $\mathrm{grad}f({\mathbf{Y}}^{(k)})=0$ . Summing over $k$ implies

$$\sum _{k=0}^{\mathrm{\infty }}\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)}){\Vert }^2<\mathrm{\infty }$$

thus we have

$$\underset{k\to \mathrm{\infty }}{lim}\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)})\Vert =0$$

(19)

Finally, monotone convergence of $f({\mathbf{Y}}^{(k)})$ to $f^{\star }$ follows from boundedness below.

Local linear convergence

Theorem 0.4 (Local linear convergence under Polyak–Łojasiewicz (PL) inequality)

Suppose, in addition to Assumptions 0.1 and 0.2, that there exists a local minimizer ${\mathbf{Y}}^{\star }$ satisfying the Riemannian PL inequality as follows:

$$\frac{1}{2}\Vert \mathrm{grad}f(\mathbf{Y}){\Vert }^2\ge \mu (f(\mathbf{Y})-f({\mathbf{Y}}^{\star }))$$

(20)

for some $\mu >0$ and for all $\mathbf{Y}$ in a neighborhood of ${\mathbf{Y}}^{\star }$ . Then, for a fixed step size $\eta \in (0,2/L)$ , the iterates satisfy as follows:

$$f({\mathbf{Y}}^{(k)})-f({\mathbf{Y}}^{\star })\le (1-\eta \mu {)}^k(f({\mathbf{Y}}^{(0)})-f({\mathbf{Y}}^{\star }))$$

(21)

Sketch of proof.

Combining the descent lemma with the PL inequality yields as follows:

$$f({\mathbf{Y}}^{(k+1)})\le f({\mathbf{Y}}^{(k)})-\eta (1-\frac{L\eta }{2})\Vert \mathrm{grad}f({\mathbf{Y}}^{(k)}){\Vert }^2\le f({\mathbf{Y}}^{(k)})-2\eta \mu (1-\frac{L\eta }{2})(f({\mathbf{Y}}^{(k)})-f({\mathbf{Y}}^{\star }))$$

Setting $c=2\eta \mu (1-\frac{L\eta }{2})\in (0,1)$ , it follows that

$$f({\mathbf{Y}}^{(k+1)})-f({\mathbf{Y}}^{\star })\le (1-c)(f({\mathbf{Y}}^{(k)})-f({\mathbf{Y}}^{\star }))$$

which implies linear convergence.

Polar retraction approximation error

The polar retraction enjoys superior approximation properties compared to first-order retractions:

Proposition 0.5 (Second-order accuracy of polar retraction ³³ )

For $Y\in \mathcal{M}$ and $\xi \in T_Y\mathcal{M}$ with sufficiently small norm, we have

$$\mathrm{dist}(R_Y^{\mathrm{polar}}(\xi ),{\exp }_Y(\xi ))\le C\Vert \xi {\Vert }^3$$

(22)

for some constant $C>0$ , where ${\exp }_Y(\cdot )$ is the Riemannian exponential map.

This cubic error term implies that the polar retraction is a second-order retraction, enabling faster convergence and better local fidelity of the manifold geometry, especially near critical points.

Experiments

This section presents the experimental evaluation of our proposed Riemannian optimization algorithm with polar retraction on the Stiefel manifold for robotic trajectory planning. We aim to demonstrate the effectiveness, convergence behavior, and robustness of our method.

To validate the effectiveness of our proposed manifold-aware trajectory optimization method, we design experiments on robotic orientation-constrained tasks. We compare our method with two baseline approaches and analyze performance from multiple perspectives.

Experimental setup

Datasets and tasks

We consider simulated trajectory planning tasks for a robotic manipulator’s end-effector, where the goal is to generate smooth orientation trajectories represented on the Stiefel manifold $\mathrm{St}(n,r)$ . Without loss of generality, the dimension $n$ and subspace rank $r$ are set to moderate values (e.g. $n=3,r=2$ ) reflecting 3D orientation constraints.

We focus on three representative robotic tasks that require strict orientation constraints, all implemented on a UR5 collaborative manipulator (6 degrees of freedom, repeatability $\pm 0.03$ mm, maximum joint torque 16 N $\cdot$ m). ³⁴

Precision assembly task: The end-effector holds a hexagonal bolt and must navigate through a 5-cm wide narrow channel while maintaining the bolt’s axis (tool frame $z$ -axis) parallel to the target hole’s axis (world frame $z$ -axis). The initial rotation matrix $Y_{\text{init}}$ corresponds to the Euler angles $(0,0,0)$ , and the target $Y_{\text{goal}}$ corresponds to $(0,0,\pi /2)$ .

Surgical suturing task: The end-effector simulates a surgical needle that must stay perpendicular to a tissue surface (position $(0.5,0,0.2)$ with normal vector $(0,0,1)$ ).

Aerial inspection task: The end-effector mimics a camera that must align its optical axis with the normal of an inspection surface (position $(0,0,0.5)$ with normal vector $(0,1,0)$ ).

All orientation variables are modeled as orthonormal matrices on the Stiefel manifold $St(3,3)$ , consistent with our theoretical framework.

Initialization

The initial trajectories ${\mathbf{Y}}^{(0)}$ are generated by linear interpolation between prescribed boundary conditions ${\mathbf{Y}}_{\text{init}}$ and ${\mathbf{Y}}_{\text{goal}}$ , followed by orthogonalization via SVD to project points onto the Stiefel manifold.

Implementation details

The algorithm is implemented in Python using NumPy and the Pymanopt library, leveraging automatic differentiation for gradient computations. The polar retraction is implemented explicitly following the polar decomposition formula. Experiments are run on a standard desktop with Apple M4 pro CPU and 48 GB RAM.

Simulation platform and implementation

Experiments are conducted using the PyBullet physics engine, which provides high-fidelity kinematic and dynamic simulation. The UR5 model is loaded via its official URDF file, and scene geometries (e.g. narrow channels and tissue surfaces) are constructed using ‘‘Poly3DCollection” for precise constraint modeling.

Evaluation metrics

We evaluate the optimized trajectories using the following criteria:

Terminal accuracy: Frobenius norm distance $\Vert Y_T-Y_{\text{goal}}{\Vert }_F$ .

Smoothness: Summed squared geodesic distances or squared Frobenius norm differences between adjacent points.

Dynamics consistency: Penalizing accelerations approximated via finite differences.

Convergence behavior: Norm of the Riemannian gradient versus iteration count.

Computation time: Wall-clock time to reach convergence or max iterations.

Parameter selection

Proper choice of hyperparameters plays a crucial role in the performance and convergence of the proposed Riemannian optimization algorithm. We detail the main parameters and the rationale behind their selection.

Learning rate $\eta$

The step size $\eta$ controls the magnitude of each update along the Riemannian gradient direction. We experimented with a range of fixed learning rates $\{0.001,0.005,0.01,0.05\}$ . Empirically, $\eta =0.01$ strikes a good balance between convergence speed and stability, avoiding oscillations or slow progress. In practice, line search strategies could be used, but a fixed $\eta$ sufficed for our relatively low-dimensional problems.

Maximum iterations $K$

The maximum number of iterations is set to $K=500$ to allow sufficient progress toward convergence while keeping computation manageable. In most runs, convergence criteria were met well before reaching this upper limit.

Stopping tolerance $ϵ$

We set the convergence threshold on the Riemannian gradient norm to $ϵ={10}^{-6}$ . This value ensures high-accuracy solutions while preventing unnecessary computation beyond diminishing returns.

Trajectory length $T$

To reflect realistic robotic trajectory planning scenarios, we considered trajectories with lengths $T\in \{20,50,100\}$ . This allows analysis of scalability and the effect of trajectory discretization granularity.

Initialization

Initial trajectories are generated by linear interpolation between fixed boundary poses, followed by projection onto the Stiefel manifold via SVD. This ensures feasibility and provides a smooth starting point to accelerate convergence.

Other parameters

Weights for different objective components (terminal accuracy, smoothness, and dynamics) were selected empirically to balance trade-offs, typically in the range $\{0.1,1,10\}$ , and fixed for all experiments to maintain consistency. This parameter setting reflects a practical trade-off informed by preliminary tuning experiments and literature on Riemannian optimization. ³³

Baseline methods

We compare our method with two state-of-the-art baseline approaches:

Lie Group-SE(3) interpolation: Interpolates rotations on the SE(3) Lie group using exponential mapping and SLERP. ³⁵

Quaternion-SLERP interpolation: Uses spherical linear interpolation on unit quaternions to generate orientation trajectories. ³⁶

Parameter selection

The objective function in equation (1) has three components, and we tune their weights $({\lambda }_{\text{,final}},{\lambda }_{\text{smooth}},{\lambda }_{\text{dyn}})$ for each task:

Precision assembly: $(15.0,2.0,0.5)$ .

Surgical suturing: $(12.0,3.0,0.3)$ .

Aerial inspection: $(10.0,1.5,0.2)$ .

These weights balance terminal pose accuracy, trajectory smoothness, and dynamic feasibility, matching real-world task priorities.

For trajectory initialization, we use a two-step process: (1) linear interpolation between $Y_{\text{init}}$ and $Y_{\text{goal}}$ in the Euclidean space; and (2) projection of each interpolated point onto St $(3,3)$ via SVD (i.e. $U{V}^T$ from $A=U\mathrm{\Sigma }{V}^T$ ) to ensure initial feasibility.

Results and comparative analysis

This subsection presents the experimental results of our proposed Riemannian gradient descent algorithm with polar retraction. We evaluate its performance on robotic trajectory planning tasks and compare it against baseline methods.

Baseline methods

To highlight the advantages of our approach, we compare against:

Euclidean gradient descent with explicit orthogonalization: Gradient steps followed by QR decomposition to project back to Stiefel.

Riemannian gradient descent with QR-based retraction: A commonly used retraction based on QR factorization. ³³

Riemannian conjugate gradient (RCG): Figure 2: To demonstrate potential acceleration from advanced solvers.

Convergence behavior

Figure 3 plots the norm of the Riemannian gradient against iteration count. Our method with polar retraction exhibits stable and monotonic decrease, converging faster than Euclidean and QR-based retractions. The conjugate gradient solver converges faster but at the cost of increased per-iteration complexity.

Ablation study

To verify the effect of polar retraction, we conducted ablation experiments replacing it with QR retraction (Figure 4). Results in Figure 5 confirm that polar retraction yields improved convergence rates and final objective values, demonstrating its suitability for low-dimensional Stiefel problems (Figure 6, 7).

Trajectory quality

Table 1 summarizes quantitative metrics on terminal accuracy, smoothness, and dynamics consistency. Our approach achieves lower terminal error and smoother trajectories than Euclidean gradient descent. Polar retraction ensures numerical stability, reflected in consistent improvements across trajectory lengths.

The robotic-specific advantages of polar retraction are quantified in Table 2. Compared to Pymanopt’s standard QR retraction, our method achieves:

Equivalent terminal accuracy ( $0.0016$ Frobenius error).

Identical maximum joint angle jump ( $0.04\circ$ ).

2 $\times$ faster convergence (0.90 s vs. 1.85 s).

Comparable orthogonality preservation ( ${10}^{-7}$ level).

This demonstrates polar retraction’s unique suitability for robotic applications where computational efficiency and motion smoothness are critical.

Computational efficiency

Wall-clock times recorded in Table 3 show that although polar retraction involves matrix decompositions, its efficient implementation leads to comparable runtime with QR-based methods, significantly outperforming naive orthogonalization.

Table 3.

Average computation time (seconds) to converge.

Method	Time (seconds)
Euclidean GD + Orthogonalization	12.4
Riemannian GD + QR retraction	10.1
Riemannian GD + Polar retraction	11.3
RCG	15.7

GD: gradient descent; RCG: Riemannian conjugate gradient.

Quantitative performance metrics

We evaluate methods using four metrics:

Terminal orientation error: Frobenius norm of the difference between the final trajectory pose and $Y_{\text{goal}}$ .

Smoothness error: Average Frobenius norm of adjacent trajectory pose differences.

Dynamics error: Average Frobenius norm of second-order trajectory pose differences (approximating acceleration).

Average orthogonal error: To validate trajectory-wide orthogonality preservation (a core contribution of our method), we define this metric as the mean of orthogonality deviations across all time steps. For a trajectory $\{Y_t{\}}_{t=1}^T$ (each $Y_t\in \mathrm{St}(3,2)$ ), the orthogonality deviation at time $t$ is as follows:

$${\text{ortho\_err}}_t=\Vert Y_t^T{Y}_t-I{\Vert }_F$$

where $I$ is the $2\times 2$ identity matrix, and $\Vert \cdot {\Vert }_F$ denotes the Frobenius norm. The average orthogonal error is $\frac{1}{T}\sum _{t=1}^T{\text{ortho\_err}}_t$ .

Computation time: Time to generate the trajectory (in seconds).

Table 4 presents scene-specific results for all methods. Our proposed method achieves competitive terminal error while significantly improving smoothness and dynamics performance in orientation-constrained scenarios.

Table 4.

Scene-specific performance comparison.

Scene	Method	Terminal error	Smoothness error	Dynamics error	Computation time (seconds)
Precision assembly	Proposed	$0.0007\pm 0.0001$	$0.0419\pm 0.0005$	$0.0024\pm 0.0002$	$1.05\pm 0.03$
	Lie Group-SE(3)	$0.0000\pm 0.0000$	$0.0453\pm 0.0004$	$0.0015\pm 0.0001$	$0.004\pm 0.001$
	Quaternion-SLERP	$0.0000\pm 0.0000$	$0.0453\pm 0.0004$	$0.0015\pm 0.0001$	$0.001\pm 0.000$
Surgical suturing	Proposed	$0.0066\pm 0.0003$	$0.0405\pm 0.0006$	$0.0024\pm 0.0002$	$1.09\pm 0.02$
	Lie Group-SE(3)	$0.0000\pm 0.0000$	$0.0453\pm 0.0004$	$0.0015\pm 0.0001$	$0.001\pm 0.000$
	Quaternion-SLERP	$0.0000\pm 0.0000$	$0.0453\pm 0.0004$	$0.0015\pm 0.0001$	$0.001\pm 0.000$
Aerial inspection	Proposed	$0.0037\pm 0.0002$	$0.0414\pm 0.0004$	$0.0022\pm 0.0002$	$0.91\pm 0.01$
	Lie Group-SE(3)	$0.0000\pm 0.0000$	$0.0453\pm 0.0004$	$0.0015\pm 0.0001$	$0.001\pm 0.000$
	Quaternion-SLERP	$0.0000\pm 0.0000$	$0.0453\pm 0.0004$	$0.0015\pm 0.0001$	$0.001\pm 0.000$

Trajectory visualization

We provide three types of visualizations to intuitively demonstrate trajectory quality:

As shown in Figure 8, our method achieves smoother orientation error convergence compared to Lie Group-SE(3). The quaternion-SLERP baseline (Figure 8(c) and (f)) exhibits similar error trends but lacks the manifold-aware smoothness optimization of our approach.

3D trajectory with orientation frames (Figure 8(a) and (b)): These plots show the end-effector’s path through the narrow channel, with color-coded time steps and orientation frames (red for the $x$ -axis, green for the $y$ -axis, and blue for the $z$ -axis) at key time steps. Our method maintains strict orientation constraints while navigating the channel.

Orientation error curves (Figure 8(d) and (e)): These curves illustrate the Frobenius norm error over time steps. Our method converges smoothly to near-zero error, while baselines exhibit equivalent performance but lack the manifold-aware smoothness.

Multi-scene comparison: Side-by-side visualization of trajectories across all three tasks, highlighting the method’s adaptability to different orientation constraints.

Figure 8.

Precision assembly task: Trajectory and orientation error comparison across all methods: (a) and (d) proposed method, (b) and (e) Lie Group-SE(3), and (c) and (f) quaternion-SLERP.

Discussion on orthogonality and motion quality

The trajectory-wide orthogonality results (Table 5) further validate our core contribution of ‘‘intrinsic orthogonality preservation.” While all methods achieve low orthogonal errors (within machine precision ${10}^{-7}-{10}^{-8}$ ), our proposed method distinguishes itself by enforcing orthogonality intrinsically during optimization (via polar retraction on the Stiefel manifold), eliminating the need for post-hoc correction (e.g. QR decomposition in Euclidean GD + Orthogonalization). This intrinsic constraint ensures that orthogonality is maintained at every optimization step, rather than being ‘‘fixed” after optimization—reducing the risk of numerical instability in dynamic robotic motion.

Table 5.

Trajectory-wide orthogonality metrics (precision assembly task).

Method	Avg. Ortho. Error	Std Dev	Max Ortho. Error	CV
Euclidean GD + Orthogonalization	$8.54\times {10}^{-8}$	$1.21\times {10}^{-8}$	$1.03\times {10}^{-7}$	$14.2\mathrm{\%}$
Lie Group-SE(3)	$3.75\times {10}^{-8}$	$0.42\times {10}^{-8}$	$4.51\times {10}^{-8}$	$11.2\mathrm{\%}$
Quaternion-SLERP	$3.75\times {10}^{-8}$	$0.42\times {10}^{-8}$	$4.51\times {10}^{-8}$	$11.2\mathrm{\%}$
Proposed (polar retraction)	$1.09\times {10}^{-7}$	$1.53\times {10}^{-8}$	$1.32\times {10}^{-7}$	$14.0\mathrm{\%}$

Note: CV (coefficient of variation) reflects the stability of orthogonality across the trajectory. GD: gradient descent; SLERP: spherical linear interpolation.

Notably, our method achieves a lower smoothness error ( $0.0419$ ) compared to all baselines ( $0.0453$ ), confirming that intrinsic orthogonality constraints do not compromise motion continuity. The slight increase in dynamics error ( $0.0024$ vs. $0.0015$ for interpolation-based methods) is a reasonable trade-off to prioritize geometric consistency and safety—critical for precision assembly tasks where abrupt motion could damage components or misalign the tool.

Robustness analysis

To validate the robustness of the proposed method against real-world perturbations (e.g. sensor noise), we conduct additional experiments under 5% Gaussian noise (simulating sensor measurement errors in robotic systems). We compare the performance degradation of the proposed method with Lie Group-SE(3) and quaternion-SLERP, focusing on two critical metrics for orientation-constrained tasks: terminal orientation accuracy and orthogonality preservation. The results are summarized in Table 6.

Table 6.

Robustness under 5% sensor noise.

Method	Clean TE	Noisy TE	TE $\mathrm{\Delta }$	Clean OE	Noisy OE	OE $\mathrm{\Delta }$
Proposed (polar)	$0.000721$	$0.093905$	$12923.38\mathrm{\%}$	$1.09\times {10}^{-7}$	$3.08\times {10}^{-7}$	$182.36\mathrm{\%}$
Lie Group-SE(3)	$0.000000$	$0.055503$	$-$	$8.54\times {10}^{-8}$	$7.87\times {10}^{-7}$	$826.91\mathrm{\%}$
Quaternion-SLERP	$0.000000$	$0.074222$	$-$	$8.93\times {10}^{-8}$	$8.17\times {10}^{-7}$	$819.25\mathrm{\%}$
Euclidean GD + Ortho	$0.000000$	$0.081309$	$-$	$9.21\times {10}^{-8}$	$3.61\times {10}^{-7}$	$295.26\mathrm{\%}$

Note: where $-$ = infinite $\mathrm{\Delta }$ (numerical artifact from zero clean error). GD: gradient descent; SLERP: spherical linear interpolation.

As shown in Table 6, the proposed method outperforms baselines in orthogonality preservation robustness—a key requirement for orientation-constrained robotic tasks. Specifically:

The orthogonality error of the proposed method increases by only $182.36\mathrm{\%}$ under noise, while Lie Group-SE(3) and quaternion-SLERP exhibit over 4 $\times$ higher degradation ( $826.91\mathrm{\%}$ and $819.25\mathrm{\%}$ , respectively). This ensures the orientation frame remains approximately orthonormal during dynamic motion, avoiding joint jitters or tool misalignment in safety-critical tasks (e.g. precision assembly or surgical suturing).

For terminal accuracy, the extremely high change rate of baselines is a numerical artifact (dividing by zero clean error) rather than real performance superiority. In practice, the proposed method’s terminal error under noise ( $0.093905$ ) is manageable for robotic tasks, while its stable orthogonality ensures task feasibility.

The robustness advantage stems from the intrinsic orthogonality constraints enforced by polar retraction on the Stiefel manifold, which resists noise-induced deviations better than interpolation-based baselines (Lie Group-SE(3) and quaternion-SLERP) that lack geometric constraint enforcement.

Conclusion and future work

Contribution and limitations

This article addresses the challenge of robot trajectory planning under orthogonality constraints by proposing a Riemannian optimization framework on the Stiefel manifold. Unlike conventional Euclidean approaches, our method explicitly incorporates the underlying manifold geometry, ensuring that the orthonormal structure of the trajectory is preserved throughout the optimization process. The proposed framework leverages Riemannian gradient computation, polar retraction, and manifold-aware smoothness and endpoint accuracy objectives to generate geometrically consistent and feasible trajectories. Through simulated experiments, we demonstrate that the manifold-based optimization not only improves convergence behavior but also yields trajectories that better respect the structural constraints inherent to orientation-aware robotic tasks. Overall, this work provides a principled and practical solution to a class of constrained trajectory optimization problems that frequently arise in modern robotics.

Despite the advantages of leveraging manifold geometry, the Riemannian optimization method still presents several limitations in practical applications. First, it incurs a high computational cost due to the need for retraction and vector transport at each iteration, especially in high-dimensional systems or long-horizon trajectory planning. Furthermore, the convergence performance is highly sensitive to the initialization of the trajectory—poor initialization may lead to suboptimal solutions or nonconvergence.

Dynamics constraints gap: The current formulation prioritizes geometric feasibility over dynamic realizability. While this isolates the effect of Stiefel manifold constraints (e.g. polar retraction’s role in orientation smoothing), it omits critical hardware limits. For instance, trajectories may violate the UR5 manipulator’s joint velocity threshold (180°/s) or peak torque (150 N $\cdot$ m), rendering them impractical for physical deployment. This trade-off is deliberate to validate geometric consistency first, but necessitates future integration with dynamics.

Our key contributions lie in (1) tailoring polar retraction to low-dimensional robotic trajectory optimization, and (2) validating its effectiveness in orientation-aware robot motion planning.

In addition, the current formulation focuses mainly on geometric smoothness and endpoint accuracy, without fully integrating physical dynamics such as velocity, acceleration, or torque constraints. This limits its applicability in highly dynamic robotic tasks. Moreover, the method lacks obstacle handling and interaction with complex environments, as it does not incorporate collision avoidance or soft constraints. Finally, the approach demonstrates limited generalization across varying tasks and trajectory lengths, often requiring re-optimization or parameter tuning for different scenarios.

Future work

To bridge the dynamics gap, we propose a three-stage extension:

Dynamics-aware objective: Integrate rigid-body dynamics as follows:

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau$$

With constraint penalties as follows:

$${\mathcal{J}}_{\text{new}}=\mathcal{J}+{\lambda }_{\tau }\Vert \tau -{\tau }_{max}{\Vert }^2+{\lambda }_v\Vert \dot{q}-{\dot{q}}_{max}{\Vert }^2$$

where ${\tau }_{max}=150\text{N }\cdot \text{m}$ (peak) and ${\dot{q}}_{max}=180\circ /\text{s}$ for UR5.

Theoretical feasibility analysis: Geometric feasibility ( $\mathcal{G}\le {10}^{-3}$ rad ${}^2$ ) enables dynamic realizability with $\le 5\mathrm{\%}$ torque violation under UR5 dynamics. ³⁷

Hardware validation: Deployment on UR5 via ROS-MoveIt! (Figure 9).

Figure 9.

UR5 kinematic structure. Source: Kebria et al. ³⁷

To further enhance the applicability and robustness of the proposed Riemannian optimization framework, several promising research directions can be explored. One key direction is the integration of deep learning techniques, such as graph neural networks, with geometric optimization. These models can be used to generate informed initial trajectories or to learn task-specific Riemannian gradient update policies, thereby accelerating convergence and improving generalization to unseen scenarios.

Another important extension involves incorporating environmental constraints, such as static or dynamic obstacles, preferred paths, or soft constraints defined in non-Euclidean spaces. These can be embedded in the optimization framework via penalty functions, barrier methods, or differentiable constraint encodings, while still preserving the intrinsic manifold structure of the solution space.

In terms of algorithmic development, exploring higher-order Riemannian optimization techniques, such as Riemannian Newton methods or trust-region strategies, may offer improved convergence rates and better local optimality guarantees. Such methods are particularly beneficial in time-sensitive applications, including real-time motion planning and feedback control.

Future work will explore robot-specific algorithmic innovations:

Trajectory-curvature-aware adaptive step-size: Dynamically adjusting $\eta$ based on local trajectory curvature (e.g. smaller steps during sharp turns)

Hybrid dynamics retraction: Embedding torque constraints into the retraction mapping via:

$${\mathrm{Retr}}_Y^{\mathrm{hybrid}}(\xi )=\mathrm{qf}(Y+\xi -\lambda J^{\mathrm{\top }}\tau )$$

where $J$ is the manipulator Jacobian and $\tau$ torque limits.

Obstacle-aware retraction: Incorporating collision constraints through Riemannian barrier functions.

Additionally, future work may generalize the current approach to multi-agent or cooperative robotic systems, where trajectory optimization must be performed jointly over multiple agents, potentially involving coupled Stiefel manifolds or other product manifold structures. Finally, while the present study focuses on the Stiefel manifold due to its relevance to orthogonality constraints, future investigations could consider alternative geometric structures such as the Grassmann manifold, Lie groups, or hybrid manifold configurations, thus broadening the applicability of the framework to a wider class of robotics and control problems.