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

Oct 7, 2026cs.RO

Making Task Abstractions Executable: Control-Aware Layout Repair for a Fixed Controller

A task abstraction can specify the intended events while its spatial layout prevents a fixed agent and controller from completing them. Starting from a supplied structured task record, we compile whole-task tracking, clearance, and actuation requirements into auditable affine layout constraints. We repair only declared continuous coordinates, preserving event order, timing, topology, and the controller. A most-violated-row update admits conditional finite-certification and net-displacement bounds; a same-compiler quadratic projection separates the representation from the optimizer. On three researcher-authored task abstractions, both backends certify all three layouts and complete all 300 fresh paired rollouts per backend. A risk-target sweep also exposes fixed event tests that the chosen certificate cannot satisfy through layout edits alone.

Chiyoung Kim, Seungyeon Back, Sumin Shim +2

Chung-Ang University · Hanyang University · Carnegie Mellon University +1

Robot Task Planning Constrained Motion Planning Robot Motion Planning

Oct 7, 2026cs.RO

Distributed Motion Planning for Multi-Robot Systems under Topological Constraints

Efficient and distributed coordination of mobile robots is one of the main challenges in multi-robot systems. Topological constraints, often expressed as topological braids, are a popular tool to encode complex coordination patterns between multiple mobile robots, as they offer a compact and abstract representation of the desired qualitative relation between the space-time trajectories of the robots. However, execution of joint motion plans encoded as braid-based topological constraints via distributed controllers is challenging, with existing approaches, generally based on the execution of one braid generator at a time, producing slow and suboptimal trajectories. We propose a distributed controller based on Model Predictive Control (MPC) to efficiently execute braid-based topological specifications. Rather than directly tracking the braid specification, we propose to use winding numbers, which are topological invariants for braids, as a proxy. This has the twofold benefit of converting braids into a continuous function, which can be easily tracked by an MPC controller through an appropriate term in the cost function, and of decoupling the global braid specification into a set of pairwise specifications, which can be tracked distributedly through the solution of only local MPC problems. To maintain global coordination, we propose a consensus-based progress estimation approach, which allows the robots to synchronize their motion toward the desired specification. We validate the proposed approach in simulation and in real-world experiments, where we demonstrate the effectiveness of the proposed approach and the improvement over existing approaches in terms of execution speed and control effort.

Gianpietro Battocletti, Dimitris Boskos, Dimos V. Dimarogonas +1

Delft Center for Systems and Control, Delft University of Technology, Delft, The Netherlands. · Division of Decision And Control Systems, KTH Royal Institute of Technology, Stockholm, Sweden.

Multi-Robot Motion Planning Multi-Robot Coordination Motion Planning Constrained Motion Planning

Oct 6, 2026cs.RO

NMPP: Nonlinear Model Predictive Planning for Agile UAV Flight in Cluttered Environments

Flying a quadrotor through a cluttered environment requires not only planning a collision-free reference trajectory based on perceived obstacles, but the reference also needs to be dynamically feasible and within the actuation limits of the vehicle, so that the controller can track it precisely. Existing methods either optimize a smooth polynomial inside a convex corridor, which limits agility, or treat obstacles as soft costs traded against tracking performance. We propose a Nonlinear Model Predictive Planning (NMPP) that imposes perceived obstacles as hard geometric constraints and hands a full-state reference to an obstacle-blind SE(3) controller. Our planner achieves a 58-67 % lower position RMSE than a linear Model Predictive Control trajectory planner and a 41-70 % lower RMSE than a polynomial trajectory planner. It also completes all forest flights with up to 9.5 m/s speed without collisions, and achieves 86 % flight success rate under a more aggressive speed profile where a state-of-the-art planner has only 26 % success rate. The real-world deployment showed reliable execution flying up to 5.5 m/s in an unknown cluttered environment.

Ondřej Procházka, Juraj Pauko, Robert Pěnička +1

Multi-robot Systems Group, Faculty of Electrical Engineering, Czech Technical University in Prague, Czech Republic

Constrained Motion Planning Nonlinear MPC UAV Navigation UAV Trajectory Optimization

Oct 6, 2026cs.RO

Model-Based Geometry-Aware Generative Optimization for Constrained Locomotion Planning

Constrained Locomotion Planning (CLP) for quadrupeds and humanoids, where robots must satisfy collision avoidance, contact consistency, kinematic feasibility, and support constraints, is challenging under high-dimensional dynamics and highly non-convex environments. Recent Model-Based Diffusion (MBD) approaches recast trajectory optimization as posterior sampling over trajectories, using known dynamics and Monte Carlo rollouts to analytically estimate the denoising score function without demonstration learning. While constrained variants further incorporate feasibility into model-based score rollouts and show promising performance, they are still limited by (1) lacking a task-modulated active constraint geometry that shapes the score direction and reverse stochasticity, and (2) using deterministic DDPM-style reverse transport without adaptive scheduling across different generative transports. Therefore, we introduce Model-Based Geometry-Aware Generative Optimization (2GO) for constrained locomotion, which turns active constraint geometry into executable denoising operators through normal- induced metric shaping, tangent-space stochastic filtering, and CFS-based retraction. 2GO further decouples generative transport from reverse stochasticity through an adaptive diffusion and flow-like schedule. Experiments on constrained quadruped and humanoid locomotion demonstrate strong performance in discrete foothold selection and continuous posture planning, with higher success rates, fewer violations, and improved execution compatibility.

Zhilin He, Xinyuan Wang, Changliu Liu

Carnegie Mellon University · University of Michigan, Ann Arbor

Constrained Generative Modeling Constrained Motion Planning Humanoid Robot Locomotion Diffusion-Based Planning Model-Based Planning Quadruped Locomotion

Oct 5, 2026cs.RO

P3: Persistent Particle Planning for Constrained Diffusion Control

Diffusion models provide expressive priors over trajectories, but adapting these priors to test-time constraints requires maintaining feasibility and consistency across successive control decisions. We introduce Persistent Particle Planning (P3), a sequential Monte Carlo framework for diffusion control that maintains a weighted population of candidate plans across replanning steps. At each control step, P3 shifts and partially re-noises the candidate trajectories, refines them under the latest observation, and uses constraint-aware weighting and resampling to select among alternative continuations without retraining the diffusion model. We consider denoising and replanning as one Feynman--Kac particle system and analyze it under an idealized repair. We prove that the re-noising depth controls how reliably a kept plan stays on its route, and that keeping a rare, well-separated route takes far fewer plans than rediscovering it by sampling from scratch. Experiments under multiple test-time constraint configurations show that population reuse reduces route switching and improves success without constraint violations. Because P3 refines earlier plans instead of redrawing them, it also needs fewer denoising iterations per replan. On maze-navigation tasks, it plans faster than both regenerated populations and methods that correct a single sampled plan by constrained optimization. Code and pretrained models are available at https://github.com/p3-username/p3-anon.

Hikmet Simsir, Mahyar Fardinfar, Ozgur S. Oguz

Department of Computer Engineering, Bilkent University

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning

Oct 5, 2026cs.RO

Dynamics-Aware Adaptive Corridors with Feasibility-Perturbed Trust-Region SQP for Certified Nonholonomic Motion Planning

Optimisation-based parking planners usually impose collision constraints only at the time samples, so a vehicle corner can cut an obstacle between samples, and no executable trajectory exists until the solver converges. We present a planner for car-like vehicles with reverse gear in which every iterate of the optimisation phase satisfies the discretised dynamics exactly and keeps the whole vehicle rectangle clear of obstacles between the samples. Each time interval receives one convex corridor that holds all vehicle corners at both ends and is shrunk by a sweep margin bounding how far the corner paths leave their chords. Separating half-planes give the corridors a direction out of obstacles when the initial guess is in collision; later, heading-aligned boxes are grown from the speed, curvature and step of the current iterate and rebuilt after accepted steps. A feasibility-perturbed trust-region sequential quadratic programming method projects each step onto the dynamics by feedback and verifies it exactly; the cost decreases monotonically, and once the corridors stop changing, limit points are Karush-Kuhn-Tucker points of the corridor-constrained problem or violate a constraint qualification. On 820 benchmark cases the planner succeeds in 818 without penetration (797 from the first initial guess), none of the 7103 evaluated optimisation-phase iterates is unusable, and it succeeds in 96.5% of the cases when 99% of the initial guesses intersect an obstacle. Its maneuvers take 0.7% longer in the median than those of a similarly certified exact-collision baseline. The guarantees hold for the planning model, not for a physical vehicle.

Yang Shi

Jiangsu College of Safety Technology, Jiangsu, China

Constrained Motion Planning Trajectory Generation Autonomous Parking Trust-Region Optimization Safe Motion Planning Motion Planning

Oct 1, 2026cs.RO

Training-Free Diffusion Planning with Analytical Local Scores

Path finding and multi-robot motion planning require trajectories that are smooth, goal-directed, and collision-free in environments with complex geometric constraints. Recent diffusion-based planners have shown that trajectory generation can be cast as iterative denoising which has opened the doors to learning-based approaches that can handle multi-modal trajectory distributions and refine entire trajectories. However, a key limitation is that diffusion planners require training on large collections of feasible trajectories, rendering them map-specific, and difficult to deploy when high-quality demonstrations are unavailable. This paper introduces a training-free diffusion-based motion planner that replaces learned global trajectory scores with analytical local scores derived from obstacle, smoothness, velocity, and inter-agent feasibility terms. The proposed idea relies on a key observation: the score of a trajectory can be reconstructed by considering only local interactions between neighboring waypoints and nearby constraints. This structure exploitation yields a decomposed denoising procedure that retains the optimization structure of classical trajectory methods while inheriting the iterative refinement behavior of diffusion models. Experiments on a large collection of complex environments and large multi-agent planning tasks show that the proposed analytical score produces smooth and feasible trajectories within limited computational costs, for example in generating feasible paths for 300+ agents in environments containing 100+ obstacles in under 6 seconds on a GPU, outperforming strong learning-based and optimization baselines, while avoiding the data requirements of learned diffusion planners.

Michael Y. Fatemi, Jinhao Liang, Ferdinando Fioretto

University of Virginia

Multi-Robot Motion Planning Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Multi-Agent Path Finding

Oct 1, 2026cs.RO

Cross-entropy optimization with prioritized constraints

When constraints conflict, an optimizer must determine which requirements to preserve and which to relax. On the one hand, a priority ordering specifies which requirements take precedence. On the other hand, penalty-based formulations encode their relative importance through numerical weights. Depending on these weights, a solution can improve its weighted score while violating intended priorities. We introduce TierCEM, a variant of the cross-entropy method that incorporates strict constraint priorities directly into elite selection without requiring per-constraint importance weights. TierCEM works by sequentially filtering sampled candidates, from highest- to lowest-priority constraint. If and when a constraint eliminates all remaining candidates, TierCEM returns to the last nonempty set and selects elites with the smallest violations of that blocking constraint, recursively preserving satisfaction of all higher-priority constraints. We evaluate TierCEM on 2D navigation and contact-rich pushing tasks in proprioceptive and learned world-model settings. Experiments show that reversing the constraint ordering changes which constraints are violated under conflict. Prioritizing progress toward the task objective also enables TierCEM to relax lower-priority constraints when they would otherwise prevent further progress.

