Constrained Motion Planning

Follow this topic Ask about this topic

Momentum

16 papers in the last four weeks, up 167% on the four weeks before. 0.2% of all new papers.

Jul 13Week of Sep 28

Latest papers 126

Jul 4, 2026cs.RO

Fast Asymptotically Optimal Kinodynamic Planning via Vectorization

Sampling-based motion planners have been shown to be effective for systems with complex kinodynamic constraints and high dimensionality. However, these algorithms struggle to achieve real-time performance, leading to recent efforts to parallelize planning. While GPU-accelerated planners have achieved significant speedups, existing approaches require specialized CUDA programming that limits accessibility and portability. We present Parallel Asymptotically Optimal Kinodynamic RRT (PAKR), a massively parallel kinodynamic planner leveraging JAX and the XLA compiler to achieve GPU acceleration through standard Python tooling. By combining our parallel planner with the AO-x meta-algorithm, we achieve asymptotic optimality through fast iterative replanning. We provide a theoretical analysis of probabilistic completeness, analyze the effects of batch size and branching factor on convergence, and demonstrate scalability to complex dynamics using the MuJoCo-XLA simulator. Experiments show competitive runtimes with state-of-the-art GPU planners and superior solution quality.

Yitian Gao, Andrew Lu, Zachary Kingston

Department of Computer Science, Purdue University

Constrained Motion Planning Kinodynamic Motion Planning GPU Acceleration Motion Planning Sampling-Based Motion Planning

Jul 3, 2026cs.RO

Continuous-Time Gaussian Belief Trees for Motion Planning

We address sampling-based motion planning for continuous-time stochastic systems under process and measurement uncertainty, with probabilistic guarantees on safety and performance. The robot dynamics are modeled as a continuous-time linear stochastic differential equation, while sensor measurements arrive at discrete time instants. We derive an offline hybrid belief propagation model in which the belief evolves according to continuous-time ODEs between measurements and undergoes discrete Kalman filter update jumps at measurement times. To ensure safety, we introduce a belief-barrier-function-based safety checker for segment-level probabilistic verification. This enables the planner to certify safety over entire continuous trajectory segments and detect inter-sample chance-constraint violations that are missed by conventional node-based checks. Together, these components provide a principled framework for sampling-based belief planning that accounts for both continuous-time uncertainty propagation and continuous-time safety requirements. We integrate the method with RRT and SST planners and evaluate it across multiple benchmark environments. The results show that the proposed method achieves high success rates and robust enforcement of chance constraints, including in narrow-passage scenarios where discrete-time counterparts fail due to missed inter-sample unsafe behavior.

Rayan Mazouz, Qi Heng Ho, Zachary N. Sunberg +1

Department of Aerospace Engineering Sciences at the University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering at Virginia Tech, Blacksburg, VA, USA

Belief-Space Planning Constrained Motion Planning Safe Motion Planning Motion Planning Sampling-Based Motion Planning

Jul 1, 2026cs.RO

SE(2) Navigation Mesh

Global navigation for ground robots in complex multi-level environments requires representations that accurately capture traversable regions while enabling efficient path planning. Current approaches present key limitations: Point clouds and volumetric occupancy maps lack explicit surface structure for traversability estimation, whereas direct pathfinding on dense triangle meshes is computationally prohibitive. Navigation meshes mitigate these challenges through polygonal abstraction of the underlying mesh, but assume yaw-invariant traversability, rendering them unsuitable for non-circular robots in constrained spaces. We propose SE(2) Navigation Mesh (SE(2) NavMesh), a polygonal representation of traversable regions that encodes yaw-dependent traversability. Our method evaluates traversability using footprint masks and constructs a graph over yaw-specific layers with explicit translational and rotational connectivity. Grounded in this representation, we develop an A*-String Pulling-A* (ASA) pathfinding strategy that hierarchically optimizes robot position and heading. We also present an online method that incrementally updates the SE(2) NavMesh from streaming point clouds during concurrent geometry reconstruction. In simulation, the SE(2) NavMesh captures over 50% more traversable area than classical NavMeshes, and the SE(2) NavMesh + ASA pipeline consistently outperforms sampling-based baselines in constrained environments. Extensive real-world experiments on a physical robot validate real-time online generation and successful navigation across multiple environments.

Shuyang Shi, Kaixian Qu, Changan Chen +3

Robotic Systems Lab, ETH Zurich, Switzerland.

Constrained Motion Planning Robot Navigation Motion Planning Robot Motion Planning

Jul 1, 2026cs.RO

From Prediction Uncertainty to Conformalized Distance Fields for Safe Motion Planning

Safe motion planning in dynamic environments requires reasoning about the uncertainty in predicted obstacle motion without sacrificing real-time performance. Existing conformal approaches conformalize a scalar score that aggregates per-obstacle prediction errors, losing spatial coherence and scaling poorly with scene density. We instead conformalize the entire predicted distance field at once. This functional conformal prediction (FCP) framework yields a distribution-free, field-level lower bound, from which safety follows uniformly: any trajectory satisfying the resulting constraint is certified safe, independent of how the control space is sampled. The key enabler is that the residual distance field is empirically low-rank and approximately time-invariant, which makes the bound decomposable in coefficient space. An envelope is fitted offline via functional PCA and a Gaussian-mixture inductive conformal procedure, then refined online by a lightweight adaptive functional conformal (AFCP) update on a low-dimensional vector. This keeps the per-step cost largely insensitive to obstacle count and retains long-run field coverage under distribution shift. We embed the envelope as a tightened safety constraint in a sampling-based model predictive controller, FCP-MPC. On the ETH--UCY pedestrian benchmarks and a dense 3D quadrotor task with up to 280 dynamic obstacles, FCP-MPC attains a favorable balance of safety, feasibility, and efficiency, reaching goals where pointwise and egocentric conformal baselines become too conservative or too expensive, while keeping per-step computation far below online uncertainty-reasoning baselines.

Jaeuk Shin, Yoonseok Ra, Insoon Yang

Department of Electrical and Computer Engineering, ASRI, Seoul National University, Seoul 08826, South Korea

Constrained Motion Planning Conformal Prediction Model Predictive Control Safe Motion Planning Dynamic Obstacle Avoidance Sampling-Based Motion Planning

Jul 1, 2026cs.RO

Search-Based Spatiotemporal and Multi-Robot Motion Planning on Graphs of Space-Time Convex Sets

Spatiotemporal motion planning, especially in multi-robot settings, requires robots to reason about collision-free regions that change over time, which is challenging in continuous spaces when feasible regions are transient and geometrically constrained. We present an algorithmic framework based on graphs of space-time convex sets (ST-GCSs), where collision-free regions are represented as convex sets in space-time and trajectories correspond to paths on the graph together with continuous motions within the selected sets. We formulate time-optimal planning on ST-GCSs as a graph-search problem over path-indexed states and develop a best-first search solver that evaluates partial paths via continuous trajectory optimization, guided by admissible heuristics and dominance checks. We further present an Exact Convex Decomposition (ECD) scheme to reserve trajectory occupancies in space-time, enabling unified handling of dynamic obstacles and multi-robot interactions. For multi-robot motion planning, we integrate ST-GCS planning and ECD into prioritized planning methods and introduce a windowed coordination scheme to improve efficiency. Extensive experiments on single-robot and multi-robot problems demonstrate substantial speedups over various planners while maintaining high solution quality, particularly in environments with narrow and transient feasible regions. Large-scale demonstrations further show that the proposed multi-robot motion planner can solve instances with up to

100

robots within only a few minutes. Project homepage: https://sites.google.com/view/stgcs

Jingtao Tang, Zining Mao, Lufan Yang +1

Simon Fraser University, Canada

Multi-Robot Motion Planning Constrained Motion Planning Graph-Based Planning Motion Planning

Jul 1, 2026cs.RO

NeHMO: Neural Hamilton-Jacobi Reachability Learning for Decentralized Safe Multi-Arm Motion Planning

Safe multi-arm motion planning is a challenging problem in robotics due to its high dimensionality, coupled configuration space, and complex collision constraints. Centralized planners are capable of coordinating all arms but often face scalability limitations, restricting applicability in real-time settings. On the other hand, decentralized methods are scalable and recent deep learning-based approaches have shown promising results. However, these depend on accurate behavior prediction or coordination protocols and may fail when other arms act unpredictably. To address these challenges, we introduce a neural Hamilton-Jacobi Reachability (HJR) learning-based approach to approximate a safety value function that captures worst-case inter-arm safety constraints. We further develop a decentralized trajectory optimization framework that uses the learned HJR representation for real-time planning. The proposed method is scalable and data-efficient, generalizes across multi-manipulator systems, and outperforms state-of-the-art baselines on challenging multi-arm motion planning tasks.

Qingyi Chen, Zachary Kingston, Ahmed H. Qureshi

Department of Computer Science at Purdue University, West Lafayette, IN, USA

Multi-Robot Motion Planning Constrained Motion Planning Reachability Analysis Multi-Robot Systems Safe Motion Planning Motion Planning+1

Jun 29, 2026cs.RO

Trajectory Optimization for Collision-Aware Redundant Robotic Multi-Axis Additive Manufacturing by Constrained Gradient Projection

Redundant robotic multi-axis additive manufacturing (MAAM) enables support-free and conformal fabrication, but trajectory optimization for long-horizon paths remains challenging under strict deposition-position constraints and time-varying collision constraints. This work proposes a computational framework for collision-aware trajectory optimization in redundant robotic MAAM. We first formulate nozzle-workpiece relative kinematics using a relative Jacobian, and develop a differentiable SDF-based collision model that captures fabrication-induced geometry evolution and provides optimization gradients. The deposition position is then enforced as a hard waypoint-wise equality constraint through iterative projection onto the self-motion manifold, with the loss gradient restricted to the corresponding tangent space. Experiments on an 8-DOF robotic MAAM platform with diverse long-horizon support-free and conformal toolpaths show that our method maintains a mean nozzle-position error below 10μm, reduces maximum joint jerk by up to

77.6\%

, and eliminates all sampled collision and orientation violations. Compared with the SQP-based baseline, it achieves up to a 10.2x speedup and improved convergence. Physical fabrication experiments further verify that the resulting smooth, collision-free trajectories enable successful printing of complex geometries with fewer visible deposition artifacts.

Zhikai Shen, Jiasheng Qu, Chenyu Xu +4

Manufacturing Constrained Motion Planning Trajectory Optimization Trajectory Generation Robotics Gradient Projection+3

Jun 27, 2026cs.RO

A Unified Framework for Multi-Contact Path Planning in the Rolling Robot Systems

