motion planning

This work addresses the suboptimality of motion plans in robotics that arises from neglecting the non-Euclidean geometric structure of configuration spaces. To this end, we propose a sampling-based planning framework that operates directly on Riemannian manifolds. Our method efficiently approximates Riemannian geodesic distances using a third-order accurate midpoint scheme and, for the first time, integrates Riemannian natural gradients with first-order retraction operations into local path generation. This approach preserves geometric fidelity while ensuring scalability to high-dimensional systems. Experimental results demonstrate that our planner consistently produces trajectories with significantly lower cost—measured under the kinetic energy metric—than both Euclidean planners and conventional numerical geodesic solvers across diverse platforms, including a planar two-link arm, a 7-DoF Franka manipulator, and an SE(2) nonholonomic system.

This work addresses the issue of excessively tortuous trajectories commonly produced by existing feedback motion planners based on simplicial complex decomposition, which lead to slow motion and high energy consumption. The authors propose a novel approach that constructs a safe, goal-directed “funnel” region by heuristically aligning local vector fields and building a goal-centric maximal star-shaped simplicial chain. By integrating systematic vector field alignment with geometric star-chain construction, the method significantly enhances path smoothness and control efficiency while preserving formal safety guarantees. Experimental results demonstrate a 91.40% average reduction in total path curvature and a 45.47% decrease in LQR control energy cost. In low-dimensional configuration spaces, the planner outperforms sampling- and optimization-based alternatives in both computational efficiency and robustness.

Nonlinear parameterizations (e.g., Euler angles, rational kinematics) in Graph-Convex Set (GCS) trajectory optimization induce metric distortion in configuration space, degrading trajectory quality and violating geometric fidelity. Method: We propose the first rigorous GCS optimization framework supporting nonconvex objective functions. Our approach introduces a “de-distortion” mechanism that integrates Lagrangian duality with certified collision-free region verification, thereby recovering the true configuration-space metric while preserving original constraint feasibility and theoretical guarantees. Contribution/Results: This work establishes the first tight, verifiable optimization support for nonconvex objectives within the GCS paradigm. Experiments across three canonical robotics scenarios—bimanual manipulation, 3D rotational planning, and rational-kinematic modeling—demonstrate significant reductions in path length and execution time, with only marginal increases in computational overhead.

This work addresses the challenge of motion planning in tightly fitted rigid-body assembly, where near-zero clearance contact imposes strong geometric constraints. To tackle this problem, we propose CMG-RRT, an algorithm that, for the first time, incorporates the critical manifold—the set of contact configurations between parts—as a guiding mechanism within a sampling-based planner. By adaptively biasing sampling toward neighborhoods of this manifold in configuration space and integrating a hierarchical subdivision strategy within the RRT framework, CMG-RRT efficiently explores feasible assembly paths. The algorithm achieves a 100% success rate on a challenging rotational assembly benchmark and, notably, provides the first fully automatic solution to the Elk interlocking puzzle. The implementation is publicly available.

To address the slow generation and low reliability of convex sets in configuration space for real-time robotic motion planning under dynamic environments, this paper proposes the first GPU-accelerated online probabilistic collision-free convex decomposition method—Safe Convex Sets (SCS). Our approach enables efficient iterative refinement of SCS sequences via parallelized configuration-space inflation, joint SCS optimization, trajectory-guided collision-feedback pruning, and Dynamic Random Map (DRM) search. Furthermore, we integrate piecewise-linear path inflation with nonlinear trajectory optimization subject to convex-set constraints to support perception-closed-loop online planning. Evaluated on standard simulation benchmarks, our method achieves a 17.1× speedup over CPU-based baselines and improves collision-free success rate by 27.9%. Real-world experiments on a KUKA iiwa 7 robot demonstrate millisecond-level response times and high robustness in dynamic settings.

This work addresses the challenge of self-collisions and environmental collisions in teleoperation, which commonly arise when controlling only the end-effector of a robotic arm. The authors propose a real-time trajectory planning method based on differentiable constraints, introducing—for the first time—a differentiable collision-avoidance constraint derived from convex optimization duality theory into the teleoperation framework. The approach models robot links using capsule primitives and represents environmental obstacles as polyhedra, generating smooth, collision-free trajectories by optimizing the operator’s input. Experiments on both simulation and a physical UR5e platform demonstrate that the method achieves high obstacle modeling fidelity while significantly reducing computational overhead, all within real-time performance bounds, thereby enabling efficient and safe teleoperation.

This work addresses the limitations of traditional task-space planners such as Bug2, which often neglect joint limits and fail to reach targets when the Jacobian becomes ill-conditioned, leading to joint limit violations or unreachable goals. To overcome these issues, the authors propose an adaptive step-size planning method that leverages a second-order inverse kinematics approximation combined with an S-procedure to construct, at each step, a Cartesian hyper-rectangle reachable under joint constraints. This construction is formulated as a semidefinite program and solved efficiently via a novel fast bisection algorithm exploiting quadratic structure. For the first time, verifiable reachability analysis is integrated into real-time task-space planning. Evaluated on 94 adversarial scenarios, the approach achieves zero joint limit violations and a 100% target-reaching success rate, substantially outperforming Bug2.

This work addresses the challenge of robotic trajectory planning in cluttered, narrow environments, where feasible regions are severely constrained and fragmented. The authors propose a function-space-based, geometry-aware trajectory optimization framework that introduces natural functional gradients to this domain for the first time. By employing a Gaussian-smoothed surrogate objective, the method enables discretization-free smoothness control over trajectories. It eliminates the need for analytical gradients, decouples time parameterization, and effectively handles non-differentiable operations—such as collision checking—through Monte Carlo estimation combined with black-box evaluations. Experimental results demonstrate that the approach substantially outperforms existing baselines in complex geometric scenarios, achieving significant improvements in both trajectory feasibility and smoothness.

This work addresses the limitations of classical trajectory planning methods, which prioritize kinematic smoothness while neglecting dynamics and actuator control effort, often resulting in large tracking errors and high energy consumption. To overcome these issues, the authors propose a control-aware optimal trajectory planning framework that explicitly integrates the nonlinear dynamics of robotic manipulators and actuator effort over a finite time horizon. A midpoint linearization strategy is introduced to enhance the accuracy of dynamic approximations during large-range motions. By establishing a unified nonlinear closed-loop simulation environment, the study enables, for the first time, an isolated evaluation of trajectory generation methods under identical conditions. Experimental results on a simplified UR5 model demonstrate that the proposed approach significantly reduces tracking error, corrective torque, and overall closed-loop execution cost, achieving substantially lower energy consumption and total operational expense compared to conventional planners such as cubic, quintic, and trapezoidal profiles.

This work addresses the challenge of nonlinear optimization with mixed equality and inequality constraints in robotic dynamics planning by introducing a novel approach based on “constraint manifolds with corners.” The method reformulates the original problem as an unconstrained optimization over a constrained state space, seamlessly embedding inequality constraints into the manifold structure through differential geometry and manifold optimization techniques. This formulation overcomes the conventional limitation of manifold optimization, which typically applies only to smooth equality constraints. Evaluated on large-scale dynamic planning tasks, the proposed approach successfully generates dynamically feasible trajectories and demonstrates superior robustness and solvability in scenarios where standard algorithms fail.