Francisco Roldan Sanchez, Pau de las Heras Molins, David Fridovich-Keil +1

LTCI, Télécom Paris, Institut Polytechnique de Paris · The University of Texas at Austin

Constrained Motion Planning Black-Box Optimization Constrained Optimization Multi-Objective Optimization

Sep 29, 2026cs.RO

Executor-aware Candidate Selection via a Feasibility Certificate

Modular robotic systems often separate motion planning from a downstream executor that enforces state-dependent hard constraints. A candidate that is geometrically valid may therefore be incompatible with the executor's available command set. We present a certificate-based candidate-selection framework that constructs a command witness from the executor hard set at predicted rollout states and verifies it against the original constraints, without changing candidate generation, ranking, or the executor. Across 5,085 geometry-valid numerical evaluations on two robot models, 795 admitted no executor-feasible command. The certificate is sufficient but conservative: none of the 795 was certified, while 7.09% of reference-feasible cases remained uncertified. In controlled FR3 and fixed-base RB-Y1 simulations, certificate admission frequently changed candidate selection, and a post-hoc exact linear-programming (LP) admission baseline revealed platform-dependent conservatism. Relative to geometry-based selection, certificate admission was associated with lower planner-command coverage and higher nominal tracking error, without a consistent advantage in reached-state interaction reserve. A planner-generated MoveIt/OMPL study further evaluates the same admission rule on externally generated candidate pools.

Sooin Choi, Soonwoong Hwang, Wansoo Kim

Department of Robotics, Hanyang University, Seoul, Republic of Korea · Department of Robotics, Hanyang University ERICA, Ansan, Republic of Korea

Constrained Motion Planning Modular Robotics Motion Planning Robot Motion Planning

Sep 24, 2026cs.RO

ReVAMP: Vector-Accelerated Motion Planning for Kinematically-Constrained Systems via Reparameterization

Robots often must satisfy one or more constraints during motion planning for real-world tasks. When such constraints reduce the valid configuration space to a measure-zero subset, sampling based planning algorithms require modifications to draw feasible samples. For many common end-effector constraints, parameterizations built on inverse kinematics (IK) provide an alternate formulation where the constraints are satisfied by construction, allowing directly sampling the feasible set. Despite their elegant approach, parameterized planners have remained slower than vector-accelerated implementations of projection-based approaches, leaving their performance ceiling an open question. We explore a new axis of vectorization built upon reparameterizing the planning space through analytic IK. This approach addresses existing inefficiencies in vectorized projection-based planners and exposes new opportunities for parallelism within the planner. We show that the planner can synthesize plans in microseconds to milliseconds for high dimensional systems (up to 20 dimensions), with complex constraints, up to 10x faster than the current state-of-the-art. Furthermore, we demonstrate how such planning speeds open up avenues for restructuring sequential manipulation pipelines.

Shrutheesh R. Iyer, Thomas Cohn, Zachary Kingston

Department of Computer Science, Purdue University · Department of Electrical Engineering and Computer Science, Massachusetts Institute of Technology

Constrained Motion Planning Robotic Manipulation Motion Planning Sampling-Based Motion Planning

Sep 24, 2026cs.RO

CAMP: Cooperative Arm-Hand Motion Planning in Constrained Spaces

Coordinated arm-hand motion planning is fundamental to dexterous robotic manipulation in complex and constrained environments. A straightforward solution is to decompose the problem into separate arm path planning and hand motion generation; however, this poses a dilemma: decomposition can miss feasible solutions that require coordinated arm-hand adaptation along the path. Alternatively, directly planning in the high-dimensional joint arm-hand configuration space captures such coupling but faces a substantially enlarged search space and nonconvex collision constraints. To characterize this coupling, we formulate feasible hand fibers that capture collision-free hand configurations for each arm configuration. Based on this formulation, we propose CAMP, a high-success and efficient cooperative arm-hand motion planner for constrained environments. CAMP constructs candidate trajectories through layered hand search with local arm relaxation, then compactly represents them using endpoint-preserving via-point movement primitives (VMPs) for coarse-to-fine joint optimization. Across six constrained simulation tasks, CAMP achieves 84.2-98.5% planning success, outperforming alternative planners with competitive efficiency. Ablation studies verify the contributions of arm relaxation, VMP representation, and coarse-to-fine optimization, while real-robot experiments demonstrate CAMP on constrained manipulation tasks. The project website is available at https://camp-armhand.github.io/.

Ziyuan Wang, Yunlong Shan, Fei Mo +6

Department of Mechanical Engineering and Automation, Harbin Institute of Technology, Shenzhen, China · School of Advanced Engineering, Great Bay University, Dongguan, Guangdong, China · State Key Laboratory for Multi-target Natural Medicine, China Pharmaceutical University, Nanjing, China +4

Constrained Motion Planning Robot Motion Generation Robotic Manipulation

Sep 22, 2026cs.RO

Induced Riemannian Metrics for Motion Planning with Constraints

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

Phone Thiha Kyaw, Thomas Cohn, Miguel Angel Rogel Garcia +1

Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory at the University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Ontario, Canada, M3H 5T6

Constrained Motion Planning Trajectory Optimization Motion Planning Sampling-Based Motion Planning

Sep 21, 2026cs.RO

Minimum Time Trajectories for a Car-Like Mobile Robot Moving with Rigid Wheels Under Non-Sliding Constraints

This paper studies the minimum time trajectoriesvof a car-like mobile robot navigating in an obstacle free environment. The robot, with forward and backward speeds, is controlled by bounded front-wheels acceleration and limited front-wheels steering rate. The paper extends previous results which solved this problem for the kinematic car-like robot. However, the kinematic model assumes pure rolling at the wheels ground contacts. This assumption requires non-sliding constraints for the front and rear wheels that can only be handled by the robot dynamics. This paper formulates the non-sliding constraints based on the robot dynamics then augments the kinematic model time-optimal path primitives with three new path primitives associated with the non-sliding constraints. The three non-sliding path primitives together with the kinematic model twelve path primitives form the car-like robot time optimal trajectories. Approximate analytic solutions for the non-sliding path primitives are also provided. Examples study the time-optimal path primitives along representative maneuvers, illustrating how the non-sliding constraints influence the time optimal trajectories of the car-like robot.

Joseph Ben-Asher, Elon Rimon, Leeor Ravina

Dept. of AE, Technion, Israel. · Dept. of ME, Technion, Israel.

Constrained Motion Planning Trajectory Optimization Trajectory Generation Vehicle Dynamics Motion Planning

Sep 21, 2026cs.RO

Feasibility Distance Fields for Heterogeneous Constraints in Robot Configuration Space

Robot manipulators are monitored by constraint-specific indicators whose units and gradient scales are not comparable, so they do not provide a common measure of the configuration-space motion remaining before violation. We define the feasibility distance field (FDF) as the distance, under a fixed positive-definite joint-space metric, to the union of infeasible configuration sets. Classical distance-to-set theory gives 1-Lipschitz continuity, almost-everywhere differentiability, and unit dual-gradient norm wherever the nearest projection is unique. The robotics contribution is an admissibility analysis showing when practical constraints define non-empty closed sets. We derive admissible formulations for external and self-collision, joint limits, dexterity, Cartesian and task-projected compliance, joint torque under payload, and dynamic manipulability. Since every field uses the same metric, heterogeneous constraints compose by a pointwise minimum, conditioned constraints retain a fixed distance space, and multi-robot constraints produce block-sparse gradients that identify which robots must react. We generate projection-based labels and train neural approximations with a distance loss and an Eikonal penalty. Simulations on a UR5e and a dual-arm cell evaluate seven fields using value, projection, sign, gradient, composition, and moving-obstacle diagnostics. Across 8,000 configurations, the largest feasible-side secant ratio is 0.920, mean learned gradient norms range from 0.994 to 0.998, and projection residuals range from 0.011 to 0.034 rad. Across 24 random obstacle paths, the external and composed collision fields achieve 90.4% and 91.6% success within 3 cm, with sign-error rates below 2%. The results support a common configuration-space margin and identify approximation errors near medial axes and sparsely sampled boundaries.

Xijing Cui, Huayan Pu, Jun Luo +1

First and third authors' affiliation · Second author's affiliation

Constrained Motion Planning Signed Distance Functions Safe Motion Planning

Sep 21, 2026cs.RO

From Semantic Decisions to Feasible Trajectories: Self-Evolving LLM-Guided Optimal Control for Narrow-Space Parking

Autonomous parking in nonconvex and narrow environments remains challenging. Although optimal-control methods can explicitly enforce vehicle dynamics and collision constraints, nonconvexity compromises solver robustness and can cause failures. Large language models (LLMs) exhibit strong semantic reasoning capabilities, but directly generating dense trajectories makes it difficult to guarantee physical feasibility. We introduce SE-LLM-OCP, a unified framework in which LLMs make high-level discrete maneuver decisions, while an optimal-control module enforces low-level vehicle dynamics and collision constraints. Online, the LLM proposes sparse maneuver plans, decomposing the parking task into a sequence of short-horizon trajectory-optimization problems. A low-level solver then sequentially solves optimal-control problems. If the solver fails, the LLM aggregates failure evidence from the solver and validation stages to guide replanning. Offline, SE-LLM-OCP automatically evolves a structured decision-making knowledge base from scratch, driven by accumulated online failures. We validate our proposed framework in simulation on a car-like vehicle model and on a differential-drive robot. Our experimental results show that SE-LLM-OCP enables safer autonomous parking in narrow scenarios and demonstrates transfer of the same maneuver representation to a different kinematic platform.

Zhengbao Yao, Yuanfu Luo, Kehan Xue

xLean Robotics Co., Ltd.

Constrained Motion Planning Trajectory Optimization Language Model-Based Control Autonomous Parking

Sep 21, 2026cs.MA

Mixed-integer flow formulations for motion planning and decision-making of networked multi-agent systems