Rolling motion planning is challenging because rolling contact imposes nonholonomic constraints and the configuration evolves on a curved manifold. The problem becomes substantially harder in multi-contact settings, where multiple bodies roll without slip and the contact states are coupled. This paper presents a new framework for multi-contact path planning in spherical rolling robotics under no-slip constraints. We first derive a compact kinematic model for multi-sphere rolling using Montana's contact-coordinate formulation, where each contact is represented by a stacked five-state vector. Building on this model, we construct a Voronoi-based roadmap directly on the spherical contact manifold, incorporating spherical-cap obstacles and mutual-exclusion regions via on-manifold collision checking, and refine discrete graph paths using manifold-consistent log-exp smoothing. The resulting smoothed surface paths are then lifted to admissible multi-contact rolling motions through the derived Montana kinematics and validated via forward simulation. We further evaluate feasibility and path quality versus trajectory smoothness, Voronoi seed density, and computation time. The proposed framework provides a foundation for extending the method to non-spherical geometries, time-varying obstacle environments, and experimental validation on physical rolling robotic platforms.

Qing Yu, Mikhail Svinin, Seyed Amir Tafrishi

Geometric Mechanics and Mechatronics in Robotics (gm2R) Lab, School of Engineering, Cardiff University, Queen’s Buildings, The Parade, Cardiff, CF24 3AA · Information Science and Engineering Department, Ritsumeikan University, Shiga, Japan

Constrained Motion Planning Robot Locomotion Motion Planning Robot Motion Planning

Jun 25, 2026cs.RO

BOWConnect: Parallel Bayesian Optimization over Windows with Learned Local Cost Maps for Sample-Efficient Kinodynamic Motion Planning

This paper presents BOWConnect, a bidirectional parallel kinodynamic motion planner that addresses three fundamental limitations of existing sampling-based methods: sample inefficiency in high-dimensional state spaces, unreliable cost heuristics under dynamic constraints, and poor performance in narrow passage environments. Unlike classical planners that rely on random control sampling and geometric distance heuristics, BOWConnect integrates Bayesian Optimization over Windows (BOW) as a learning-based steering function within a parallel tree-based exploration framework, enabling each worker to learn local cost maps and constraints to guide sampling toward dynamically feasible and collision-free controls. A bidirectional architecture simultaneously grows forward and backward trees from the start and goal regions in parallel threads, with a spatial hashing mechanism enabling fast connection queries and a boundary value problem solver generating kinodynamically consistent bridge trajectories. Extensive evaluations across ten benchmark environments demonstrate that BOWConnect achieves 100% success while delivering the fastest or near-fastest planning time in complex scenarios, including narrow passages and non-convex spaces where state-of-the-art planners fail or degrade substantially. Real-world deployment on a ground vehicle and a quadrotor confirms real-time planning with no collisions. Videos of real-world and simulated experiments, high-resolution versions of the figures, and the open-source code are available at https://bow-connect.github.io/.

Sourav Raxit, Abdullah Al Redwan Newaz, Jose Fuentes +1

Department of Computer Science, Louisiana State University New Orleans, New Orleans, LA 70148, USA · Knight Foundation School of Computing and Information Sciences, Florida International University, Miami, FL 33199, USA

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Sampling-Based Motion Planning

Jun 25, 2026cs.RO

RelAfford6D: Relational 6D Affordance Graphs for Constraint-Driven Robotic Manipulation

Bridging abstract semantics and precise physical control remains a fundamental challenge in open-world robotic manipulation. While recent data-driven policies show promise, their reliance on isolated contact points or latent affordance embeddings lacks the rigorous kinematic constraints necessary for complex articulated objects.To overcome the limitation, we introduce RelAfford6D, a novel training-free framework centered on a Relational 6D Affordance Graph. Given a free-form instruction, our system deduces a semantic topology linking a primary interacting part to its physical anchor. By elevating these topological nodes into precise metric

SE(3)

poses via vision foundation models, we analytically formulate downstream execution as a kinematic constraint satisfaction problem. The robot synthesizes continuous trajectories by tracking strictly defined physical manifolds (e.g., revolute or prismatic orbits). Coupled with a closed-loop tracking mechanism for dynamic replanning against disturbances, our physically grounded approach achieves superior zero-shot success rates, cross-category generalization and execution robustness in both simulation and the real world environments, outperforming existing data-driven baselines.

Guodong Zhang, Qichen He, Wenyuan Xie +6

School of Computer Science, Shanghai Jiaotong University, Shanghai, P. R. China

Constrained Motion Planning Zero-Shot Robotic Manipulation Language-Conditioned Robot Manipulation Generalization in Robotic Manipulation Robotic Manipulation Articulated Object Manipulation+1

Jun 24, 2026eess.SY

PRISM: Efficient and Locally Optimal Probabilistic Planning with Reachability Guarantees

Belief-space planning under motion uncertainty and state and control constraints remains a fundamental challenge, largely due to the difficulty of establishing reachability guarantees in constrained belief spaces. Existing constrained belief-space planners rely on sampling to construct multi-query belief roadmaps and explicitly find feasible trajectories between sampled nodes to establish reachability. These methods often struggle to cover the belief space or use robust control techniques that improve coverage at the cost of indirect, high-cost trajectories; they also lack finite-time or finite-memory completeness guarantees. We propose PRISM, a multi-query motion planning algorithm for belief spaces with state and control constraints that targets both high coverage and low cost. We present a new result on controllability of the state covariance under constraints, which is used by PRISM to decompose belief-space planning into deterministic mean planning and covariance shrinking. PRISM further includes an online local optimization method that reduces the cost of feasible belief-space trajectories. Under mild assumptions on the start and goal distributions, we prove that PRISM guarantees full coverage (i.e. completeness) despite actuator and obstacle constraints. In challenging simulated scenarios, PRISM achieves substantially higher roadmap coverage than state-of-the-art belief-space planning methods while producing trajectories with lower mean cost and cost variance. For example, PRISM achieves 100% coverage in easy and medium-difficulty scenarios, and, in the hardest scenario, which violates PRISM's coverage assumptions, it still achieves 97-100% coverage, while all other methods achieve less than 45%.

Alex Rose, Christopher Jewison, Jonathan P. How

Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge MA 02139 · Draper, Cambridge MA 02139

Belief-Space Planning Constrained Motion Planning Trajectory Optimization Reachability Analysis Sampling-Based Motion Planning

Jun 24, 2026cs.AI

Reasonable Motion: A General ASP Foundation for Environment Constrained Movement Trajectory Computation

We present a general answer set programming based hybrid quantitative-qualitative method for computing constrained branching trajectory modes for moving objects in real-world settings. The method performs constrained traversal of an environment graph, enumerating geometrically admissible motion behaviours as stable models, each constituting a distinct trajectory mode characterised by both domain-dependent and independent factors such as derived event sequence, map topology, and domain norms. The hybrid trajectory computation method is generally applicable across motion characteristics typically encountered in diverse dynamic domains with moving objects, e.g., autonomous driving. We demonstrate applicability and highlight how computed trajectories are traceable to their underlying stable model, thereby affording verifiable interpretability that purely learned approaches cannot provide. We also perform an empirical evaluation with Argoverse 2, a large-scale real-world autonomous driving benchmark representative of the class of dynamic domains within the scope of the proposed method.

Julius Monsen, Jakob Suchan, Mehul Bhatt +1

Örebro University, Örebro, Sweden · CoDesign Lab > Cognitive Vision · Constructor University Bremen, Bremen, Germany

Constrained Motion Planning Trajectory Generation Autonomous Driving Answer Set Programming Motion Planning

Jun 24, 2026cs.LG

UC-Search: Risk-Aware Test-Time Search for Delayed Constrained Time-Series Control

Time-series deployments often need delayed feasible decisions, not only accurate forecasts. UC-Search is a trace-only retained-search layer for delayed constrained control: a frozen backbone emits forecasts or action scores, a hard-feasibility automaton rolls paths forward, and bounded search returns the first action of a feasible trajectory. The main claim is conditional: retained lookahead can improve delayed constrained decisions only when delayed feasible-set coupling, retained-prefix premises, and fail-closed release certificates hold. The promoted public endpoint is Phase128 certified M4 expanded40: validation selects Certificate-Constrained Retained Pareto Beam with

λ=0.25

, the held-out test has certificate/risk-active rates

1.0000/0.9642

, and the weakest family remains above the unchanged

0.95

gate at

0.9516

on M4Weekly. The author-defined public

9

-family suite remains an uncertified stress-test boundary. The paper reports a trace-only mechanism, one certified public endpoint, failed-route certificates, and deployment boundaries rather than a universal risk-control theorem.

Xibai Wang

NeuroQuant Labs Limited Hong Kong SAR, China

Constrained Motion Planning Inference-Time Search Model-Based Planning

Jun 21, 2026eess.SY

Active Sensing and Deferred-Decision Trajectory Optimization for Robust Target Identification

We study trajectory optimization in mobile sensing systems that must identify which member of a finite candidate set is the true target, while maintaining reachability to all potential candidate targets, under resource constraints. Deferred-Decision Trajectory Optimization (DDTO) addresses this setting by computing trajectories that reach individual targets but remain coincident for as long as possible before separating toward different targets. We propose Active-Sensing DDTO (AS-DDTO), which extends DDTO by adding a trajectory-dependent information-acquisition term to the planning objective. The resulting planner maintains reachability to candidate targets while biasing the coincident portion of the trajectories toward regions that enable earlier target identification. The framework supports Bayesian updates and conformal candidate-set updates for distance-dependent sensing. We derive a mixed-integer conic reformulation and provide guarantees on recursive feasibility, belief concentration, and fixed-time coverage for the raw conformal candidate set. Numerical simulations show improved target identification compared with standard DDTO under distance-dependent sensing uncertainty and limited sensing budget.

Farbod Siahkali, Mengxue Hou, Vijay Gupta

Department of Electrical and Computer Engineering, Purdue University, West Lafayette, IN 47906, USA · Department of Electrical Engineering, University of Notre Dame, Notre Dame, IN 46556, USA

Constrained Motion Planning Trajectory Optimization Trajectory Generation Informative Path Planning Active Sensing Constrained Optimization

Jun 19, 2026cs.RO

THREAD: Trajectory Planning for Hybrid Rigid-Soft Manipulators with Environment-Aware Diffusion

Manipulation in confined environments, such as threading a manipulator through narrow apertures, remains a fundamental challenge, especially for conventional rigid robots. Hybrid rigid-soft manipulators offer promise but face two compounding planning challenges: backbone shapes feasible in free space become infeasible under environmental contact, and planning rigid and soft segments independently ignores their kinematic coupling. We present THREAD, the first diffusion-based trajectory planner for hybrid manipulation, learning a generative prior over physically realizable backbone trajectories conditioned on local environment geometry, with physics-inspired losses encoding curvature, smoothness, and collision constraints jointly across both segments. Trained in simulation, THREAD achieves 92.4% task success with 5x fewer collisions than the strongest baseline. We show cross-embodiment real-world transfer with minimal online updates, successfully threading through apertures as small as 1.3x the soft segment diameter.

Shivani Kamtikar, Pranav Asthana, Naveen Kumar Uppalapati +2

University of Illinois Urbana-Champaign · University of Maryland College Park

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Robotic Manipulation Robot Motion Planning Soft Robotics+1

Jun 17, 2026cs.RO

