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