This work investigates the use of flow-based connectivity maintenance constraints in mixed-integer linear programming (MILP) trajectory planning and decision-making models for networked multi-agent systems (MAS). We integrate flow-based encodings for standard and k-hop connectivity into MILP multi-vehicle maneuvering models that are widely used alongside receding horizon planning strategies. Their necessity and sufficiency is demonstrated, guaranteeing full coverage of potential network topologies. The flow formulation for standard connectivity decreases the growth of the required inequality constraints from exponential to polynomial w.r.t. the size of the MAS when compared to the state-of-the-art subtour elimination (SEC) method. The flow-based k-hop connectivity constraints decrease the number of required binary variables and decouple its growth from the number of hops. However, the impact of these formulations in performance is not straightforward due to the introduction of a substantial number of continuous flow optimization variables and, in the case of k-hop connectivity, additional inequality constraints. We investigate this trade-off through a statistical evaluation of costs and optimization times using a conventional branch-and-bound commercial solver and trials performed with randomized environments for increasingly larger MAS. The results show that the flow formulation outperforms SEC in standard connectivity problems, enabling the solutions to be computed for larger MAS considering the imposed optimization time limit. The reduction in number of binary variables enabled by the k-hop flow formulations decreases the theoretical worst-case number of iterations required by the branch-and-bound algorithm to compute the global optimal solution. Our results show that this advantage did not translate into improvements in the average performance when compared to the baseline.

Angelo Caregnato-Neto, Paul-Louis Delacour, Raf Van de Plas +2

Department of Computational Mechanics (DMC), School of Mechanical Engineering (FEM), State University of Campinas (UNICAMP), São Paulo, Brazil · Delft Center for Systems and Control, Delft University of Technology, Delft, Netherlands · Dept. of Biochemistry, Vanderbilt University, Nashville, TN, USA

Constrained Motion Planning Mixed-Integer Linear Programming Combinatorial Optimization Multi-Agent Systems Multi-Agent Planning

Sep 16, 2026cs.RO

M $^3$ P-R1: Reinforcement Learning for Large Language Model Guided Multi-Modal Motion Planning via MIP Code Generation

Multi-Modal Motion Planning (M

^3

P) requires joint reasoning over continuous motions and discrete mode transitions, making it difficult to solve efficiently. For instance, a bipedal robot may walk to a target location and then use its arms to grasp an object. This scenario captures both mode transitions and continuous dynamics, yielding feasible paths that neither purely discrete nor continuous planners can handle. While Mixed-Integer Programming (MIP) offers a principled framework, constructing tractable formulations for non-convex problems is typically manual and domain-specific, especially in the approximate, discretization-based MIP regime needed for non-convex robotic tasks. We propose M

^3

P-R1, a reinforcement learning method that fine-tunes large language models (LLMs) to decompose M

^3

P tasks into MIP variables, constraints, and objectives. Instead of directly outputting answers, which are often prone to hallucination, the model generates executable Python code using MIP optimization libraries and constraint interfaces. This enables solver-backed execution for robust and verifiable solutions. Trained with an outcome-driven reward against the solver, M

^3

P-R1 learns to compose modality-level discretization primitives and synthesize cross-modal coupling constraints, producing executable MIP programs for complex M

^3

P tasks.

Xingpeng Sun, Zherong Pan, Kai Cheng +3

Purdue University United States · LightSpeed Studios United States

Constrained Motion Planning Large Language Model-Based Robot Planning Combinatorial Optimization Program Synthesis LLM-Guided RL Motion Planning+2

Sep 15, 2026cs.RO

VCTP: Vehicle-Conditioned Terrain Planning for Off-Road Navigation

A vehicle's heading affects both the surfaces beneath its tires and its pitch and roll. We present Vehicle-Conditioned Terrain Planning (VCTP), which retains these relationships by evaluating shared elevation and surface-ID layers at eight headings. Body geometry constrains admissibility, while loaded wheel contacts determine modeled surface cost and predicted pitch and roll. Established D* Lite and vehicle-state search use these evaluations to plan routes with forward and reverse motion. When observations change, VCTP recomputes every affected body or contact query. In fully observed two-track simulations, sampling at wheel contacts rather than at the vehicle center lowers modeled surface cost by 39.3% while shortening the route. In offline planning on RGator recordings, VCTP also reduces modeled costs over identified surfaces and observed support relative to distance-focused planning, although incomplete coverage leaves full-route rankings unresolved. Selective updates match full recomputation in all 474 comparisons using recorded map changes. These results identify when wheel-contact placement and vehicle heading affect route choice.

Akshay Naik, Ramavarapu S. Sreenivas, Dustin Nottage +1

University of Illinois Urbana-Champaign, Urbana, IL, USA: 1Department of Electrical and Computer Engineering · University of Illinois Urbana-Champaign, Urbana, IL, USA: 2Department of Industrial and Enterprise Systems Engineering · U.S. Army Engineer Research and Development Center, Construction Engineering Research Laboratory, Champaign, IL, USA

Constrained Motion Planning Off-Road Robot Navigation Motion Planning Robot Motion Planning

Sep 11, 2026cs.RO

Language-Guided Terrain-Adaptive Neural MPC for Autonomous Traversal of Articulated Tracked Robots

In urban search and rescue, articulated tracked robots (ATRs) must traverse structured but contact-rich environments such as stairwells and cluttered building interiors. Reliable autonomy remains challenging because robot-terrain interaction (RTI) is hybrid and discontinuous, and effective flipper-track coordination is difficult to model analytically. We present ASTRIL-MPC, a language-guided neural kinematics model predictive control (MPC) framework for autonomous traversal. A learned kinematics model predicts short-horizon task-state increments from a height sequence and recent trajectories; NMPC plans with multi-objective costs and strict feasibility constraints; and a large language model (LLM) proposes bounded updates to selected weights and bounds through a safety-checked interface with range clipping, rate limiting, and consistency checks. The compiled predictor enables a full control cycle within 100 ms. Across three traversal tasks and a multi-height generalization setting, ASTRIL-MPC improves an aggregate traversal-quality score by up to 71% over a non-adaptive NMPC and by 67% over a PPO baseline, while eliminating measurable collision impacts during descent. These results indicate that combining terrain-conditioned neural kinematics, optimization-based planning, and language-guided adaptation yields data-efficient and robust autonomy for articulated tracked robots. Real-robot trials over four indoor obstacles further demonstrate transfer to contact-rich physical traversal.

Zhenfeng Gan, Yanbo Chen, Lirong Che +3

Constrained Motion Planning Language Model-Based Control Terrain-Aware Robot Locomotion Model Predictive Control Search-and-Rescue Robotics

Sep 11, 2026cs.RO

GeoTrussRover: Morphological Computation with Contact-Semantic Control Primitives

Reconfigurable robots can change their contact geometry when a fixed body cannot negotiate an obstacle. A variable-geometry truss (VGT) distributes this shape change through a load-bearing structure, but coupling it to a mobile base creates a high-dimensional coordination problem. GeoTrussRover combines an electrically actuated VGT, a wheeled base, and contact-semantic morphology planning and control. We solve one source traversal and extract four contact-semantic primitives that describe coordination among 21 members. Physics-constrained projection adapts them to unseen step heights with the same contact topology. When every phase remains feasible, adaptation does not recompute the complete motion. If one phase violates the new physical constraints, only that phase is recomputed. A full-space QP then tracks the adapted motion and corrects member and wheel errors. For transfer from 0.10m to 0.075m, the method reduces objective-function evaluations by 63.7% relative to full recomputation. Contact-phase feasibility analysis covers step heights from 0.10 to 0.46m, or 1.08 to 4.97 wheel radii, with the upper value near the theoretical feasible boundary. The electric prototype traverses 2.11 wheel radii. The resulting low-dimensional representation stores task coordination in a hyper-redundant, load-bearing morphology and reuses it during locomotion.

Muyuan Ma, Yi Zhang, Yang Yang +12

Constrained Motion Planning Robot Morphology-Control Co-Design Robotics Robot Motion Generation Terrain-Aware Robot Locomotion Robot Locomotion

Sep 11, 2026cs.RO

Planning along Differentiable Charts of Constraint Manifolds with General-Purpose IK Solvers

Planning trajectories for robot manipulators under kinematic equality constraints restricts feasible motions to a measure-zero submanifold of the configuration space, requiring special algorithmic treatment. A promising strategy is parametrizing the set of feasible configurations using analytic inverse kinematics (IK). Bespoke analytic IK functions can be written to be differentiable, a necessary property for gradient-based trajectory optimization. But the vast majority of IK functions are computed by automated meta-solvers like IKFast, and are difficult to modify for differentiability. We present a new approach for computing gradients of analytic IK parameterizations: we leverage the inverse function theorem to recover the desired gradients from the ordinary forward kinematic Jacobian. Furthermore, we present a least-squares domain extension and an optimization-amenable description of the reachability constraint, which preserves gradient signal outside the reachable workspace. We demonstrate the efficacy of our approach through numerical experiments and downstream tasks, including a hardware demonstration of an RB-Y1 picking up a box and placing it on a table. Project website: https://cohnt.github.io/inverse-function-theorem-parameterization/

Thomas Cohn, Seiji Shaw, Harel Biggie +3

Computer Science and Artificial Intelligence Laboratory (CSAIL), Massachusetts Institute of Technology, Cambridge, MA, USA

Constrained Motion Planning Trajectory Optimization Trajectory Generation Motion Planning Inverse Kinematics

Sep 9, 2026cs.RO

PccDiffuser: Multi-solution Motion Planning for Continuum Robots

We present the PccDiffuser, a conditional diffusion framework for continuum robots that learns a multimodal distribution over complete configuration-space paths and samples multiple candidate solutions in parallel, which are subsequently converted into an executable trajectory by time allocation considering actuator constraints. Under the piecewise constant-curvature model, we use exponential co-ordinates to describe the robot kinematics, and use graph neural network to encode a variable number of environment obstacles. Analytical differential kinematics is incorporated in the denoising process to improve terminal accuracy and whole-body clearance. On a mixed test set comprising workspace with zero to four obstacles, PccDiffuser achieved a success rate of 91%. Compared with existing sampling- and optimisation-based benchmarks, it delivered both a higher success rate and greater computational efficiency, with the latter advantage becoming more substantial when sampling more candidate solutions. Experiments on a three-section tendon-driven continuum robot further demonstrate consecutive planning, multi-solution planning, and whole-body obstacle avoidance.

Ke Qiu, Sifan Chen, Si Wang +3

Department of Control Science and Engineering, Zhejiang University, Hangzhou, China

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

Aug 30, 2026cs.RO

Sampling-based Certified Planning with Graphs of Convex Sets

Planners on graphs of convex sets return trajectories that are collision-free by construction, provided the convex regions are collision-free. The region generator only promises that property probabilistically, and no planner in the family verifies it. We report the first measurement of what the gap costs. On a scaled 14-DOF bimanual library,

3.2\%

of interface samples are in collision, and a search-based GCS planner (\gcsstar) turns that volume error into a

62\%

answer error:

18

of

29

pick-and-place queries return trajectories that drive the arms through the shelves, up to

91

,mm deep, reported as successes. Repairing the library does not work; a ten times stricter acceptance contract, sums-of-squares certified regions, and uniform margins each destroy the connectivity planning needs before they deliver soundness. We instead build a planner that certifies its answers. It samples the overlaps and shared faces of the decomposition, prunes with an admissible informed bound, and verifies the one candidate each search round proposes, continuously, by a chain of clearance certificate balls with no resolution parameter; failures are repaired with local in-region detours, and the convex polish is re-verified. Head-to-head on all