Coupled Routing and Configuration Optimization for Multi-Viewpoint Robotic Inspection

We present a unified framework that turns a set of 6-DoF inspection viewpoints into a time-optimal, collision-free route for a 9-DoF robotic system. Unlike modular pipelines that fix a single inverse-kinematics (IK) configuration per viewpoint, build an all-pairs travel-time map, and then route, our method jointly optimizes the visiting order and the per-viewpoint configuration in a single global search. The three-dimensional self-motion manifold of each viewpoint is parameterized in closed form so that the pose constraint holds by construction, the rest-to-rest travel time is approximated by a closed-form admissible double-integrator surrogate, and the tour is encoded by random keys. A derivative-free optimizer (CMA-ES) minimizes a cheap penalized objective over order and configuration, after which direct-collocation trajectory optimization is applied only to the edges of the selected route to certify dynamic feasibility and torque limits, and to return exact timings. This reduces the trajectory solves from quadratic to linear in the number of viewpoints and removes the decoupling that prevents modular pipelines from being globally time-optimal. Simulations and real-robot experiments on a KUKA LBR iiwa with a 2-DoF linear stage validate feasibility, smooth execution, and reduced end-to-end inspection time relative to modular and naive distance-based baselines.

Minh Nhat Vu, Khang Nguyen, Vu Trung Tran +1

Center for Artificial Intelligence Research (CAIR), VinUniversity, Hanoi, Vietnam · VinRobotics JSC, Hanoi, Vietnam · Division of Computer and Mathematical Sciences, Mohamed bin Zayed University of Artificial Intelligence (MBZUAI), Abu Dhabi, UAE.

Constrained Motion Planning Trajectory Optimization Kinodynamic Motion Planning Robotic Inspection Trajectory Generation Robot Task Planning+1

Jun 15, 2026cs.RO

PolyMerge: Compressing 3D Gaussian Splats with Polytope Coverings for Provably Safe Resource-Constrained Navigation

Obstacle avoidance is essential for safe navigation and motion planning. Recent radiance field reconstruction methods enable object detection and modeling with high fidelity, but remain too memory- and compute-intensive for on-board perception-based path planning. To address these limitations, we propose PolyMerge to convert a large, photorealistic 3D Gaussian Splatting (3DGS) model of a scene into a lightweight representation of convex polytopes whose union provably over-approximates all obstacles in the original 3DGS model. PolyMerge tunes the polytope count to trade off conservativeness and compute cost, and integrates with control barrier functions (CBFs) to plan collision-free paths. We showcase PolyMerge in simulation and hardware experiments on a Crazyflie drone, which uses PolyMerge to compute and follow safe trajectories in real time under severe onboard compute constraints, outperforming baselines in speed while guaranteeing safety. For our code and videos, visit https://athlon76.github.io/PolyMerge-website/.

Jihoon Hong, Chih-Yuan Chiu, Sara Fridovich-Keil +1

1Georgia Institute of Technology.

3DGS Compression Constrained Motion Planning Safe Robot Navigation Robot Safety Approximation Algorithms Robot Collision Avoidance

Jun 14, 2026cs.RO

Pixels to Proofs: Probabilistically-Safe Latent World Model Control via Parallel Conformal Robust MPC

We present SLS^2, a framework for safe feedback motion planning from pixels using robust model predictive control (MPC) in learned latent world models. Our approach trains an action-conditioned joint-embedding world model with compact Markovian latent states, enabling efficient gradient-based trajectory optimization through learned latent dynamics. To enforce safety for the true system despite imperfect latent predictions, we inform a GPU-accelerated system level synthesis (SLS) robust MPC scheme with conformal prediction to obtain calibrated latent error bounds and robust latent-space constraint sets. We further learn and conformalize a latent constraint checker, allowing the SLS planner to impose probabilistic safety constraints during closed-loop execution. We evaluate our method on vision-based control tasks, where it improves both goal-reaching performance and safety over latent world-model and safe-planning baselines.

Devesh Nath, Anutam Srinivasan, Haoran Yin +3

Georgia Institute of Technology

Constrained Motion Planning World Model-Based Planning Model Predictive Control Safe Motion Planning Latent World Models Vision-Based Robot Control

Jun 14, 2026cs.RO

Robots as Tokens: Unified Diffusion Transformer for Coordinated Multi-Robot Trajectory Generation

The success of generative models in language and visual generation has inspired extensive applications to generative robot planning. However, most existing works either focus on single-robot planning, or generate multi-robot trajectories in a sequential manner with iterative post-processing to resolve inter-robot conflicts. In this work, we investigate whether coordinated multi-robot trajectories, as a special spatiotemporal distribution, can be learned and generated with a generative model in a feed-forward manner. We propose Robots as Tokens (Roken), a unified diffusion transformer that directly generates multi-robot trajectories that satisfy both (individual) safety and (global) connectivity constraints. The core design of Roken is to represent each robot as a discrete token, allowing them to naturally interact with each other through self-attention, and cross-attend to map tokens for environment layouts. We further introduce several auxiliary tasks based on Bayes' theorem to provide multi-scale spatial-temporal supervision for efficient learning of the conditional distribution. In training, Roken absorbs diverse expert trajectories from different team sizes. During inference, Roken behaves as a versatile multi-robot planner that can handle single-robot planning, coordinated multi-robot trajectory generation, and conditional trajectory generation by fixing some robot tokens as conditions. Experiments in diverse cluttered environments show that Roken can generate coordinated multi-robot trajectories to perform connectivity-constrained goal navigation tasks with high success rates, outperforming the baseline method used to generate the training dataset. Roken also demonstrates good scalability after training with mixed team sizes, and shows generalization to unseen or partially observed environments, verifying its potential to learn from diverse data and perform versatile tasks.

Ruofei Bai, Jie Chen, Yuxin Cai +3

Nanyang Technological University, Singapore · Agency for Science, Technology and Research, Singapore · National University of Singapore, Singapore

Multi-Robot Motion Planning Constrained Motion Planning Trajectory Generation Diffusion Transformer Diffusion-Based Planning Multi-Robot Coordination

Jun 13, 2026cs.LG

DiRecT: Safe Diffusion-Based Planning via Receding-Horizon Denoising

Diffusion models have emerged as powerful tools for planning and control by learning multimodal distributions over actions and trajectories. Yet reliable inference-time safety enforcement remains a key barrier to their deployment in safety-critical tasks. Existing approaches typically project each denoising iterate onto the feasible set, even though constraints are defined only on the final clean trajectory. Enforcing feasibility on noisy intermediate samples can therefore overconstrain the sampling dynamics, substantially degrading sample quality. To address this limitation, we introduce DiRecT (Diffusion-based planning via Receding-horizon denoising with Terminal constraints), a training-free algorithm for constrained sampling from diffusion models via stochastic optimal control (SOC). DiRecT enforces constraints only on the final clean sample, avoiding unnecessary restrictions on the intermediate denoising dynamics. Inspired by model predictive control, we derive a principled receding-horizon surrogate for the otherwise intractable constrained SOC formulation, yielding an efficient algorithm that cleanly separates stochastic denoising from constraint satisfaction, progressively steering samples toward feasible final trajectories without distorting the learned diffusion dynamics. Furthermore, DiRecT is highly flexible: it can leverage off-the-shelf or domain-specific optimizers, incorporate priors over environment dynamics, and optimize additional soft rewards. Extensive experiments on safe planning benchmarks demonstrate that DiRecT substantially improves deployment safety and task performance over existing diffusion-based planning baselines.

Paolo Giaretta, Zeyang Li, Navid Azizan

MIT

Constrained Motion Planning Diffusion-Based Planning Stochastic Optimal Control Diffusion Sampling Safe Motion Planning

Jun 12, 2026cs.RO

Robustness without Wrinkles: Parallel Simulation and Robust MPC for Certified Deformable Manipulation

We present CORD-SLS, a real-time control method for safe deformable object manipulation, with a focus on ropes and cloth. At its core is a GPU-parallel differentiable simulator with contact smoothing which enables efficient gradient-based planning through intermittent contact. To robustly satisfy constraints under model and sensing uncertainty, we develop a real-time, GPU-parallel output-feedback robust model predictive control (MPC) algorithm that plans with this simulator. We further show that the simulator accelerates model-based RL for training neural manipulation policies. To improve real-world robustness, we use conformal prediction to calibrate visual-feedback and perception-error bounds for MPC, producing reachable tubes that enable high-probability safe control. We evaluate CORD-SLS on high-dimensional, contact-rich rope and cloth manipulation tasks in simulation and hardware, including obstacle avoidance, routing, folding, and smoothing. Across settings, CORD-SLS achieves millisecond-speed planning, exceeding baselines in safety, speed, and task success.

Wei-Chen Li, Jeffrey Fang, Sasanka Polisetti +2

Georgia Institute of Technology, Atlanta, GA 30308

Constrained Motion Planning Contact-Rich Robotic Manipulation Deformable Object Manipulation Model Predictive Control Dynamic Robotic Manipulation Differentiable Physics+1

Jun 12, 2026cs.RO

Semidefinite Relaxations for Collision-Free Motion Planning

We study semidefinite relaxations for collision-free motion planning. We focus on a point robot moving from start to goal through spherical obstacles in

\mathbb{R}^n

, subject to path continuity constraints and squared derivative costs; a setting that is conceptually simple yet captures the hardness of collision-free motion planning. We formulate this problem exactly as a nonconvex problem over polynomial curves, and present a natural semidefinite relaxation. We contribute two key theoretical insights; to our knowledge this is the first theoretical analysis of semidefinite relaxations for collision-free motion planning. First, we show that solving the convex relaxation is equivalent to solving, to global optimality, a related motion planning problem in a potentially higher-dimensional space. This geometric interpretation yields necessary and sufficient conditions for tightness, and a clear intuition for when the relaxation is loose. Second, we show that the relaxation admits a symmetry reduction that makes it significantly smaller than one might expect, with positive semidefinite cone sizes that scale linearly with the polynomial degree and are independent of the ambient dimension. The resulting relaxation is 10 to 100 times faster than direct nonlinear programming transcriptions solved with SNOPT and IPOPT, exhibits significantly lower variance in solve times, and reliably finds a locally optimal path for the original problem. We demonstrate its effectiveness as a convex steering function in an RRT planner for minimum-snap quadrotor planning with

C^4

continuous trajectories.

Bernhard Paus Graesdal, Alexandre Amice, Pablo A. Parrilo +1

Department of Electrical Engineering and Computer Science, Massachusetts Institute of Technology, Cambridge, MA, USA.

Semidefinite Programming Constrained Motion Planning Trajectory Optimization Motion Planning

Jun 10, 2026cs.RO

Learning Unions of Convex Sets via Invertible Latent Decomposition for Path Planning

Collision-free path planning in cluttered, real-world environments relies on a representation of the collision-free space, and existing representations broadly fall into two categories. Explicit representations, such as unions of convex sets, can be plugged into optimization-based planners as hard collision-free constraints, but their parameters scale poorly with configuration-space dimension. Implicit representations, by contrast, are flexible and scale well to complex geometries, yet typically lack such guarantees. We bridge this gap with ILD (Invertible Latent Decomposition), a framework that jointly learns an invertible mapping and a union of explicit convex polytopes in the resulting latent space. Planning is carried out over these latent convex sets, and the invertible mapping decodes the resulting paths back to the original configuration space while preserving feasibility with respect to the refined explicit safe regions. We further propose Visibility-Guided Sampling (VGS) to keep the convex sets connected for path planning. Across 2D navigation, 6-DoF, and 14-DoF manipulation environments, ILD achieves broader coverage, better inter-set connectivity, and higher path-planning success rates than prior baselines, with zero observed false positives after test-time refinement. On a 14-DoF bimanual manipulator, we further demonstrate real-time collision-free planning, with test-time refinement adapting to scene-geometry changes during real-world deployment on a single 6-DoF arm.

Taerim Yoon, Dongho Kang, Kisang Park +3

Korea University · ETH Zurich

Constrained Motion Planning Representation Learning Safe Motion Planning Motion Planning

Jun 8, 2026cs.RO

Safe Polytope-in-Polytope Motion Planning and Control with Control Barrier Functions

Autonomous mobile robots operating in tight environments require motion planning frameworks that account for the physical footprint of the robot. Simplifying the geometry to a point or a circle is conservative and discards information needed to successfully and safely traverse narrow passages. This work proposes a safe local motion planning and control method that guarantees that a polytopic robot footprint stays inside a continuously updated convex free-space region. The containment condition is formulated as a set of discrete-time control barrier function constraints within a model predictive controller. The number of safety constraints depends on the complexity of the local free-space geometry and the robot shape, instead of the number of obstacles. The proposed free-space formulation does not need any obstacle detection or segmentation. A comparative analysis against a polytope-based obstacle avoidance formulation confirms favorable scaling up to a reduction of 91

\times

in computation time as the number of obstacles increases. The approach is validated in simulation with an autonomous surface vehicle and on hardware with a non-holonomic mobile robot, using both occupancy grids and LiDAR sensing. The experiments demonstrate safe real-time motion planning and control at 10~Hz on an onboard embedded computer, including reactive avoidance of dynamic obstacles.

Alejandro Gonzalez-Garcia, Dries Dirckx, Jan Swevers +1

Constrained Motion Planning Control Barrier Functions Robot Safety Model Predictive Control Safe Motion Planning Motion Planning+2

Jun 8, 2026cs.RO

Bayesian Optimization for Learning Nonlinear MPC in Autonomous Agent Navigation

Real-time autonomous navigation in dynamic, unknown environments remains a fundamental challenge for mobile robotics. We propose a map-free framework that tightly integrates reactive rolling-horizon planning with nonlinear Model Predictive Control (MPC). At each control cycle, a LiDAR-based Gaussian occupancy representation is constructed and used to generate collision-free trajectories via A* search, which are then tracked by a CasADi/IPOPT MPC formulation incorporating a smooth sigmoid obstacle barrier. To improve robustness to parameter sensitivity, we adopt an offline Bayesian optimization scheme based on Tree-structured Parzen Estimators (TPE), which identifies near-optimal controller parameters with respect to a composite navigation objective. In addition, a Gaussian Process surrogate is used to analyze parameter sensitivity and provide insight into the optimization landscape. The proposed framework is robot-agnostic and is evaluated on the Unitree Go2 quadruped in simulation using Gazebo, followed by deployment on the physical robot. Experimental results show that parameters tuned in simulation transfer effectively to hardware, maintaining comparable performance without additional tuning. The full system achieves up to a 90.0% navigation success rate when deployed, along with a 38.9% average improvement in the evaluation metrics across simulated environments.

Lorenzo Ortolani, Gabriel Voss, Gabriele Beltrami +2

Talos Robotics AI Milan, Italy

Constrained Motion Planning Nonlinear MPC Robot Navigation Dynamic Obstacle Avoidance Optimal Control

Jun 7, 2026cs.RO

Real-Time and Accurate Collision-Free Teleoperation via Differentiable Constraint-Based Trajectory Planning

In teleoperation, the human operator typically controls only the end-effector pose, which often leads to self-collisions of the manipulator and collisions with environmental obstacles, since joints and links are not controlled individually. A common strategy to mitigate this issue is to enhance the operator's input using optimal-control-based trajectory planning. As derivative-based solvers require differentiable constraints, existing approaches either approximate robots and obstacles with spheres, reducing geometric accuracy, or approximate derivatives, degrading convergence and increasing computation times. We address these limitations by adapting a recent formulation of differentiable collision-avoidance constraints, based on duality in convex optimization, to the teleoperation setting. The robot is approximated with capsules and the environment with polytopes. We compare the resulting trajectory planning method against state-of-the-art techniques in simulation with varying numbers of obstacles and evaluate it on a UR5e manipulator in a real-world teleoperation test. Results show that our approach achieves lower computation times while enabling more accurate obstacle modeling, leading to smoother and collision-free end-effector teleoperation.

Max Grobbel, Tristan Schneider, Daniel Flögel +1

FZI - Forschungszentrum Informatik, Karlsruhe, Germany · Department of Electrical Engineering, Karlsruhe Institute of Technology, Karlsruhe, Germany

Collision Avoidance Constrained Motion Planning Trajectory Optimization Robot Teleoperation Safe Motion Planning Robot Collision Avoidance

Jun 5, 2026cs.RO

Path Planning Using Deep Deterministic Policy Gradient: A Reinforcement Learning Approach

Path-planning for autonomous vehicles in threat-laden environments is a fundamental challenge because the problem is nonlinear and nonconvex even in simplest scenarios. While traditional optimal control methods can be used to find ideal paths, the computational time is often too slow for real-time decision-making. To solve this challenge, we propose a method based on Deep Deterministic Policy Gradient (DDPG) and model the threat as possibly multiple circular 'no-go' zones. A mission is regarded as a failure if the vehicle enters this restricted zone at any time or does not reach a neighborhood of the destination. The DDPG agent is trained through trial and error in a simulated environment, learning a direct mapping from its current state (position and heading) to a series of feasible actions that guide the agent to safely reach its destination. The reword function has three parts: (a) an attractive field centered at the final destination, (b) some repulsive fields centered at the origins of circular obstacles, and (c) a penalty of control energy consumption (the magnitude of heading change) that indirectly in favor for straight path. The DDPG trains the agent using these incentives to find the largest possible set of starting points wherein a safe path to the destination is guaranteed. This provides critical information for mission planning, showing beforehand whether a task is achievable from a given starting point, assisting pre-mission planning activities. The approach is validated in simulation. A comparison between the DDPG method and a traditional optimal control (pseudo-spectral) method is carried out. The results show that the learning-based agent produces effective paths while being significantly faster, making it a better fit for real-time applications.

Qiang Le, Yaguang Yang, Isaac E. Weintraub

Hampton University, Hampton, VA 23669, USA · Air Force Research Laboratory, Wright-Patterson AFB, OH 45433, USA

Constrained Motion Planning Safe Motion Planning Autonomous Driving Planning RL for Autonomous Driving Deep Deterministic Policy Gradient

May 27, 2026cs.RO

Natural Functional Gradients for Smooth Trajectory Optimization

Generating collision-free and smooth motions remains a central challenge in robotic manipulation, particularly in cluttered environments and narrow passages where feasible regions are highly constrained and fragmented. We propose a trajectory optimization framework that performs geometry-aware updates directly in function space using natural functional gradients. The method optimizes a Gaussian-smoothed surrogate objective that regularizes the optimization landscape through smooth trajectory perturbations while preserving trajectory-level structure. Because the updates are defined intrinsically in function space, trajectory regularity can be controlled independently of a particular time discretization. We derive a practical Monte-Carlo estimator of the natural functional gradient that requires only black-box trajectory evaluations, making the method applicable when analytic gradients are unavailable or unreliable due to collision checking and contact-rich simulation. Experiments on constrained robotic manipulation tasks demonstrate that the proposed method improves trajectory feasibility and produces smoother motions than representative planning and trajectory optimization baselines in environments with narrow geometric clearances. Additional results, videos, and implementation details are available at the project page: https://kisangpark.github.io/natural-functional-gradient/

Kisang Park, Chanwoo Kim, Kyungjae Lee +1

Department of Artificial Intelligence, Korea University, Seoul, Republic of Korea · Department of Statistics, Korea University, Seoul, Republic of Korea · 3RLWRLD, Seoul, Republic of Korea

Constrained Motion Planning Trajectory Optimization Trajectory Generation Robotic Manipulation Robot Motion Planning

May 26, 2026cs.RO

Simulation-Informed Diffusion for Decentralized Multi-robot Motion Planning

Decentralized multi-robot motion planning requires each robot to generate collision-free trajectories from local observations, without global sensing or reliable communication. However, most existing planners, whether classical or learning-based, generate trajectories from a static snapshot of the local observation, which limits their ability to anticipate the future behavior of neighboring robots. This limitation is critical as the number of robots increases and the environment becomes more cluttered. To overcome this challenge, this paper introduces Simulation-Informed Diffusion (SID), a decentralized framework built on constraint-aware diffusion models (CADM). SID first uses CADM to simulate the future trajectories of neighboring robots from their currently observed states, and then uses the same CADM to plan each robot's own trajectory under safety constraints informed by these simulations. Crucially, the accurate simulation of neighbors enables a minimal communication scheme that triggers coordination only when necessary in highly congested scenarios. Experiments across diverse environments show that SID consistently outperforms baseline methods in terms of planning effectiveness and constraint satisfaction, and scales to scenarios with 108 robots and 160 obstacles.

Jinhao Liang, Sven Koenig, Ferdinando Fioretto

University of Virginia · University of California, Irvine

Multi-Robot Motion Planning Constrained Motion Planning Diffusion-Based Planning Safe Motion Planning

May 26, 2026cs.RO

Provably Safe Motion Planning Under Unknown Disturbances

We present a provably safe sampling-based motion planning algorithm for robotic systems affected by random disturbances of unknown distribution. We consider systems with linear or linearizable dynamics evolving in workspace with arbitrary-shaped obstacles subject to state and control constraints. Safety requirements are formulated as chance-constraints. Our approach leverages data from trajectories of the system to learn a Wasserstein ambiguity tube, i.e., a sequence of ambiguity sets, which contains the trajectory of the system's state distribution with high confidence. This ambiguity tube is then used in a probabilistically complete algorithm to grow a sampling-based motion planning tree that respects the constraints of the problem. We show that learning several lower-dimensional ambiguity tubes instead of a single high-dimensional one effectively reduces the conservatism and boosts scalability. Additionally, we design an efficient bandit-based validity checker that remarkably increases the empirical performance of our approach without sacrificing probabilistic completeness. Case studies show our algorithm finds valid plans in cluttered environments under strict safety thresholds, outperforming state-of-the-art methods.