29

task queries it delivers zero invalid answers against

21

for the reference, reaches its first certified answer in

0.11

,s against

1.59

,s for the reference's unverified one, and reproduces the reference optimum exactly on every query whose reference answer is physically valid.

Peng Xie, Amr Alanwar

TUM School of Computation, Information and Technology, Department of Computer Engineering, Technical University of Munich, 74076 Heilbronn, Germany

Constrained Motion Planning Robotic Manipulation Safe Motion Planning Motion Planning Sampling-Based Motion Planning

Aug 16, 2026cs.RO

Contact Modes Are Strata: What Geometric Structure Buys in Discrete-Continuous Planning

Contact-rich manipulation poses a discrete question and a continuous one at once, namely which contacts are active and how to move while they hold. The two are coupled by a change of dimension, since each contact that a robot maintains confines its motion to a lower-dimensional manifold. We make that coupling the explicit object of planning by observing that a contact mode is not merely analogous to a stratum of the configuration space; it is one. A plan is then a walk over strata whose within-stratum segments are geodesics. On two contact-rich manipulation tasks in simulation, pushing a T-shaped block around obstacles and reorienting a cube in a dexterous hand, our planner returns solutions within seconds with no mode, contact sequence, or stratum given in advance.

Phone Thiha Kyaw, Jonathan Kelly

Space and Terrestrial Autonomous Robotic Systems (STARS) Laboratory University of Toronto Institute for Aerospace Studies (UTIAS) Toronto, Ontario M3H 5T6, Canada

Constrained Motion Planning Contact-Rich Robotic Manipulation Motion Planning

Aug 15, 2026cs.RO

Accelerating Optimization over Graphs of Convex Sets via Neural Network Approximations

Motion planning problems such as collision-free navigation and contact-rich manipulation can be naturally formulated as optimization problems that couple discrete decisions with continuous trajectories. The Graphs of Convex Sets (GCS) framework offers a practical solution to these problems. It represents discrete decisions as nodes of a graph and encodes continuous trajectories in the edges connecting them. However, the resulting optimization subproblems can become computationally prohibitive for online replanning. In this work, we propose a learning-based strategy to mitigate this limitation. Specifically, we replace the costly convex relaxation step required by nominal GCS with a single forward pass through a Graph Attention Network that predicts a set of highly probable candidate paths through the graph. A lightweight ranking network then orders these candidates by their estimated trajectory cost. Evaluating them in this order, we terminate our search early while still recovering a near-optimal motion plan. We validate the resulting pipeline across diverse robotic tasks, including collision-free motion planning for a 3D quadrotor and a 7-DoF manipulator, and planning through contact for planar pushing. Across both convex and non-convex cost and constraint settings, our approach yields up to two orders of magnitude speedup over nominal GCS while maintaining a 100% success rate, at the cost of some suboptimality in the recovered solutions. Code implementations and video demonstrations can be found at https://neural-gcs.github.io/.

Ananya Trivedi, Sarvesh Prajapati, Zhexin Xu +3

Northeastern University, Boston, Massachusetts, USA. · University of Maryland, College Park, Maryland, USA. · Northeastern University

Constrained Motion Planning Trajectory Optimization Graph-Based Planning Robot Motion Planning

Aug 10, 2026cs.RO

Whole-Body Planning for Humanoids Navigating Confined Spaces via Self-Collision Avoidance References

Humanoid locomotion in highly confined environments requires navigating dense environmental obstacles and complex self-collision bounds while maintaining multi-contact dynamic feasibility. Traditional trajectory optimizers frequently struggle in these restricted spaces, as navigating the large collision space with splines on particle abstractions is insufficient and leads to poor local minima. To address this, we propose a three-stage whole-body planning framework that formulates kinematic path planning directly over kinematically reachable rigid-body volumes. By integrating differentiable collision avoidance into a reachability-constrained formulation, our framework synthesizes volume-informed guides that reliably guide a full-order trajectory optimizer over long horizons. We show that these optimized plans serve as high-quality references to train a residual reinforcement learning policy for robust online execution. We validate our approach on the Unitree G1 humanoid across three benchmark testbeds exceeding NIST emergency response standards, achieving restricted confinement ratios (

C_r < 1.5

). Our framework generates feasible trajectories across 12-to-18-second tasks with complex foot and hand contacts where standard baselines fail, while the learned policy successfully tracks these plans under extensive domain randomization in physics simulation.

Carlos Gonzalez, Luis Sentis

Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712, USA.

Constrained Motion Planning Humanoid Robot Locomotion Kinodynamic Motion Planning Robot Motion Planning Robot Collision Avoidance

Aug 10, 2026cs.RO

Entanglement-Free Trajectory Planning for Tethered Mobile Robots with a Slack Tether

In motion planning algorithms for tethered mobile robots, the entanglement state of the tether is a critical aspect to consider during the planning phase. This is particularly important in case of a slack tether, where the shape of the tether is not determined solely by the geometry of the environment and the location of the obstacles, but also by the dynamics of the tether, by the trajectory followed by the robot, and possibly by exogenous forces. In this scenario, preventing entanglement requires planning a robot trajectory that accounts for the entanglement definition and for the dynamics of the robot and of the tether. In this work, we propose a motion planning algorithm for tethered mobile robots with a slack tether that computes dynamically feasible entanglement-free trajectories to navigate through an environment with static obstacles. By considering the entanglement state during all the stages of the planning pipeline, we are able to compute safer trajectories that avoid entanglement during the motion of the robot. We achieve this through a three-step pipeline, which includes (i) the construction of a topological model of the entanglement-free configuration space of the tethered robot, (ii) the generation of a set of candidate paths using this model, and (iii) the computation of a dynamically feasible entanglement-free trajectory by solving a homotopy-constrained trajectory generation problem. The resulting trajectory can then be executed to lead the robot to its target location, while maintaining the tether in an entanglement-free configuration. We demonstrate the benefits of this algorithm in simulations, where we show how the planning algorithm avoids violations of the entanglement constraints, resulting in safer and more reliable trajectories.

Gianpietro Battocletti, Dimitris Boskos, Bart De Schutter

Delft Center for Systems and Control, Delft University of Technology, Delft, The Netherlands

Constrained Motion Planning Kinodynamic Motion Planning Trajectory Generation Safe Motion Planning Robot Motion Planning

Aug 10, 2026cs.RO

Graph-Guided Safe Diffuser: Topological Graph Guidance for Safe Diffusion Planning

Many diffusion-based planners enforce safety through inference-time guidance, but such interleaved trajectory deformations often degrade kinematic feasibility due to manifold rupture. We propose Graph-Guided Safe Diffuser (G2SD), a hierarchical framework that leverages a high-level topological graph planner to guide a low-level diffusion model. G2SD enforces safety at a structural level by abstracting the data manifold into a learned latent graph, on which high-level planning is performed. Continuous trajectories are generated by diffusion planners, which are conditioned on the graph node representations selected by the high-level planner. Theoretical analyses demonstrate conditions under which manifold rupture occurs in diffusion planners, and show that G2SD improves safety by reducing the constraint violation probability as the number of segments increases. Experiments demonstrate that G2SD substantially outperforms baselines, increasing goal-reaching rate without any collision from 40-50% to 98% in Maze2D navigation and also achieving superior task scores in locomotion.

Nakgyu Yang, KwangBin Lee, SooJean Han

School of Electrical Engineering, Korea Advanced Institute of Science and Technology (KAIST) Daejeon, Republic of Korea

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Graph-Based Planning Safe Motion Planning

Aug 7, 2026cs.RO

CoCoNav: Conformal Control for Safe Robot Navigation in Crowds

Safe and efficient robot navigation in crowds requires anticipating pedestrian motion despite uncertain and potentially shifting prediction errors. Existing reactive methods can produce oscillatory behavior, while predictive planners often treat forecasts as exact or rely on restrictive error models. Incorporating conservative uncertainty sets as hard constraints can also render model predictive control (MPC) infeasible. We propose \textit{CoCoNav}, a crowd-navigation framework that combines online conformal calibration with runtime-certified planning. A horizon-specific conformal proportional--integral controller adapts trajectory-error bounds to regulate long-run empirical coverage, enabling the framework to respond to changing prediction errors. A \textit{relax-then-verify} planner preserves solver feasibility by generating nominal trajectories with soft-constrained MPC and separately certifying them, together with contingency maneuvers, against the calibrated bounds before execution. Simulations and quadruped experiments show that CoCoNav achieves a favorable balance among collision avoidance, task success, and navigation efficiency relative to the evaluated baselines.

Cheng Guo, Mingzhe Ni, Zheng Liang +5

Department of Computer Science, The University of Manchester, Manchester, UK. · Human-Robot Interfaces and Interaction Laboratory, Italian Institute of Technology, Genoa, Italy. · Genisom AI, Shanghai, China. +3

Constrained Motion Planning Conformal Prediction Safe Robot Navigation Social Robot Navigation Robot Safety Model Predictive Control+1

Aug 6, 2026cs.RO

Search-Aided Joint Agent-Environment Reinforcement Learning for Robust Lifelong Multi-Agent Path Finding with Rotations

Lifelong Multi-Agent Path Finding (LMAPF) requires repeatedly planning collision-free paths for agents that continuously receive new goals upon reaching their current ones. While many learning-based planners have been proposed for LMAPF, most rely on oversimplified kinematic assumptions that may overlook motion constraints critical to real-world performance. In this work, we study a more realistic LMAPF model derived from many real-world automated warehouse systems, termed LMAPF-R2, which incorporates robust safety constraints and in-place rotation constraints. These constraints substantially increase coordination difficulty, particularly in highly constrained spaces. To address these challenges, we propose Search-Aided Joint Reinforcement Learning (SJRL). We first augment neural policies with Causal PIBT, a single-step search-based planner that resolves agents' collisions and propagates their intentions. We then introduce a unified RL formulation that jointly optimizes agent and environment policies, where the environment policy learns graph edge costs to provide global movement guidance via backward Dijkstra search. Experiments demonstrate that SJRL achieves significant improvements over the strong search-based planner, Causal-PIBT, across multiple high-density maps. We further validate SJRL in a challenging mixed-reality warehouse environment with 8 physical robots and 248 virtual robots.

He Jiang, Jingtian Yan, Yulun Zhang +5

1Carnegie Mellon University · University of Southern California · 3National University of Singapore

Constrained Motion Planning Robust MAPF Multi-Agent Path Finding Cooperative MARL

Aug 5, 2026cs.RO

GASP: GPU-Accelerated Safe Planner for Real-Time Collision-Aware Motion Generation with Latent Trajectory Sampling