Ibon Gracia, Qi Heng Ho, Luca Laurenti +1

Department of Aerospace Engineering Sciences at the University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering, Virginia Tech, VA, USA · Delft University of Technology, the Netherlands, and the Italian Institute of Artificial Intelligence, Italy

Constrained Motion Planning Robot Safety Wasserstein Distributionally Robust Optimization Safe Motion Planning Motion Planning Sampling-Based Motion Planning

May 24, 2026cs.RO

Manifold-Constrained MPPI: Real-Time Sampling-Based Control for Nonlinear Equality-Constrained Robotic Systems

Sampling-based model predictive control methods, such as Model Predictive Path Integral (MPPI), offer derivative-free optimization and robustness in complex robotic systems. However, standard MPPI relies on cost-based soft penalties that cannot guarantee hard-constraint satisfaction, severely limiting its applicability to highly constrained tasks such as closed-chain manipulation. To address this, we propose Manifold-Constrained MPPI (MC-MPPI), a real-time sampling-based control framework that regulates nonlinear equality constraints while preserving the computational advantages of MPPI. The key idea is to decouple the constrained optimal control problem into latent-space planning and execution-level correction. At the planning stage, a Variational Autoencoder (VAE) learns a low-dimensional latent representation of the constraint manifold, enabling MPPI to efficiently generate near-feasible candidate trajectories without per-sample modification. Since this reference enables accurate linearization of the equality constraints, an execution-level Quadratic Programming (QP) controller resolves the residual manifold mismatch in a single solve rather than through iterative projection. Experiments on a 14-DoF closed-chain dual-arm system in both simulation and real-world settings demonstrate 100 Hz planning and 500 Hz execution, equality-residual regulation in static and dynamic environments, and a 95% success rate over 40 randomized hardware trials. Supplementary videos and implementation details are available at https://rcilab.github.io/mcmppi.

Seulchan Lee, Sanghyun Kim

School of Mechanical Engineering, Kyung Hee University, Yongin, South Korea

Robotic Control Constrained Motion Planning Constrained Optimization MPPI Control

May 22, 2026cs.RO

Signal Temporal Logic Motion Planning via Graphs of Convex Sets

This paper investigates continuous-time motion planning under Signal Temporal Logic (STL) specifications. The goal is to generate smooth robot trajectories that satisfy high-level logical and timing requirements while respecting low-level motion constraints. To this end, we propose an efficient framework that combines timed-automata reasoning with graphs of convex sets (GCS). An STL specification is first represented by a timed automaton, which is then coupled with a convex decomposition of the configuration space to form a joint transition system encoding both task progress and region occupancy. Based on this joint transition system, the STL motion-planning problem is reformulated as a shortest-path problem over a GCS, whose solution induces a smooth Bézier-spline trajectory satisfying the STL specification, smoothness requirements, and velocity bounds. We establish the soundness of the proposed formulation and analyze its computational complexity, showing that, once the timed automaton and convex decomposition are fixed, the convex relaxation scales polynomially with the configuration-space dimension and the Bézier degree. We further develop a compact timed-automaton construction for an expressive STL fragment using dedicated templates and Boolean composition. Numerical experiments on low-dimensional benchmarks, a

3

-D quadrotor, a

30

-DoF humanoid, and a hardware experiment on a UR-3 robot arm demonstrate that the proposed method efficiently solves complex STL motion-planning problems and produces smooth executable trajectories.

Yu Chen, Ancheng Hou, Mingyang Feng +2

School of Automation & Intelligent Sensing, Shanghai Jiao Tong University, Shanghai 200240, China · Institute of Artificial Intelligence, Xiamen University, Xiamen, China

Constrained Motion Planning Trajectory Generation Motion Planning Robot Motion Planning

May 21, 2026cs.LG

Steered Generation via Gradient-Based Optimization on Sparse Query Features

Latent steering exploits internal representations of Large Language Models (LLMs) to guide generation, yet interventions on dense states can entangle distinct semantic features. In this paper, we investigate attention query activations as a high-fidelity site for precise control, hypothesizing that manipulating the attention mechanism itself offers sharper steerability than general state interventions. We introduce Prototype-Based Sparse Steering, a framework that applies Sparse Autoencoders (SAEs) specifically to query activations, to decompose them into interpretable features, then apply gradient-based optimization during inference to align the sparse representation with class prototypes of target behaviors. To validate this architectural insight, we first analyze the mechanism in Textualized Gridworld, a controlled environment for verifiable planning constraints. We demonstrate that optimizing sparse query features enables effective navigation of rigid planning requirements (i.e., safe vs. short paths), confirming the method's ability to satisfy objective rules. We then demonstrate the framework's versatility by training SAEs on a high-dimensional educational domain, where the framework steers the cognitive complexity of feedback (i.e., Bloom's Taxonomy). Our experiments establish that sparse query representations provide the necessary disentanglement for unified, interpretable control over both logical planning and stylistic nuance.

Sumanta Bhattacharyya, Pedram Rooshenas

Department of Computer Science University of Illinois Chicago

Constrained Motion Planning Language Model Steering Large Language Model-Guided Optimization Sparse Autoencoders Activation Steering

May 21, 2026cs.RO

Verified Task-Space Motion Planning Under Joint-Space Constraints

Reactive task-space planners such as Bug2 operate with fixed Cartesian step sizes and are unaware of the manipulator's joint-angle limits. When the Jacobian is poorly conditioned, even small Cartesian steps can demand joint changes that exceed admissible bounds; clipping the joints to their limits causes tracking drift and can prevent goal reaching entirely. We address this by computing, at each planning step, the largest Cartesian hyperrectangle that is \emph{certifiably reachable} under joint displacement bounds. Using a second-order polynomial approximation of the inverse kinematics and the S-procedure, we formulate a small semidefinite program whose solution yields the certified half-width~

λ^\star

. An equivalent bisection procedure exploiting the quadratic structure solves the certification in sub-millisecond time. Integrating this certificate with Bug2 yields a planner whose step size adapts to local kinematic conditioning. In a statistical evaluation over 94 adversarial scenarios spanning six joint-limit settings, the SOS-verified planner achieves \emph{zero} joint-limit violations with a 100% goal-reaching rate, whereas a standard Bug2 planner violates joint limits in 6--11% of steps and fails to reach the goal in up to 18% of scenarios.

Hanjiang Hu, Changliu Liu, Yebin Wang

Robotics Institute, Carnegie Mellon University · Mitsubishi Electric Research Laboratories (MERL), Cambridge, MA 02139, USA

Constrained Motion Planning Safe Motion Planning Motion Planning

May 20, 2026cs.RO

Safe and Steerable Geometric Motion Policies for Robotic Dexterous Manipulation

Robotic dexterous manipulation requires continuously reconciling objectives and constraints defined on heterogeneous geometric spaces: a robot controlled on a

\mathbb{R}^7

configuration manifold may need to track end effector poses on

\mathrm{SE}(3)

while satisfying obstacle avoidance margins in

\mathbb{R}

. We present Safe Pullback Bundle Dynamical Systems (SafePBDS), a geometrically consistent framework that computes optimal, certifiably safe configuration manifold accelerations from objectives and safety requirements on arbitrary task manifolds. SafePBDS builds on prior work that combines predefined task manifold dynamical systems to produce autonomous motion. Its first innovation is a pullback control barrier function construction, which converts task manifold safety conditions into linear constraints on configuration manifold accelerations. The second innovation is a task manifold action interface that allows a high-level policy to inject low dimensional residual motions; zero input recovers the autonomous behavior, while safety is preserved under arbitrary inputs. This lets high-level policies efficiently steer exploration while leaving precise motion to the autonomous behavior. We validate SafePBDS in simulation and on a 23-DOF Franka Panda-Allegro Hand platform. On dexterous grasping, SafePBDS achieves a

92.5\%

success rate across 20 household objects and 120 trials. Using the action interface, the method can exclude any one of the four fingers during grasping via a one-dimensional action, achieving

94.4\%

3-finger grasp success across 3 objects and 36 trials. The efficient planning and safety guarantee of SafePBDS also enables the first model-based, fully actuated palm-down in-hand reorientation, exceeding

360^\circ

of yaw rotation in both directions under varying object weight and wrist motion. Demo video and details: https://tml.stanford.edu/safe-pbds

Albert Wu, Riccardo Bonalli, Thomas Lew +1

Computer Science Department, Stanford University, Stanford, CA 94305, USA. · Laboratory of Signals and Systems, University of Paris-Saclay, CNRS, CentraleSup´elec, France. · Toyota Research Institute, Los Altos, CA 94022, USA.

Robotic Control Constrained Motion Planning Robotic Grasping Dexterous Manipulation In-Hand Manipulation Control Barrier Functions+3

May 20, 2026cs.RO

A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Kinodynamic planning for car-like vehicles on uneven terrain requires simultaneously optimizing competing objectives such as path efficiency and pose stability. This work presents an adaptive epsilon-constraint method integrated into a Model Predictive Control (MPC) framework, where the epsilon bounds are dynamically adjusted based on terrain descriptors to explore the Pareto front in real time. To capture vehicle-terrain dynamics, we develop a semi-parametric model combining analytical vehicle dynamics with a Sparse Gaussian Process (SGP) trained on the same terrain descriptors. The proposed epsilon-MPC is evaluated against MPPI and GAKD baselines, achieving a 94% navigation success rate while reducing maximum orientation deviation by 24% and improving multi-objective trade-off quality by 23%.

Otobong Jerome, Geesara Kalathunga, Tiago Nascimento

Laborat´orio de Engenharia de Sistemas e Rob´otica, Universidade Federal da Para´ıba (UFPB), Jo˜ao Pessoa, Para´ıba, 58055-000, Brazil · School of Computer Science, University of Lincoln, Lincoln, United Kingdom

Constrained Motion Planning Kinodynamic Motion Planning Robot Navigation Model Predictive Control Vehicle Dynamics Motion Planning+1

May 19, 2026cs.RO

KIO-planner: Attention-Guided Single-Stage Motion Planning with Dual Mapping for UAV Navigation

Autonomous UAV flight in confined, wall-dense environments requires low-latency and reliable motion planning under strict safety constraints. Traditional optimization-based planners suffer from mapping latency and easily fall into local minima when navigating through dense structural obstacles. Meanwhile, existing end-to-end learning methods struggle to extract fine-grained geometric features from raw depth images and lack hard kinodynamic constraints, leading to unpredictable collisions near walls. To address these issues, we propose KIO-planner, an attention-guided single-stage trajectory planning framework. First, we integrate a Convolutional Block Attention Module (CBAM) into the perception backbone to adaptively focus on critical structural edges and traversable space. Second, we introduce a novel Dual Mapping mechanism--comprising physical bounds activation and a deterministic Geometric Safety Shield in the depth-pixel space--to enforce kinodynamic feasibility and collision-free flight without global map fusion. Extensive high-fidelity simulated experiments demonstrate that KIO-planner enables highly agile navigation at speeds up to 3.0 m/s. Compared to the state-of-the-art baseline, KIO-planner achieves lower inference latency (approximately 24 ms) and generates significantly smoother trajectories, reducing control cost by 28.4%. Most notably, our Dual Mapping substantially increases the worst-case safety margin, measured by minimum distance to obstacles, from 0.48 m to 0.76 m, ensuring fast, smooth, and safer navigation in highly constrained environments.

Dexing Yao, Haochen Li, Junhao Wei +11

Faculty of Applied Sciences Macao Polytechnic University Macao, China · College of Animal Science and Technology Zhongkai University of Agriculture and Engineering Guangzhou, China · School of Economics and Management South China Normal University Macao, China +1

Constrained Motion Planning Kinodynamic Motion Planning UAV Navigation Safe Motion Planning Motion Planning Robot Collision Avoidance

May 18, 2026eess.SY

Probabilistic Recursively Feasible Motion Planning Under Uncertain Environments

Safe motion planning in uncertain, time-varying environments is challenging because the safe region can change unpredictably across planning steps, often causing a loss of recursive feasibility. In this work, we present a Probabilistic Recursively Feasible Model Predictive Control (PRF-MPC) framework that guarantees recursive feasibility with a specified probability. We introduce properties that an ideal predictor should satisfy to ensure distributional consistency, and use these properties to derive closed-form expressions for the means and covariances of trajectories predicted at future time steps. Building on this analysis, we construct safety constraints that ensure, with high probability, that the current safe set is contained within the safe sets at future time steps, thereby probabilistically guaranteeing recursive feasibility. Simulation results on a lane-change scenario demonstrate that the proposed method significantly improves recursive feasibility.

Hyeontae Sung, Hyeongchan Ham, Junyoung Park +2

School of Electrical Engineering, Korea Advanced Institute of Science and Technology, Daejeon, Korea · The SYCAMORE Lab, ´Ecole Polytechnique F´ed´erale de Lausanne (EPFL), Switzerland

Constrained Motion Planning Model Predictive Control Safe Motion Planning Motion Planning

May 16, 2026cs.RO

NORM-Nav: Zero-Shot Mobile Robot Navigation with Natural Language Behavioral Constraints

Mobile robots operating in human-centered environments must generate not only collision-free paths but also trajectories that follow local behavioral conventions. Conventional costmap-based navigation emphasizes geometric feasibility and often overlooks such requirements, which can result in socially inappropriate behaviors. This paper presents NORM-Nav, a zero-shot framework that integrates natural language behavioral constraints into costmap-based planning. An LLM parses each instruction into structured constraints and grounds them using real-time vision--LiDAR perception. These constraints are encoded as multi-layer costmaps that represent geometric, semantic, directional, and velocity cues and are directly compatible with standard grid-based planners. Simulation and real-world experiments indicate that NORM-Nav improves task success rates and produces trajectories closer to human references than representative baselines. The project website is available at https://ei-nav.github.io/NORM-Nav.

Dongjie Huo, Junhui Wang, Chao Gao +3

College of Information Science and Technology, Beijing University of Chemical Technology · Institute for AI Industry Research (AIR), Tsinghua University · Institute of Systems Engineering and Collaborative Laboratory for Intelligent Science and Systems, Macau University of Science and Technology +1

Constrained Motion Planning Trajectory Generation Large Language Model-Based Robot Planning Social Robot Navigation Language-Conditioned Robot Control Robot Motion Planning

May 15, 2026cs.RO

Constrained MPC-Based Motion Planning for Morphing Quadrotors in Ultra-Narrow Passages under Limited Perception

This paper introduces a motion planning framework to plan morphology and trajectory for morphing quadrotors under extremely constrained environments. We develop a novel obstacle avoidance cost function for nonlinear model predictive control (MPC) that enables navigation through extremely narrow gaps under limited perception from a 2D LiDAR. Classical artificial potential field-based costs typically have a high cost in narrow passages, artificially blocking the navigable path. In contrast, we propose a smooth exponential obstacle cost that preserves low traversal cost within narrow gaps while maintaining strong collision avoidance behavior. The formulation avoids hard activation thresholds and introduces a cost reduction factor to reduce the cost within narrow passages. Direct use of 2D LiDAR measurements in MPC allows navigation around arbitrarily shaped obstacles. The method is embedded within an acados-based nonlinear MPC framework. Simulation and experimental results demonstrate successful traversal of narrow corridors where typical repulsive cost functions would fail. The approach provides a computationally efficient and practical solution for navigating through tight spaces while maintaining safety from the obstacles. While we are implementing the framework on the morphing quadrotors, the cost function formulation is general-purpose for any mobile robot application, and is not limited to the morphing quadrotors. The implementation code is available at Github Repo and a short video is available at Video Link.

Harsh Modi, Xiao Liang, Minghui Zheng

Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA · Department of Civil & Environmental Engineering, Texas A&M University, College Station, TX 77843, USA

Aerial Robotics Constrained Motion Planning Trajectory Generation Nonlinear MPC Model Predictive Control Safe Motion Planning+1

May 15, 2026cs.MA

From Gridworlds to Warehouses: Adapting Lightweight One-shot Multi-Agent Pathfinding for AGVs

Multi-agent pathfinding (MAPF) under one-shot planning is a core component of warehouse automation, yet classical formulations typically assume four-connected 2D grids with unit-time moves in four directions. To fill reality gaps while still being trackable with discrete combinatorial search, this work proposes a more practical counterpart tailored to differential-drive AGVs. We term this multi-agent warehouse pathfinding (MAWPF), featured with four constraints: (i) agent actions are restricted to straight motion and in-place rotation; (ii) rotations require multi-step costs; (iii) acceleration and deceleration are considered, and; (iv) follower collisions are prohibited to prevent rear-end crashes. To solve MAWPF efficiently, we adapt representative suboptimal MAPF algorithms-PP, LNS2, PIBT, and LaCAM-and conduct comprehensive benchmarking. Our experiments reveal that PP and LNS2 struggle to solve instances with many agents, while PIBT-based approaches achieve preferable scalability with increased solution cost. We believe that these constitute an important step toward adapting classical gridworld MAPF to operational warehouse setups.

Hiroki Nagai, Keisuke Okumura

National Institute of Advanced Industrial Science and Technology (AIST), Japan

Multi-Robot Motion Planning Constrained Motion Planning Robot Navigation Multi-Agent Path Finding

May 13, 2026cs.RO

TinySDP: Real Time Semidefinite Optimization for Certifiable and Agile Edge Robotics

Semidefinite programming (SDP) provides a principled framework for convex relaxations of nonconvex geometric constraints in motion planning, yet existing solvers are too computationally expensive for real-time control, particularly on resource-constrained embedded systems. To address this gap, we introduce TinySDP, the first semidefinite programming solver designed for embedded systems, enabling real-time model-predictive control (MPC) on microcontrollers for problems with nonconvex obstacle constraints. Our approach integrates positive-semidefinite cone projections into a cached-Riccati-based ADMM solver, leveraging computational structure for embedded tractability. We pair this solver with an a posteriori rank-1 certificate that converts relaxed solutions into explicit geometric guarantees at each timestep. On challenging benchmarks, e.g., cul-de-sac and dynamic obstacle avoidance scenarios that induce failures in local methods, TinySDP achieves collision-free navigation with up to 73% shorter paths than state-of-the-art baselines. We validate our approach on a Crazyflie quadrotor, demonstrating that semidefinite constraints can be enforced at real-time rates for agile embedded robotics.

Ishaan Mahajan, Jon Arrizabalaga, Andrea Grillo +4

Columbia University, USA · Massachusetts Institute of Technology, USA · ´Ecole Polytechnique F´ed´erale de Lausanne (EPFL), Switzerland +1

Semidefinite Programming Constrained Motion Planning Robotics Model Predictive Control Constrained Optimization Safe Motion Planning

May 12, 2026cs.MA

Distance-Constrained Unlabeled Multi-Agent Pathfinding

We study a graph pathfinding problem Distance-

r

Independent Unlabeled Multi-Agent Pathfinding, finding a set of collision-free paths between two sets where agents must stay at pairwise distance at least

r+1

at all times. This additional constraint, generalizing collision modeling for classical MAPF, targets aspects of real-world multi-agent coordination. This additional distance constraint makes feasibility (i.e., whether a solution exists) PSPACE-complete, in contrast to standard (unlabeled) MAPF, where it can be decided in polynomial time. We address the challenge via two complementary approaches: (i) reduction-based optimal algorithms with a feasibility-preserving compression procedure, and (ii) a configuration generator-based search. Despite the hardness, empirical results show that our algorithm can handle hundreds of agents in a practical timeframe.

Takahiro Suzuki, Yuma Tamura, Keisuke Okumura

Tohoku University · National Institute of Advanced Industrial Science and Technology (AIST)

Constrained Motion Planning Multi-Agent Coordination Multi-Agent Path Finding

May 9, 2026cs.RO

IMPACT: An Implicit Active-Set Augmented Lagrangian for Fast Contact-Implicit Trajectory Optimization

Contact-implicit trajectory optimization (CITO) has attracted growing attention as a unified framework for planning and control in contact-rich robotic tasks. Recent approaches have demonstrated promising results in manipulation and locomotion without requiring a prescribed contact-mode schedule. It is well known that the underlying mathematical programs with complementarity constraints (MPCCs) remain numerically ill-conditioned, and systematic, scalable solution strategies for CITO remain an active area of research. More efficient and principled solvers that can handle contact constraints are therefore essential to broaden the applicability of CITO. In this work, we develop an augmented-Lagrangian approach to CITO for solving MPCC-based CITO with stationarity guarantees. The method can be interpreted as identifying the implicit contact-mode branches on the fly during the trajectory optimization (TO) iterations; we call this approach IMPACT (IMPlicit contact ACtive-set Trajectory optimization). We provide an efficient C++ implementation tailored to trajectory-optimization workloads and evaluate it on the open-source CITO and contact-implicit model predictive control (CI-MPC) benchmarks. On CITO, IMPACT achieves 2.9x-70x speedups over strong baselines (geometric mean 13.8x). On CI-MPC, we show improved control quality for contact-rich trajectories on dexterous manipulation tasks in simulation. Finally, we demonstrate the proposed method on real robotic hardware on a T-shaped object pushing task.

Jiayun Li, Dejian Gong, Georgia Chalvatzaki

1PEARL Lab, Dept. of Computer Science, TU Darmstadt, Germany · 2Hessian.AI, Darmstadt, Germany · 3Robotics Institute Germany (RIG)

Constrained Motion Planning Trajectory Optimization Contact-Rich Robotic Manipulation Augmented Lagrangian Method Constrained Optimization Optimal Control

May 5, 2026cs.RO

Feasibility-aware Hybrid Control for Motion Planning under Signal Temporal Logics

In this work, a novel method for planar task and motion planning based on hybrid modeling is proposed. By virtue of a discrete variable which models local constraint satisfaction and enables local feasibility analysis, the proposed control architecture unifies planning with control design. Concurrently, control barrier functions are designed on a transformed disk version of the original nonconvex and geometrically complex robotic workspace, thus amending the issue of deadlocks. Simulations of the proposed method indicate effective handling of multiple overlapping spatio-temporal tasks even in the face of input saturation.

Panagiotis Rousseas, Dimos V. Dimarogonas

∗KTH Royal Institute of Technology, School of Electrical Engineering and Computer Science, Division of Decision and Control Systems

Constrained Motion Planning Signal Temporal Logic Feedback Control Safe Motion Planning Motion Planning Robot Motion Planning

May 4, 2026cs.RO

Visibility-Aware Mobile Grasping in Dynamic Environments

This paper addresses the problem of mobile grasping in dynamic, unknown environments where a robot must operate under a limited field-of-view. The fundamental challenge is the inherent trade-off between seeing'' around to reduce environmental uncertainty and moving'' the body to achieve task progress in a high-dimensional configuration space, subject to visibility constraints. Previous approaches often assume known or static environments and decouple these objectives, failing to guarantee safety when unobserved dynamic obstacles intersect the robot's path during manipulation. In this paper, we propose a unified mobile grasping system comprising two core components: (1) an iterative low-level whole-body planner coupled with velocity-aware active perception to navigate dynamic environments safely; and (2) a hierarchical high-level planner based on behavior trees that adaptively generates subgoals to guide the robot through exploration and runtime failures. We provide experimental results across 400 randomized simulation scenarios and real-world deployment on a Fetch mobile manipulator. Results show that our system achieves a success rate of 68.8% and 58.0% in unknown static and dynamic environments, respectively, significantly boosting success rates by 22.8% and 18.0% over the \nam approach in both unknown static and dynamic environments, with improved collision safety.

Tianrun Hu, Anxing Xiao, David Hsu +1

†School of Computing, National University of Singapore · §Smart Systems Institute, National University of Singapore

Active Perception Constrained Motion Planning Robotic Grasping Motion Planning Dynamic Obstacle Avoidance

May 4, 2026cs.RO

Feedback Motion Planning for Stochastic Nonlinear Systems with Signal Temporal Logic Specifications

We study feedback motion planning for continuous-time stochastic nonlinear systems under signal temporal logic (STL) specifications. We propose a framework that synthesizes control policies for chance-constrained STL trajectory optimization problems, with the goal of ensuring that the closed-loop stochastic system satisfies a given STL formula with high probability (e.g., 99.99%). Our approach is based on a predicate erosion strategy that transforms the intractable stochastic problem into a deterministic STL trajectory optimization problem with tightened STL formula constraints. The amount of erosion is determined by a probabilistic reachable tube (PRT) that bounds the deviation between the stochastic trajectory and an associated nominal trajectory. To compute such bounds, we leverage contraction theory and feedback design, and develop several tracking controllers. This yields a complete feedback motion planning pipeline which can be implemented by numerical optimizations. We demonstrate the efficacy and versatility of the proposed framework through simulations on several robotic systems and through experiments on a real-world quadrupedal robot, and show that it is less conservative and achieves higher specification satisfaction probability than representative baselines.

Liqian Ma, Zishun Liu, Glen Chou +1

Georgia Institute of Technology, Atlanta, GA 30332

Constrained Motion Planning Trajectory Optimization Stochastic Optimal Control Motion Planning Robot Motion Planning

May 3, 2026cs.RO

Optimized and kinematically feasible multi-agent motion planning

Multi-agent motion planning (MAMP) is an important problem for autonomous systems with multiple agents. In this work we propose a two-step method for finding optimized and kinematically feasible solutions to MAMP problems. The first step finds an initial feasible solution using state-of-the-art methods such as conflict-based search (CBS) or priority-based search (PBS), and the second step is an improvement step which improves the solution by solving a multi-phase optimal control problem (OCP) where the initial solution is used to warm-start the solver. We also propose a method for generating motion primitives in an optimized way under the constraint that the primitive durations are all multiples of the same sample time. We evaluate our proposed framework on a MAMP problem for tractor-trailer systems. We extend the safe interval path planning with interval projections (SIPP-IP) algorithm so it can handle more general cost functions and larger agents, but our results show that for the tractor-trailer system a simple lattice-based planner performs better due to less conservative collision checks. Our experiments also indicate that CBS performs better than PBS for this system as it achieves a higher success rate in environments with obstacles and had a lower average runtime, although both planners achieve solutions of similar quality after the improvement step.

Anja Hellander, Kristoffer Bergman, Daniel Axehill

Division of Automatic Control, Linköping University, Sweden · Saab AB, Linköping, Sweden

Constrained Motion Planning Trajectory Optimization Multi-Agent System Optimization Kinodynamic Motion Planning Mobile Robotics Motion Planning+1

Apr 29, 2026eess.SY

Safe Navigation using Neural Radiance Fields via Reachable Sets

Safe navigation in cluttered environments is an important challenge for autonomous systems. Robots navigating through obstacle ridden scenarios need to be able to navigate safely in the presence of obstacles, goals, and ego objects of varying geometries. In this work, reachable set representations of the robot's real-time capabilities in the state space can be utilized to capture safe navigation requirements. While neural radiance fields (NeRFs) are utilized to compute, store, and manipulate the volumetric representations of the obstacles, or ego vehicle, as needed. Constrained optimal control is employed to represent the resulting path planning problem, involving linear matrix inequality constraints. We present simulation results for path planning in the presence of numerous obstacles in two different scenarios. Safe navigation is demonstrated through using reachable sets in the corresponding constrained optimal control problems.

Omanshu Thapliyal, Malarvizhi Sankaranarayanasamy, Ravigopal Vennelakanti

Researcher, Hitachi America Ltd. Santa Clara, USA

Constrained Motion Planning Safe Robot Navigation Robot Safety Robot Motion Planning

Apr 22, 2026cs.RO

Navigating the Clutter: Waypoint-Based Bi-Level Planning for Multi-Robot Systems

Multi-robot control in cluttered environments is a challenging problem that involves complex physical constraints, including robot-robot collisions, robot-obstacle collisions, and unreachable motions. Successful planning in such settings requires joint optimization over high-level task planning and low-level motion planning, as violations of physical constraints may arise from failures at either level. However, jointly optimizing task and motion planning is difficult due to the complex parameterization of low-level motion trajectories and the ambiguity of credit assignment across the two planning levels. In this paper, we propose a hybrid multi-robot control framework that jointly optimizes task and motion planning. To enable effective parameterization of low-level planning, we introduce waypoints, a simple yet expressive representation for motion trajectories. To address the credit assignment challenge, we adopt a curriculum-based training strategy with a modified RLVR algorithm that propagates motion feasibility feedback from the motion planner to the task planner. Experiments on BoxNet3D-OBS, a challenging multi-robot benchmark with dense obstacles and up to nine robots, show that our approach consistently improves task success over motion-agnostic and VLA-based baselines. Our code is available at https://github.com/UCSB-NLP-Chang/navigate-cluster

Jiabao Ji, Yongchao Chen, Yang Zhang +4

UC Santa Barbara · Massachusetts Institute of Technology · Harvard University +2

RL for Robotics Multi-Robot Motion Planning Constrained Motion Planning Multi-Robot Systems Robot Task Planning Motion Planning

Apr 20, 2026cs.RO

DAG-STL: A Hierarchical Framework for Zero-Shot Trajectory Planning under Signal Temporal Logic Specifications

Signal Temporal Logic (STL) is a powerful language for specifying temporally structured robotic tasks. Planning executable trajectories under STL constraints remains difficult when system dynamics and environment structure are not analytically available. Existing methods typically either assume explicit models or learn task-specific behaviors, limiting zero-shot generalization to unseen STL tasks. In this work, we study offline STL planning under unknown dynamics using only task-agnostic trajectory data. Our central design philosophy is to separate logical reasoning from trajectory realization. We instantiate this idea in DAG-STL, a hierarchical framework that converts long-horizon STL planning into three stages. It first decomposes an STL formula into reachability and invariance progress conditions linked by shared timing constraints. It then allocates timed waypoints using learned reachability-time estimates. Finally, it synthesizes trajectories between these waypoints with a diffusion-based generator. This decomposition--allocation--generation pipeline reduces global planning to shorter, better-supported subproblems. To bridge the gap between planning-level correctness and execution-level feasibility, we further introduce a rollout-free dynamic consistency metric, an anytime refinement search procedure for improving multiple allocation hypotheses under finite budgets, and a hierarchical online replanning mechanism for execution-time recovery. Experiments in Maze2D, OGBench AntMaze, and the Cube domain show that DAG-STL substantially outperforms direct robustness-guided diffusion on complex long-horizon STL tasks and generalizes across navigation and manipulation settings. In a custom environment with an optimization-based reference, DAG-STL recovers most model-solvable tasks while retaining a clear computational advantage over direct optimization based on the explicit system model.

Ruijia Liu, Ancheng Hou, Xiao Yu +1

School of Automation and Intelligent Sensing, Shanghai Jiao Tong University, Shanghai, China. · Institute of Artificial Intelligence, Xiamen University, Xiamen, China.

Constrained Motion Planning Trajectory Generation Robot Motion Planning

Apr 19, 2026cs.RO

Planning Smooth and Safe Control Laws for a Unicycle Robot Among Obstacles

This paper presents a framework for safe navigation of a unicycle point robot to a goal position in an environment populated with obstacles from almost any admissible state, considering input limits. We introduce a novel QP formulation to create a Cinfinity-smooth vector field with reduced total bending and total turning. Then we design an analytic, non-linear feedback controller that inherently satisfies the conditions of Nagumo's theorem, ensuring forward invariance of the safe set without requiring any online optimization. We have demonstrated that our controller, even under hard input limits, safely converges to the goal position. Simulations confirm the effectiveness of the proposed framework, resulting in a twice faster arrival time with over 50% lower angular control effort compared to the baseline.

Aref Amiri, Basak Sakcak, Steven M. LaValle

Faculty of Information Technology and Electrical Engineering, University of Oulu · Department of Advanced Computing Sciences, Maastricht University

Robotic Control Constrained Motion Planning Safe Robot Navigation Robot Safety Feedback Control Quadratic Programming+1

Apr 17, 2026cs.RO

Contact-Aware Planning and Control of Continuum Robots in Highly Constrained Environments

Continuum robots are well suited for navigating confined and fragile environments, such as vascular or endoluminal anatomy, where contact with surrounding structures is often unavoidable. While controlled contact can assist motion, unfavorable contact can degrade controllability, induce kinematic singularities, or introduce safety risks. We present a contact-aware planning approach that evaluates contact quality, penalizing hazardous interactions, while permitting benign contact. The planner produces kinematically feasible trajectories and contact-aware Jacobians which can be used for closed-loop control in hardware experiments. We validate the approach by testing the integrated system (planning, control, and mechanical design) on anatomical models from patient scans. The planner generates effective plans for three common anatomical environments, and, in all hardware trials, the continuum robot was able to reach the target while avoiding dangerous tip contact (100% success). Mean tracking errors were 1.9 +/- 0.5 mm, 1.2 +/- 0.1 mm, and 1.7 +/- 0.2 mm across the three different environments. Ablation studies showed that penalizing end-of-continuum-segment (ECS) contact improved manipulability and prevented hardware failures. Overall, this work enables reliable, contact-aware navigation in highly constrained environments.

Aedan Mangan, Kehan Long, Ki Myung Brian Lee +3

Department of Mechanical and Aerospace Engineering, University of California San Diego, San Diego, CA 92093, USA · SceniX Inc, New York, NY 10011, USA · Department of Electrical and Computer Engineering, University of California San Diego, San Diego, CA 92093, USA +1

Constrained Motion Planning Continuum Robot Control Motion Planning Robot Motion Planning Surgical Robotics

Apr 3, 2026cs.RO

Goal-Conditioned Neural ODEs with Guaranteed Safety and Stability for Learning-Based All-Pairs Motion Planning

This paper presents a learning-based approach for all-pairs motion planning, where the initial and goal states are allowed to be arbitrary points in a safe set. We construct smooth goal-conditioned neural ordinary differential equations (neural ODEs) via bi-Lipschitz diffeomorphisms. Theoretical results show that the proposed model can provide guarantees of global exponential stability and safety (safe set forward invariance) regardless of goal location. Moreover, explicit bounds on convergence rate, tracking error, and vector field magnitude are established. Our approach admits a tractable learning implementation using bi-Lipschitz neural networks and can incorporate demonstration data. We illustrate the effectiveness of the proposed method on a 2D corridor navigation task.

Dechuan Liu, Ruigang Wang, Ian R. Manchester

Australian Centre for Robotics (ACFR), and the School of Aerospace, Mechanical and Mechatronic Engineering, The University of Sydney, Sydney, NSW 2006, Australia

Constrained Motion Planning Safe Motion Planning Motion Planning Neural ODEs Robot Motion Planning

Mar 17, 2026cs.RO

Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness

Motion planning under dynamics constraints, i.e, kinodynamic planning, enables safe robot operation by generating dynamically feasible trajectories that the robot can accurately track. For high-DOF robots such as manipulators, sampling-based motion planners are commonly used, especially for complex tasks in cluttered environments. However, enforcing constraints on robot dynamics in such planners requires solving either challenging two-point boundary value problems (BVPs) or propagating robot dynamics, both of which cause computational bottlenecks that drastically increase planning times. Meanwhile, recent efforts have shown that sampling-based motion planners can generate plans in microseconds using parallelization, but are limited to geometric paths. This paper develops FLASK, a fast parallelized sampling-based kinodynamic motion planning framework for a broad class of differentially flat robot systems, including manipulators, ground and aerial vehicles, and more. Differential flatness allows us to transform the motion planning problem from the original state space to a flat output space, where an analytical time-parameterized solution of the BVP problem can be obtained. A trajectory in the flat output space is then converted back to a closed-form dynamically feasible trajectory in the original state space, enabling fast validation via ``single instruction, multiple data" parallelism. Our framework is fast, exact, and compatible with any sampling-based motion planner, while offering theoretical guarantees on probabilistic exhaustivity and asymptotic optimality based on the closed-form BVP solutions. We extensively verify the effectiveness of our approach in both simulated benchmarks and real experiments with cluttered and dynamic environments, requiring mere microseconds to milliseconds of planning time.

Thai Duong, Clayton W. Ramsey, Zachary Kingston +2

Department of Computer Science, Rice University, Houston, TX, 77005, USA

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Sampling-Based Motion Planning

Mar 11, 2026cs.RO

PC-Diffuser: Path-Consistent Capsule CBF Safety Filtering for Diffusion-Based Trajectory Planner

Autonomous driving in complex traffic requires planners that generalize beyond hand-crafted rules, motivating data-driven approaches that learn behavior from expert demonstrations. Diffusion-based trajectory planners have recently shown strong closed-loop performance by iteratively denoising a full-horizon plan, but they remain difficult to certify and can fail catastrophically in rare or out-of-distribution scenarios. To address this challenge, we present PC-Diffuser, a safety augmentation framework that embeds a certifiable, path-consistent barrier-function structure directly into the denoising loop of diffusion planning. The key idea is to make safety an intrinsic part of trajectory generation rather than a post-hoc fix: we enforce forward invariance along the rollout while preserving the diffusion model's intended path geometry. Specifically, PC-Diffuser (i) evaluates collision risk using a capsule-distance barrier function that better reflects vehicle geometry and reduces unnecessary conservativeness, (ii) converts denoised waypoints into dynamically feasible motion under a kinematic bicycle model, and (iii) applies a path-consistent safety filter that eliminates residual constraint violations without geometric distortion, so the corrected plan remains close to the learned distribution. By injecting these safety-consistent corrections at every denoising step and feeding the refined trajectory back into the diffusion process, PC-Diffuser enables iterative, context-aware safeguarding instead of post-hoc repair...

Eugene Ku, Yiwei Lyu

Department of Computer Science and Engineering, Texas A&M University, College Station, TX, USA

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Control Barrier Functions Safety Filtering Safe Motion Planning+1

Feb 28, 2026cs.RO

Online Generation of Collision-Free Trajectories in Dynamic Environments

In this paper, we present an online method for converting an arbitrary geometric path, represented by a sequence of states, and generated by any planner (e.g., sampling-based planners such as RRT or PRM, search-based planners such as ARA*, etc.), into a kinematically feasible, jerk-limited trajectory. The method generates a sequence of quintic/quartic splines that can be discretized at a user-specified control rate and streamed to a low-level robot controller. Our approach enables real-time adaptation to environmental changes and can be re-invoked at any instant to generate a new trajectory from the robot's current state to a desired target state or sequence of states. Under a bounded-obstacle-velocity assumption, the method provides conditional stopping-safety guarantees over a finite time interval in dynamic environments, while allowing bounded geometric deviation from the original path. Kinematic constraints, including jerk limits, are explicitly considered. We validate the approach in a comparative simulation study against a competing method, demonstrating favorable behavior w.r.t. smoothness, computational time, and real-time performance, particularly with frequent target-state changes (up to 1 [kHz]). Real-robot experiments demonstrate applicability in real-world scenarios, including scenarios with a human as an obstacle.

Nermin Covic, Bakir Lacevic

Faculty of Electrical Engineering, University of Sarajevo, Bosnia and Herzegovina

Constrained Motion Planning Trajectory Optimization Trajectory Generation Robot Motion Generation Safe Motion Planning Dynamic Obstacle Avoidance+1

Feb 2, 2026cs.RO

Kino-PAX $^+$ : Near-Optimal Massively Parallel Kinodynamic Sampling-based Motion Planner

Sampling-based motion planners (SBMPs) are widely used for robot motion planning with complex kinodynamic constraints in high-dimensional spaces, yet their serial computation design results in planning speeds that scale poorly with problem complexity. Recent efforts to parallelize SBMPs have achieved significant speedups in finding feasible solutions; however, they provide no guarantees of optimizing an objective function. We introduce Kino-PAX

^{+}

, a massively parallel kinodynamic SBMP with asymptotic near-optimal guarantees. Kino-PAX

^{+}

builds a sparse tree of dynamically feasible trajectories by decomposing traditionally serial operations into three massively parallel subroutines. The algorithm focuses computation on the most promising nodes within local neighborhoods for propagation and refinement, enabling rapid improvement of solution cost. We prove that, while maintaining probabilistic

δ

-robust completeness, this focus on promising nodes ensures asymptotic

δ

-robust near-optimality. Our results show that Kino-PAX

^{+}

finds solutions up to three orders of magnitude faster than existing serial methods and achieves lower solution costs

Nicolas Perrault, Qi Heng Ho, Morteza Lahijanian

Department of Aerospace Engineering Sciences, University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering, Virginia Tech, VA, USA

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Sampling-Based Motion Planning

Nov 10, 2025cs.RO

Continuous-Space Roadmap Generation for Mobile Robot Fleets with Distance Constraints and Geometry-Aware Discretization

Efficient routing of mobile robot fleets requires roadmaps with high redundancy, short path lengths, and sufficient node and edge clearance for conflict-free operation. Existing grid-based methods sacrifice geometric fidelity and impose Manhattan-distance path length constraints, whereas existing continuous-space methods neglect minimum distance constraints and transport demand. This paper proposes a continuous-space roadmap generation method that addresses this gap by placing nodes at convex corner points of the free space and at station interaction points, discretizing free space via local grid expansion, enforcing minimum inter-node and node-edge distance constraints derived from robot dimensions, and applying transport demand-driven K-shortest path pruning. The method is evaluated across three intralogistics environments using two multi-agent pickup and delivery (MAPD) solvers against three baselines: a reaction-diffusion sampling method (GSRM), an 8-connected grid, and random sampling. Under Priority Inheritance with Backtracking (PIBT), the proposed method outperforms GSRM by 1.2-23.4 % at maximum fleet size, the grid by at least 9.1 %, and random sampling by more than 10.4 % across all environments, with a space-time A* solver confirming these results. It further attains near-optimal normalized path lengths of 1.03-1.05 and the highest inter-station connectivity at comparable roadmap complexity.

Marvin Rüdt, Constantin Enke, Kai Furmans

Institute for Material Handling and Logistics, Karlsruhe Institute of Technology, Karlsruhe, Germany.

Multi-Robot Motion Planning Constrained Motion Planning Roadmap-Based Motion Planning Motion Planning Multi-Agent Path Finding

Nov 5, 2025cs.RO

Manifold-constrained Hamilton-Jacobi Reachability Learning for Decentralized Multi-Agent Motion Planning

Safe multi-agent motion planning (MAMP) under task-induced constraints is a critical challenge in robotics. Many real-world scenarios require robots to navigate dynamic environments while adhering to manifold constraints imposed by tasks. For example, service robots must carry cups upright while avoiding collisions with humans or other robots. Despite recent advances in decentralized MAMP for high-dimensional systems, incorporating manifold constraints remains difficult. To address this, we propose a manifold-constrained Hamilton-Jacobi reachability (HJR) learning framework for decentralized MAMP. Our method solves HJR problems under manifold constraints to capture task-aware safety conditions, which are then integrated into a decentralized trajectory optimization planner. This enables robots to generate motion plans that are both safe and task-feasible without requiring assumptions about other agents' policies. Our approach generalizes across diverse manifold-constrained tasks and scales effectively to high-dimensional multi-agent manipulation problems. Experiments show that our method outperforms existing constrained motion planners and operates at speeds suitable for real-world applications. Video demonstrations are available at https://youtu.be/RYcEHMnPTH8 .

Qingyi Chen, Ruiqi Ni, Junyoung Kim +1

Department of Computer Science at Purdue University, West Lafayette, IN 47907, USA.

Multi-Robot Motion Planning Constrained Motion Planning Reachability Analysis Safe Motion Planning Motion Planning