We present GASP, a GPU-Accelerated Safe Planner for real-time, collision-aware joint-space motion generation in known environments. GASP combines a clamped B-spline trajectory parameterization with a convolutional residual neural network that predicts the free interior control points, while analytically inserted boundary control points enforce initial and final derivative constraints for collision-aware planning under non-stationary conditions. A conditional variational autoencoder samples multiple trajectory candidates, which are decoded and validated in parallel on the GPU, yielding a batched planner for collision-aware coupled joint-space motion with near-millisecond inference. We validate GASP as an online motion-generation module, where it achieves analytical-level success rates with high collision-aware feasibility and substantially reduces inference time relative to GPU-based trajectory optimization. We further deploy GASP as a reinforcement-learning reset planner in competitive robotic table tennis, matching the baseline return rate while roughly halving training-time collisions.

Colin Merk, Stefanos Charalambous, Peter Dürr +1

Sony, Zurich, Switzerland.

Constrained Motion Planning Trajectory Generation Robot Motion Generation Safe Motion Planning Robot Motion Planning Robot Collision Avoidance

Aug 5, 2026cs.RO

SCOPE: Field-of-View-Aware Path Planning in Unknown 3D Environments via Safety-Volume Certification

Safe navigation with a body-mounted limited-field-of-view sensor requires the complete robot-inflated volume of an intended motion to be observed and verified free before execution. We formulate this requirement as online safety-volume certification in an unknown voxel map and construct a certified graph whose vertices correspond exactly to positions with fully known-free safety volumes. Based on this representation, we propose SCOPE (Safety Certification through Observation Planning and Execution), a planning framework that decouples optimistic goal-directed guidance from certified execution. SCOPE converts the first uncertified point along an optimistic route into an explicit observation obligation, resolves it through target-centric viewpoint search, and recursively clears intermediate obligations when useful viewpoints are not yet certified-reachable. A certified preview mechanism and an observation-aware trajectory optimization backend enable smooth execution. We prove conditional complete planning: under ideal monotone sensing and exhaustive finite-domain graph search, SCOPE reaches the goal whenever a finite feasible sequence of certified sensing actions exists within its planning primitives. Across 60 randomized tasks in three unknown 3D environments, SCOPE reaches every goal while maintaining near-zero entry into non-certified inflated space. Preview reduces mean mission time by 27%, and real-robot demonstrations in two representative scenarios validate the complete system.

Junbin Yuan, Muqing Cao, Yunwoo Lee +2

Department of Mechanical Engineering at Carnegie Mellon University, Pittsburgh, PA 15213 USA · Robotics Institute, School of Computer Science, Carnegie Mellon University, Pittsburgh, PA 15213 USA · Department of Mechanical Engineering, Brigham Young University, Provo, UT 84602 USA

Constrained Motion Planning Safe Robot Navigation Robot Safety Graph-Based Planning Motion Planning

Aug 4, 2026cs.RO

Projection-Retraction MPPI: Exact Constraint-Manifold Control for Manipulators

Model Predictive Path Integral (MPPI) control is widely used in manipulation for its gradient-free, parallel handling of non-convex costs. Manipulation tasks, however, often impose constraints that hold throughout the motion: a closed kinematic chain that two grasping arms keep exactly, or joint limits and obstacle clearances that are never crossed. MPPI handles such constraints only through the cost, as soft penalties that hold approximately and fail under a strong task cost. To address this, we propose Projection-Retraction MPPI (PR-MPPI), which enforces the constraints inside the sampled dynamics. At every rollout step, the sampled velocity is projected to satisfy both constraint types: the equality restricts it to a subspace, and each inequality to a half-space within that subspace, so inequality handling never breaks the equality. This projection, however, satisfies the constraints only to first order, and a finite step leaves a small drift off the equality. Therefore, we retract the returned command back onto the constraint to numerical tolerance and independent of task weighting. We validate PR-MPPI on 14-DoF dual-arm systems. In simulation, the returned commands satisfy the closed-chain equality to numerical tolerance through a joint-limit stress test and randomized obstacle avoidance. On real hardware, the arms of a Unitree H1-2 humanoid reactively avoid a moving obstacle. Code and experiment videos are available at https://rcilab.github.io/prmppi.

Seulchan Lee, Leesai Park, Minhyeong Kang +1

School of Mechanical Engineering, Kyung Hee University, Yongin-si 17104, Republic of Korea · Institutes of Convergence Technology, Suwon-si 16229, Republic of Korea

Robotic Control Constrained Motion Planning Bimanual Robotic Manipulation Robotic Manipulation MPPI Control

Aug 4, 2026cs.RO

Accelerating Human-Aware Robot Trajectory Generation via Diffusion and Consistency Distillation

This research proposes a constrained motion planning framework for robot manipulators in human-robot interaction (HRI). For a non-redundant manipulator with a fully specified end-effector pose, additional requirements such as collision avoidance and self-collision avoidance are difficult to handle as simple null-space secondary tasks. This limitation makes it challenging to generate feasible joint-space trajectories in HRI environments where safety and kinematic constraints must be considered simultaneously. To address this limitation, collision- and self-collision-aware trajectories are generated using Rapidly-exploring Random Tree (RRT) and RRT* algorithms, and the resulting dataset is used to train a diffusion model that generates constraint-satisfying trajectories through guided sampling. To reduce the inference time required for iterative diffusion sampling, consistency distillation is applied, and a joint-weighted jerk regularization term is incorporated into the loss function to promote smoother trajectories by penalizing abrupt changes in joint acceleration. Simulation results show that the consistency model generates 150 trajectory candidates in less than 100 ms, maintains a high episode success rate, and substantially reduces joint and end-effector jerk when jerk regularization is applied.

Byeong-Il Ham, Hyun-Bin Kim, Kyung-Soo Kim

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Robot Motion Generation Safe Motion Planning Robot Collision Avoidance

Aug 3, 2026cs.RO

Biconvex Optimization for Smooth Minimum-Time Trajectories around Convex Obstacles

We present a biconvex approach for minimum-time motion planning around convex obstacles that is guaranteed to converge, is anytime, and supports derivative constraints to arbitrary order. We jointly convexify the minimum-time objective and all derivative constraints through a change of variables, and handle collision avoidance via time-varying separating planes, reducing the problem to a biconvex program. This program is solved by alternating between computing maximum-margin separating planes and optimizing the trajectory. By only adding planes for obstacles that the current iterate collides with, the trajectory can jump around obstacles and escape local minima. The method is guaranteed to converge starting from a simple collision-free polygonal curve. In our experiments on drone navigation and dual-arm bin unloading, we find that the proposed method reliably produces high-quality trajectories with computation times comparable to state-of-the-art decomposition-based motion planners, while handling a larger class of problems and being substantially more robust to bad initialization. Project page:https://wernerpe.github.io/bmtp-website/

Peter Werner, Tobia Marcucci, Daniela Rus

Massachusetts Institute of Technology Computer Science and Artificial Intelligence Laboratory · University of California Santa Barbara

Constrained Motion Planning Trajectory Optimization Trajectory Generation Nonconvex Optimization Motion Planning Robot Collision Avoidance

Aug 2, 2026cs.RO

Complete Motion Planning using Workspace-Fibered Decomposition for nR-Planar Manipulator

We propose a workspace-fibered decomposition framework for motion planning in nR planar redundant manipulators operating in cluttered environments. Rather than planning directly in the full n-dimensional configuration space, the method incrementally constructs obstacle-constrained reachable workspaces of lower-dimensional non-redundant sub-chains and recursively lifts them through redundant orientation fibers. This yields a sequence of reduced planning manifolds that preserve branch-consistent reachability structure while avoiding explicit construction of the full configuration-space obstacle geometry. We first establish that, for planar position-only manipulators, the obstacle-constrained reachable workspace induced by the minimal non-redundant sub-chain provides an exact characterization of feasibility with respect to the connected component of the start configuration, enabling early infeasibility detection prior to introducing redundant degrees of freedom (DOF). We then introduce an incremental fiber-lifting procedure that propagates reachable workspace structure through successive redundant links while enforcing local inverse-kinematic branch consistency using Jacobian determinant continuity constraints. The resulting representation admits efficient reduced-space planning directly on recursively-constructed workspace-fiber manifolds. Experimental results on redundant nR planar manipulators demonstrate that the proposed construction preserves collision-free connectivity structure across successive lifting stages while substantially reducing collision checking complexity relative to direct configuration space reasoning.

Aayush Rath, Antony Thomas

Robotics Research Center, IIIT Hyderabad, Hyderabad 500032, India.

Constrained Motion Planning Hierarchical Planning Motion Planning Robot Motion Planning

Aug 1, 2026cs.RO

RIT*: Riemannian Informed Trees for Cost-Adaptive Optimal Motion Planning

We present Riemannian Informed Trees (RIT*), a planning framework that replaces Euclidean primitives in batch-informed search with their Riemannian counterparts. RIT* constructs a tighter, cost-consistent informed set, performs a nearest-neighbour search under an anisotropic distance metric, and evaluates edge costs efficiently via a cascading scheme. We further introduce a Collision-Adaptive Metric Refinement (CARM), which learns an obstacle-proximity cost field online from collision feedback, reducing the reliance on prior metric design in practical settings. Experiments across environments from 2-D to 14-D show that RIT* is competitive in low-dimensional and spatially constant-metric settings and produces substantially lower-cost solutions when the metric varies spatially in high-dimensional configuration spaces. Performance gains scale with anisotropy and dimension, reaching up to 13.0% improvement in median initial cost over BIT* in the 3-D anisotropic benchmark, up to 9.0% in median final cost over BIT* in 6-DOF manipulation, and 24.8-63.5% in a 14-DOF bimanual planning problem, where Euclidean-informed baselines degrade. Videos and code can be found here: https://muhayyuddin.github.io/ritstar/

Muhayy Ud Din, Ahmed Nadar, Jan Rosell +1

Center for Autonomous Robotic Systems, Khalifa University (KU-CARS) · Institute of Industrial and Control Engineering, Universitat Politècnica de Catalunya, Barcelona, Spain

Constrained Motion Planning Motion Planning Sampling-Based Motion Planning

Jul 31, 2026cs.RO

Motion Planning for Mobile Manipulators Navigating Doorways via Model Predictive Control

Navigating doorways is a fundamental capability for mobile manipulators operating in human environments, requiring coordinated motion between the mobile base and manipulator arm. This paper presents a motion planning framework that generates dynamically feasible and collision-free trajectories for autonomously opening and traversing both push and pull doors. The proposed method formulates the robot and door as a coupled dynamical system within a nonlinear Model Predictive Control (MPC) optimization framework. Manipulation feasibility is enforced through a penalty-based constraint, avoiding explicit arm kinematic modeling in the planner. Simulations and a hardware experiment demonstrate that the approach successfully plans feasible trajectories for door traversal.

Kasra Sinaei, Kasun Weerakoon, Christopher Bradley +2

The Pennsylvania State University, University Park, PA 16802 USA · AlphaZ Inc., Baldwin Park, CA 91706 USA

Constrained Motion Planning Mobile Robotics Robot Navigation Dynamic Robotic Manipulation Motion Planning

Jul 29, 2026cs.AI

Multi-Agent Planning with Spatio-Temporal and Topological Constraints using STL-GO

Multi-agent planning problems arise in a variety of engineering applications, such as multi-robot wildfire fighting and unmanned aerial inspection in factories. A particular challenge is the existence of spatio-temporal (i.e., when and/or where an agent should do what) and topological constraints (i.e., how agents should interact), as typically formalized via the notion of graphs. Over the last years, various frameworks have been proposed that can capture such constraints via spatio-temporal logics. We focus here on spatio-temporal logic with graph operators (STL-GO), a recent formalism that supports reasoning about multiple agents and their topologies, such as sensing, communication, and task topologies. In this paper, we consider the problem of planning multi-agent paths that satisfy constraints written in STL-GO. This problem is particularly challenging due to the need of encoding multiple, potentially time-varying graphs via the graph operators inherent to STL-GO. We present two encodings of this problem, one based on mixed-integer programming (MIP) and another based on satisfiability modulo theory (SMT), with soundness guarantees. We provide a unified interface for specifying agent constraints, their graph topologies, and the STL-GO specification, enabling seamless use of both methods and facilitating direct comparison between them. We evaluate both encodings on a multi-UAV search-and-rescue benchmark, ablating over team size and graph complexity, highlighting the expressiveness of the proposed encodings under dynamic multi- graph interactions.

Sheryl Paul, Vidisha Kudalkar, Anand Balakrishnan +3

University of Southern California, Los Angeles, CA, USA · University of Texas at Austin, Austin, TX, USA · ETH Zurich, Zurich, Switzerland +1

Constrained Motion Planning Combinatorial Optimization Mixed-Integer QP Multi-Agent Planning Multi-Agent Path Finding

Jul 29, 2026cs.RO

Risk-Aware Motion Planning with Learned Trajectory Primitives and Probabilistic Safety Assessment

This paper presents a radial basis function network (RBFN)-informed motion planning framework for safe and efficient urban autonomous driving. The proposed approach combines RBFN-based candidate trajectory generation with an analytic collision probability assessment and optimization-based trajectory refinement. The network learns jerk-minimal trajectories, enabling the MPC to operate within a reduced and dynamically consistent search space. Candidate motion primitives are selected based on an accurate probabilistic risk measure. This design decreases solver complexity while preserving safety and constraint satisfaction. The framework is evaluated in numerous urban driving scenarios. Results demonstrate improved risk awareness and fewer vehicle-limit violations compared to benchmark methods. The proposed approach integrates learning-based trajectories into optimization-based motion planning, thereby ensuring safety and interpretability.

Marc Kaufeld, Dian Zhuang, Johannes Betz

Professorship of Autonomous Vehicle Systems, Technical University of Munich, 85748 Garching, Germany; Munich Institute of Robotics and Machine Intelligence (MIRMI) · School of Engineering and Design, Technical University of Munich, 85748 Garching, Germany

Collision Avoidance Constrained Motion Planning Trajectory Optimization Trajectory Generation Safe Motion Planning Motion Planning+2

Jul 27, 2026cs.RO

Motion Generation With Environmental Constraints

Robot motion planning faces challenges in high-dimensional spaces and uncertain environments, often constrained by the need for collision-free motions. We advocate an alternative approach, Environmental Constraint Exploitation (ECE), where deliberate contact with the environment simplifies planning by reducing dimensionality and computational complexity. By integrating ECE into motion planning algorithms, we bias exploration to task-relevant regions and leverage contact for uncertainty reduction to improve robustness during execution. We evaluate ECE benefits with RRT-based planners and demonstrate their practical benefits in a real-world application. This work consolidates and extends prior research, showcasing how ECE simplifies motion planning while enhancing adaptability and performance in complex environments.

Előd Páll, Oliver Brock

Robotics and Biology Laboratory, Technische Universität Berlin, Germany · Science of Intelligence, Research Cluster of Excellence, Berlin, Germany

Constrained Motion Planning Motion Planning Robot Motion Planning Sampling-Based Motion Planning

Jul 27, 2026cs.RO

Model Predictive Planner for UAV Navigation in Non-Convex Air Corridors

This work presents a motion planning framework for UAV navigation in non-convex urban air corridors. The planner is based on a mixed-integer tracking model predictive control formulation that enforces corridor feasibility and dynamic consistency within a single optimization problem. To guarantee convergence to the target and mitigate the occurrence of local minima induced by non-convex geometry, a shortest-path-based offset cost with feasibility constraints is embedded directly into the planning problem. Numerical simulations show that the proposed formulation generates dynamically valid trajectories that satisfy the corridor constraints and converge to the target without relying on external global planning stages.

Henrique Silva, Marcelo A. Santos, Guilherme V. Raffo

Graduate Program in Electrical Engineering, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil · Department of Management, Information and Production Engineering, University of Bergamo, Dalmine, BG, Italy · Department of Electronic Engineering, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil

Constrained Motion Planning Trajectory Generation UAV Navigation Model Predictive Control UAV Trajectory Optimization Motion Planning

Jul 25, 2026eess.SY

Actuator-Aware Spatiotemporal Tube Synthesis for Temporal Reach-Avoid-Stay Tasks

This work proposes an actuator-aware spatiotemporal tube (STT) synthesis framework to accomplish temporal reach-avoid-stay (T-RAS) tasks for an unknown nonlinear multi-input and multi-output (MIMO) system under actuator constraints. Existing STT synthesis methods address actuator saturation after the tube generation either through repeated online re-optimization or controller redesign. Instead, the proposed framework incorporates actuator constraints directly into the tube synthesis process. The STT centerline and width are parameterized using Bernstein polynomial basis functions, whose convex-hull property enables sample-free enforcement of geometric and derivative constraints. By analyzing the worst-case closed-loop error dynamics of an approximation-free prescribed performance controller (PPC) used for STT tracking, we derive a linear actuator feasibility constraint. The constraints are embedded directly in terms of the tubes' Bernstein control points into the STT synthesis optimization for actuator-feasible tube generation, eliminating the need for online re-optimization or controller redesign. A simulation study on an omnidirectional mobile robot performing a T-RAS task shows that the proposed framework adheres to the prescribed actuator limits throughout the task and reduces required control effort by approximately

50\%

compared with an existing STT synthesis method.

Keshab Patra, K Madhava Krishna

Robotics Research Center, International Institute of Information Technology - Hyderabad Hyderabad, Telangana, India

Robot Trajectory Tracking Constrained Motion Planning Trajectory Optimization

Jul 24, 2026cs.RO

Conformal Constraint Tightening for Chance-Constrained Motion Planning with Unknown Dynamics

Motion planning algorithms compute control sequences that drive autonomous robots to goal regions while avoiding unsafe states. Existing methods, from sampling-based planning to deep reinforcement learning, typically provide task-completion guarantees only with respect to a nominal model or simulator, which may be invalidated when the true dynamics are unknown or difficult to model accurately. This letter addresses this limitation for systems with unknown dynamics and an available approximate nominal model, contributing a planner-agnostic constraint-tightening procedure that equips existing planners with a probabilistic task-completion guarantee on the true system. We leverage conformal prediction to provide a probabilistic bound on the nominal-to-true trajectory deviation over a distribution of planning problems. We tighten the planning constraints using that bound, and show that solving the tightened problem under the nominal model is a sufficient condition for solving the original problem on the true system with a prescribed probability. We validate the theoretical guarantees empirically and demonstrate substantially improved task completion relative to nominal-model planning.

Shubham Natraj, Bruno Sinopoli, Yiannis Kantaros

Department of Electrical and Systems Engineering, Washington University in St. Louis, 1 Brookings Dr, St. Louis, MO 63130, USA · School of Electrical, Computer and Energy Engineering, Arizona State University, 650 E. Tyler Mall, Tempe, AZ 85281, USA

Constrained Motion Planning Safe Motion Planning Motion Planning

Jul 24, 2026cs.RO

Embodying Multi-Hand Manipulation Policies by Searching the Assignment and Null Spaces

Learned manipulation policies increasingly predict motions for abstract "hands" and are attractive in practice because they rely on easily collected demonstrations and transfer across robot platforms. Executing these trajectories on multi-arm robots, however, is not trivial. Multi-hand policy outputs must be assigned to physical arms, each arm must realize a configuration-space motion that tracks its prescribed end-effector trajectory, and all arms must respect kinematic limits and avoid collisions. In the absence of algorithms that directly address this problem, practitioners typically extend single-arm inverse-kinematics (IK) pipelines in an ad hoc way, with no guarantees of feasibility or safety. In this work, we close this execution gap with a search-based framework that is theoretically complete for grounding policy-generated multi-hand trajectories onto physical multi-arm systems. Building on Conflict-Based Search, our method explicitly searches over both the discrete assignment of trajectories to arms and the continuous Jacobian null spaces of redundant manipulators, using redundancy to avoid inter-arm collisions while tracking the prescribed motions. This unified treatment of assignment and null-space motion yields a practically efficient planner that safely realizes coordinated manipulation-policy outputs on multi-arm robots. See omcbsa.github.io for more.

Yorai Shaoul, Jiaoyang Li, Maxim Likhachev

The Robotics Institute, Carnegie Mellon University

Multi-Robot Motion Planning Constrained Motion Planning Bimanual Robotic Manipulation Conflict-Based Search Motion Planning Inverse Kinematics

Jul 21, 2026cs.RO

Stochastic Multi-Objective Kinodynamic Planning Against Adversaries

This paper addresses multi-objective kinodynamic planning in environments with stochastic hybrid adversaries that probabilistically transition to adversarial modes based on the ego state. The goal is to construct the Pareto-front of paths that trade off execution cost and the probability of safety constraint violation (risk). Existing chance-constrained planners evaluate risk over open-loop trajectories, yielding overly conservative solutions that fail to account for ego-agent reactivity. To address this limitation, we shift the planning space to sequences of closed-loop policies, and integrate sample-based risk evaluation directly into tree construction via Monte-Carlo particle rollouts. We first introduce Stochastic Multi-Objective RRT (SMO-RRT), for which we prove probabilistic completeness, followed by Stochastic Multi-Objective Stable Sparse RRT (SMO-SST), which leverages selective pruning to improve numerical performance at the cost of completeness. For both algorithms, we derive a finite-sample bound on the probability of chance constraint violation for systems with non-Gaussian, state-dependent uncertainty, enabling probabilistically safe planning in a broad class of environments applicable to multi-agent systems, social navigation, and autonomous driving.

Thomas Marshall Vielmetti, Daniel Cherenson, Dimitra Panagou

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Multi-Objective Motion Planning Sampling-Based Motion Planning

Jul 21, 2026eess.SY

STL-GCS: A Planner-Controller Framework for Signal Temporal Logic via Graphs of Time-varying Convex Sets

We present a unified trajectory planning and control framework for the satisfaction of Signal Temporal Logic (STL) specifications defined over convex predicates. At the planning layer, STL tasks are encoded as time-varying convex sets in configuration space, specifically designed so that forward invariance of the system with respect to these sets implies satisfaction of the specification with a prescribed robustness margin. This representation is then lifted to the joint time--configuration space and combined with the Graphs of Convex Sets (GCS) framework, yielding a shortest-path formulation of the planning problem over convex spatio-temporal sets. Trajectories are parameterized by B-splines, which enable continuous-time enforcement of STL satisfaction, collision avoidance, and smoothness constraints. At the control layer, the same time-varying sets used for planning are exploited to design a feedback controller that tracks the planned trajectory while prioritizing satisfaction of the STL specification during execution in the presence of tracking errors and model mismatch. We validate the proposed approach in simulation and in real-world experiments on space robotic platforms.

Nicola De Carli, Gregorio Marchesini, Dimos V. Dimarogonas

Robot Trajectory Tracking Constrained Motion Planning Signal Temporal Logic Robot Motion Planning

Jul 21, 2026cs.RO

Pose-Parameterized Motion Planning and CBF-QP Self-Collision Filtering for a Long-Reach Drilling Boom

Long-reach drilling booms must reach successive poses without self-collision. Moving from operator-supervised control toward autonomy requires collision-aware motion planning and execution. For the Sandvik SB60, this study adapts established methods by integrating pose-parameterized planning with a capsule-based control barrier function quadratic program (CBF-QP) in measured-state inverse kinematics (IK). A fixed task-specific parameter set within each task generates waypoints, detours, timed references, and chained motion without target-specific retuning. The offline detour planner screens candidate waypoints using 23 selected rod-segment-to-body-region distances, whereas the online CBF-QP filters joint velocities using 14 configured capsule-pair constraints from a nine-primitive whole-body capsule model. Evaluation considers two drilling tasks in a manufacturer-developed SB60 Simscape Multibody model: a five-target restricted-orientation tour and a three-target full-pose tour. Across several hundred thousand samples, the method produced zero IK failures, generated several detour waypoints, achieved millimetre-level mean final-position error, and recorded no sampled CBF margins below the reported thresholds.

Mehdi Heydari Shahna, Tuomo Kivelä, Jouni Mattila

Faculty of Engineering and Natural Sciences, Tampere University, 33720 Tampere, Finland. · Sandvik Mining and Construction Oy, Tampere, Finland.

Constrained Motion Planning Control Barrier Functions Robotic Tool Use Safe Motion Planning Robot Motion Planning Robot Collision Avoidance

Jul 20, 2026cs.RO

Manifold-Guided Motion Planning for Tight Assemblies

Motion planning for rigid-body assembly poses a fundamental challenge in robotics due to tight geometric constraints. In such scenarios, feasible motions often require passing through (near-)zero clearance configurations in which the parts are tightly constrained by contact. In this work, we introduce Critical-Manifold Guided RRT (CMG-RRT), a sampling-based planner designed specifically for tight assembly problems. Our key observation is that in tight assemblies, valid solution paths lie on or near a critical manifold: the subset of configuration space consisting of poses with at least one contact point between parts. CMG-RRT guides exploration by adaptively biasing sampling toward neighborhoods of the critical manifold using a hierarchical subdivision of the configuration space. We prove that CMG-RRT is probabilistically complete under standard clearance assumptions. Empirical evaluation on challenging rotational assembly benchmarks demonstrates a 100% success rate across all tested instances, including, to the best of our knowledge, the first fully automatic solution of the Elk disentanglement puzzle. Our open source software is available through our project page: https://www.cgl.cs.tau.ac.il/projects/tight-assembly-planning.

Dror Livnat, Michael M. Bilevich, Michal Kleinbort +1

Blavatnik School of Computer Science and Artificial Intelligence, Tel-Aviv University, Israel.

Constrained Motion Planning Motion Planning Robotic Assembly Sampling-Based Motion Planning

Jul 20, 2026cs.RO

Task-Space Constrained Stochastic Trajectory Optimization for Time-Optimal Forestry Crane Motion Planning

Efficient, collision-free, and time-optimal motion planning is a fundamental requirement for autonomous forestry cranes operating under hydraulic pump-flow constraints. The Via-Point-based Stochastic Trajectory Optimization (VP-STO) algorithm has demonstrated near-time-optimal hybrid motion planning in this domain, but requires a fixed terminal joint configuration specified prior to optimization. For kinematically redundant manipulators such as forestry cranes, this pre-commitment to a single inverse kinematics solution restricts the planner's ability to exploit redundancy, particularly under the nonlinear, globally coupled pump-flow constraint where admissible joint velocities depend on their combined hydraulic demand. This paper presents TSC-VP-STO, a task-space-constrained extension of VP-STO that replaces the strict terminal joint-space constraint with a task-space constraint, jointly optimizing the trajectory and the redundant degrees of freedom of the terminal configuration. This enables the planner to adapt end configurations to the environment-dependent motion and hydraulic flow allocation, yielding more balanced pump utilization and shorter trajectory durations. We formalize the approach through a configuration space decomposition and derive a concrete reachability constraint for the forestry crane kinematics. Experimental evaluations across multiple planning targets and via-point configurations demonstrates a reduction on trajectory durations by 12-15% on average and improved pump-flow utilization compared to the baseline VP-STO. The practical applicability of TSC-VP-STO is validated through real-world deployment on a forestry crane, including a full log-loading cycle.

Marc-Philip Ecker, Christoph Fröhlich, Bernhard Bischof +2

Automation & Control Institute (ACIN), TU Wien, 1040 Vienna, Austria · Center for Vision, Automation & Control, AIT Austrian Institute of Technology GmbH, 1210 Vienna, Austria

Stochastic Optimization Constrained Motion Planning Trajectory Optimization Motion Planning

Jul 20, 2026cs.RO

Finite-Time Curvature-Constrained Vector Field for Saturation-Free Motion Planning of Nonholonomic Robots

Accurately steering a robot to a target configuration is fundamental in engineering, yet remains challenging for nonholonomic mobile robots. Vector fields (VFs) provide a natural framework by specifying desired motion directions throughout the workspace and enabling direct integration with feedback control. However, most existing VF-based methods cannot explicitly generate trajectories satisfying curvature constraints. Actuator limits are therefore often enforced by input saturation, which may invalidate stability guarantees and degrade closed-loop performance when not considered in controller design. In addition, these methods usually ensure only asymptotic convergence without an explicit settling-time bound. To address these issues, we propose a generalized motion planning and control framework consisting of a finite-time curvature-constrained vector field (FT-C2VF) and a saturation-free control law. Depending on the motion objective, the framework drives the robot to the target configuration in finite time or through it periodically. First, the FT-C2VF is constructed using complementary gains to achieve finite-time convergence while ensuring that the curvature of its integral curves is continuous, bounded, and monotonically decreasing with the radial ratio. Second, an almost globally C1-smooth, saturation-free controller is developed to track the FT-C2VF without Jacobian information, while keeping all control inputs within prescribed actuator limits. Third, dynamical-systems analysis establishes almost-global finite-time stability of the target equilibrium. Numerical simulations show improved performance over representative VF-based methods, and outdoor experiments on an Ackermann-steered vehicle confirm the effectiveness and robustness of the proposed approach.

Zhouru Xiao, Sha Luo, Yang Lu +5

School of Artificial Intelligence and Robotics, Hunan University. China. · College of Information Science and Engineering, Hunan Normal University, China. · College of Systems Engineering and the State Key Laboratory of Digital Intelligent Modeling and Simulation, National University of Defense Technology, China. +1

Constrained Motion Planning Mobile Robotics Feedback Control Motion Planning Robot Motion Planning

Jul 17, 2026cs.RO

Minimum Time Dubins Airplane Paths with Asymmetric Climb Rates

Dubins airplane paths approximate the limited maneuverability of fixed-wing vehicles with minimum curvature and climb rate constraints. However, the symmetric climb rate constraints result in sub-optimal paths and conservative vehicle performance. In this work, we propose asymmetric Dubins airplane paths, which consider asymmetric climb rates for climbing and descending. We revisit the time optimality conditions and show that the asymmetric flight path angle constraints preserve optimality. We show that by considering asymmetric climb rates, we can take advantage of full performance of the vehicle, reducing the minimum time by 71% for connecting randomly generated states. We also demonstrate that the added climb rate results in 2.8 times faster to find the median solution time when integrated into a sampling-based planning task on rugged terrain, due to the added feasibility. We further demonstrate the practicality of the approach with a real-world flight.

Jaeyoung Lim, Giuseppe Loianno

Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, CA 94720, USA

Aerial Robotics Constrained Motion Planning Kinodynamic Motion Planning Trajectory Generation UAV Navigation UAV Trajectory Optimization+1

Jul 17, 2026cs.RO

A Task-Space Receding Horizon Controller for Fast Collision Avoidance

Real-time collision avoidance for robotic manipulators requires fast reactions to unexpected obstacle motion and lookahead to avoid becoming trapped by near-future constraints. Full model predictive control can provide this foresight, but its online cost may grow quickly with horizon length, model fidelity, and the number of active geometric constraints. Conversely, horizon-free reactive methods are computationally efficient but can be short-sighted in dynamic clutter. We present a task-space receding-horizon controller that uses a short contact-consistent rollout to generate a terminal kinematic reference satisfying internal non-penetration constraints, then computes only the first input of a smooth minimum-acceleration transition toward that reference. Starting from a closed-loop inverse-kinematics regulation law, the rollout is performed with an iterative dynamics solver operating on inflated convex robot and obstacle geometries, so that robot-obstacle contacts, dynamic obstacle motion, and self-collisions can shape the terminal reference without requiring full constrained trajectory optimization. We analyze the contact-inactive closed loop and show local exponential task-space regulation under standard regularity assumptions. For contacts activated inside the rollout, we characterize the corresponding discrete updates and bound the effect of moving obstacles on regular operating sets. Simulations on a 40-DOF multi-chain system show that intermediate horizons balance anticipation, responsiveness, and computational cost. Hardware experiments on a 6-DOF platform demonstrate consistent sim-to-real behavior without accurate inertial parameter estimation, and comparisons against dynamic optimization fabrics and model predictive control (MPC) baselines show improved success rates in dynamic clutter while preserving solve times compatible with real-time execution in the tested regimes.

Mattia Penzotti, Marco Controzzi

Biorobotics Institute and the Department of Excellence in Robotics and AI, Scuola Superiore Sant’Anna, Pisa, Italy

Robotic Control Constrained Motion Planning Model Predictive Control Dynamic Obstacle Avoidance Robot Collision Avoidance

Jul 16, 2026cs.RO

Motion Planning with Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling

Single-Robot Motion Planning (SRMP) in highly non-convex constrained environments, where robots must satisfy collision-free guarantees, dynamic feasibility, and task-related constraints, is challenging under complex constraints and computational limits. Recent Model-Based Diffusion (MBD) approaches recast the SRMP as trajectory optimization that samples from a posterior over trajectories, using known dynamics, and analytically estimates the score function from rollout samples to guide diffusion denoising toward a low-cost, clean trajectory without demonstration learning. While existing works further adapt MBD to constrained environments and showcase promising performance, they are still limited by (1) enforcing safety either via soft feasibility diffusion priors or hard projection operators, but lack a unified framework to integrate both, and (2) fixing safety enforcement to neglect the changing of diffusion scheduling. Therefore, we introduce Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling (MD-COAS) for SRMP that unifies the inexact Augmented Lagrangian Method (iALM) soft diffusion prior with a Convex Feasible Set (CFS)-based hard projection operator, and adaptively schedules and co-optimizes safety enforcement, along with diffusion scheduling. Experiments demonstrate that our method achieves higher safety & success rates, faster convergence, and lower final costs than baseline planners on randomly generated highly non-convex 2D benchmarks and a 7-DoF robot arm avoidance task.

Zhilin He, Bowei Li, Jianlin Dou +2

Carnegie Mellon University · University of Pennsylvania

Constrained Motion Planning Trajectory Optimization Trajectory Generation Diffusion-Based Planning Model-Based Planning Safe Motion Planning+2

Jul 14, 2026cs.RO

Globalized Constrained Stein Variational Inference for Diverse Feasible Robot Motion Planning

Robot motion planning is inherently multimodal, yet classical planners typically return only a single solution. Probabilistic formulations address this limitation by maintaining a distribution over motions, allowing the planner to reason over multiple low-cost alternatives. In robotics, however, motion samples must also satisfy strict constraints, including collision avoidance, joint limits, contact conditions, and dynamics consistency. These hard requirements make motion sampling substantially more challenging: within a limited planning budget, the ensemble must cover diverse low-cost motions while ensuring that every sample remains feasible under the relevant constraints. We propose SteinSQP (Stein Variational Sequential Quadratic Programming), a constrained Stein variational inference method for diverse feasible robot motion sampling. SteinSQP evolves an interacting particle ensemble, as in Stein variational methods, while embedding constraints directly into a kernel-space SQP subproblem. We solve the resulting constrained Stein-Newton subproblem with a GPU-friendly matrix-free primal-dual algorithm, enabling efficient batched ensemble updates. To globalize the method, we introduce an ensemble-level merit function that jointly balances objective value, constraint violation, and particle diversity. Across five constrained motion-planning tasks, SteinSQP returns fully feasible ensembles while preserving diverse motion alternatives. Compared with first-order constrained Stein baselines and serial multistart nonlinear programming, SteinSQP shows faster and more robust ensemble convergence in terms of iterations, improves particle-wise feasibility, and achieves faster batched time-to-solution on challenging robot-scale tasks.

Jiayun Li, Georgia Chalvatzaki

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

Constrained Motion Planning Constrained Optimization Motion Planning Variational Inference Robot Motion Planning Multi-Objective Motion Planning

Jul 14, 2026cs.RO

Model-Based Diffusion Optimal Control for Multi-Robot Motion Planning

Multi-Robot Motion Planning in continuous environments, where robots must generate dynamically feasible, collision-free trajectories, is challenging due to the combinatorial growth of the joint trajectory space and the difficulty of enforcing dynamic feasibility and hard safety constraints. Recent approaches recast trajectory planning as probabilistic inference, sampling from a posterior over trajectories using diffusion models whose score functions are learned from demonstration data. While showing promising performance, these approaches are limited: they often rely on sizable demonstration datasets and struggle to rigorously enforce dynamics and hard safety constraints during sampling. To this end, we introduce Model-Based Diffusion Optimal Control (MDOC), a model-based diffusion planner that efficiently produces dynamically feasible trajectories without relying on data. Crucially, we show that MDOC's safety mechanism -- combining known dynamics models with Control Barrier Function-constrained projections -- naturally scales to multi-robot planning settings through Conflict-Based Search. Across simulation experiments, this integrated method consistently outperforms representative baseline planners in sample efficiency, geometric smoothness, and success rate, while reducing computation time and producing collision-free trajectories.

Zhilin He, Yorai Shaoul, Jiaoyang Li

Carnegie Mellon University, Pittsburgh, PA, USA

Multi-Robot Motion Planning Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Conflict-Based Search Model-Based Planning+2

Jul 13, 2026cs.RO

CR-Solver: GPU-Accelerated Kinematics Solver for Tendon-driven Continuum Robots

Continuum robots provide intrinsic compliance, high dexterity, and safe physical interaction, enabling navigation and manipulation in confined and unstructured environments. Despite recent advances in sensing and control, heightening the need for precise motion generation, most widely used planning libraries are grounded in rigid-body assumptions, creating a critical gap for fast and practical tools for continuum robots. To address this, we present CR-Solver, a two-stage, optimization-based solver for the motion generation of tendon-driven continuum robots. Our method unifies inverse kinematics, path following, and trajectory planning within a single constrained nonlinear optimization framework. Leveraging GPU-accelerated parallel optimization, CR-Solver delivers fast, accurate, and constraint-aware solutions. We validate our approach on three tasks, demonstrating significant speedups over traditional CPU-based solvers while achieving a consistently high success rate above 95% and millimeter-level accuracy. The solver is implemented in pure Python, reducing the barrier to adoption and offering a practical, extensible foundation for continuum robots' high-performance motion planning.

Heqing Yang, Yang Yi, Linqing Zhong +2

Beihang University.

Constrained Motion Planning Robot Motion Generation Inverse Kinematics Robot Motion Planning

Jul 12, 2026cs.RO

D-SafeMPC: Diffusion-Driven Safe Model Predictive Control with Discrete-Time Control Barrier Functions

A key limitation on the use of diffusion models in robotic planning is their inability to inherently enforce safety or dynamical constraints, which often results in physically infeasible or unsafe outputs. Hybrid approaches that employ model predictive control (MPC) to address this problem can be unstable, as poor trajectory initializations from the diffusion model prevent the MPC from converging to a safe and feasible solution. To overcome these challenges, we propose D-SafeMPC, which enhances the interaction between diffusion and control. Our method guides the reverse diffusion process with control barrier functions (CBFs) and control Lyapunov functions (CLFs) and employs an iterative-projection scheme where an MPC refines the trajectory at each denoising step. This steers sampling toward safe, goal-directed regions and provides reliable MPC warm starts. In simulations on a Franka manipulator across four scenarios (one static-obstacle and three dynamic-obstacle settings) and in a sim-to-real experiment on a physical Franka robot, D-SafeMPC improves safety, task success rates, and planning efficiency over state-of-the-art baselines. To facilitate reproducibility, our source code and experimental configurations are available in a repository at https://github.com/erdiphd/D-SafeMPC

Erdi Sayar, Ersin Daş, Joel W. Burdick +2

Department of Electrical Engineering and Information Technology, Paderborn University, 33098 Paderborn, Germany · Department of Mechanical, Materials, and Aerospace Engineering, Illinois Institute of Technology, Chicago, IL 60616, USA · Mechanical and Civil Engineering, California Institute of Technology, Pasadena, CA 91125, USA +1

Constrained Motion Planning Diffusion-Based Planning Diffusion Guidance Model Predictive Control Safe Motion Planning

Jul 11, 2026cs.RO

PIER-Flow: Physics-Informed Efficient Rectified Flow for Real-Time Mobile Robot Navigation

Autonomous navigation in dense and highly dynamic environments requires both physically feasible control and low-latency replanning. Optimization-based methods such as Model Predictive Control (MPC) explicitly handle robot kinematics and safety constraints, but repeated nonlinear optimization can limit real-time responsiveness. Deterministic behavior-cloning policies enable efficient inference but may fail to represent multimodal avoidance behaviors, whereas diffusion policies capture multimodality at the cost of time-consuming iterative denoising. We propose PIER-Flow (Physics-Informed Efficient Rectified Flow), a lightweight navigation policy for mobile robots. By distilling an MPC expert into a continuous-time Ordinary Differential Equation (ODE), PIER-Flow achieves single-step action generation through parallel latent sampling and lightweight feasibility selection. We introduce a physics-informed training objective to enforce kinematic consistency, paired with an asynchronous action chunking architecture for robust sim-to-real deployment. Extensive simulations demonstrate that PIER-Flow achieves a 98.85% success rate and zero collisions, with an average inference of

\sim

1.29 ms, which accelerates planning by 37.2

\times

compared to MPC and over 800

\times

against standard diffusion models. Crucially, real-world deployment on a resource-constrained edge computer further achieves an approximately stable inference latency of

\sim

5.3 ms, avoiding the latency spikes and freezing events observed with planning baselines.

Shibo Li, Zhongcheng Wang, Jiahe Cao +2

School of Automation, Northwestern Polytechnical University, Xi’an, China · Department of Robotics, Mohamed bin Zayed University of Artificial Intelligence, Abu Dhabi SE45 05, United Arab Emirates

Constrained Motion Planning Robot Policy Learning Rectified Flow Robot Navigation Physics-Informed ML

Jul 7, 2026cs.RO

EvoPlan: Evolutionary Neuro-Symbolic Robot Planning with Spatio-Temporal Guarantees

LLM-based robot planners are fluent but cannot guarantee that their plans are executable or safe. Classical PDDL planners can guarantee these properties, but only after the problem is fully specified, and they make poor use of an LLM's ability to read context and repair plans. This paper presents a neuro-symbolic framework with three parts. All LLM calls use a locally-hosted open-weight model, so the pipeline can be deployed on-robot with no cloud dependency. First, an offline procedure that mines a single global Signal Temporal Logic (STL) constraint on mobility from demonstration data. The procedure recovers codified rules (e.g., stopping at red lights, mined from nuPlan driving logs) or population preferences (e.g., social-navigation comfort, mined from SCAND teleoperation), depending on what the demonstrations encode. Because the demonstrations are a one-class signal, we generate the missing negatives with counterfactual perturbations and an LLM violation generator and then fit the constraint by evolutionary search. We use the mined constraint to shield a vision-language driving policy on Bench2Drive and two discrete-action navigation policies on HA-VLN-CE. Second, an evolutionary PDDL planner: an LLM proposes and repairs plans, programmatic validators decide which ones survive, and the validated portion of the plan grows over iterations. We test the planner on the open-world ALFWorld Text benchmark, where it beats strong baselines and stays robust when the goal vocabulary does not match the action-model vocabulary. Third, a constrained execution loop: the planner's plan is compiled into waypoints, the waypoints are checked against the mined constraint, and the planner re-plans on a violation. We illustrate the full pipeline via demonstrations using the Gazebo simulator.

Bhavya Sai Nukapotula, Samin Moosavi, Haoze Wang +4

Texas A&M University · Westwood High School

Constrained Motion Planning Large Language Model-Based Robot Planning Signal Temporal Logic Robot Safety Robot Task Planning Safe Motion Planning+1