Motion Planning

In cooperative environments, such as in factories or assistive scenarios, it is important for a robot to communicate its intentions to observers, who could be either other humans or robots. A legible trajectory allows an observer to quickly and accurately predict an agent's intention. In adversarial environments, such as in military operations or games, it is important for a robot to not communicate its intentions to observers. An illegible trajectory leads an observer to incorrectly predict the agent's intention or delays when an observer is able to make a correct prediction about the agent's intention. However, in some environments there are multiple observers, each of whom may be able to see only part of the environment, and each of whom may have different motives. In this work, we introduce the Mixed-Motive Limited-Observability Legible Motion Planning (MMLO-LMP) problem, which requires a motion planner to generate a trajectory that is legible to observers with positive motives and illegible to observers with negative motives while also considering the visibility limitations of each observer. We highlight multiple strategies an agent can take while still achieving the problem objective. We also present DUBIOUS, a trajectory optimizer that solves MMLO-LMP. Our results show that DUBIOUS can generate trajectories that balance legibility with the motives and limited visibility regions of the observers. Future work includes many variations of MMLO-LMP, including moving observers and observer teaming.

Sampling-based motion planners (SBMPs) are widely used for robot motion planning with complex kinodynamic constraints in high-dimensional spaces, yet their serial computation design results in planning speeds that scale poorly with problem complexity. Recent efforts to parallelize SBMPs have achieved significant speedups in finding feasible solutions; however, they provide no guarantees of optimizing an objective function. We introduce Kino-PAX$^{+}$, a massively parallel kinodynamic SBMP with asymptotic near-optimal guarantees. Kino-PAX$^{+}$ builds a sparse tree of dynamically feasible trajectories by decomposing traditionally serial operations into three massively parallel subroutines. The algorithm focuses computation on the most promising nodes within local neighborhoods for propagation and refinement, enabling rapid improvement of solution cost. We prove that, while maintaining probabilistic $δ$-robust completeness, this focus on promising nodes ensures asymptotic $δ$-robust near-optimality. Our results show that Kino-PAX$^{+}$ finds solutions up to three orders of magnitude faster than existing serial methods and achieves lower solution costs

We address the problem of reactive motion planning for quadrotors operating in unknown environments with dynamic obstacles. Our approach leverages a 4-dimensional spatio-temporal planner, integrated with vision-based Safe Flight Corridor (SFC) generation and trajectory optimization. Unlike prior methods that rely on map fusion, our framework is mapless, enabling collision avoidance directly from perception while reducing computational overhead. Dynamic obstacles are detected and tracked using a vision-based object segmentation and tracking pipeline, allowing robust classification of static versus dynamic elements in the scene. To further enhance robustness, we introduce a backup planning module that reactively avoids dynamic obstacles when no direct path to the goal is available, mitigating the risk of collisions during deadlock situations. We validate our method extensively in both simulation and real-world hardware experiments, and benchmark it against state-of-the-art approaches, showing significant advantages for reactive UAV navigation in dynamic, unknown environments.

Learning structured task representations from human demonstrations is essential for bimanual manipulation, where action ordering, object involvement, and interaction geometry vary significantly across executions. A key challenge lies in jointly capturing the discrete semantic task structure and the temporal evolution of object-centric geometric relations in a form that supports reasoning over task progression. We introduce a semantic-geometric graph-based task representation that jointly encodes object identities, inter-object semantic relations, and per-object motion histories, via a Message Passing Neural Network (MPNN) encoder and a Transformer-based decoder. The encoder operates solely on the temporal scene graph, producing structured representations decoupled from action labels. The decoder then conditions on action-context to forecast future actions, associated objects, and object motions. This decoupling learns task-agnostic representations, enabling encoder reuse across embodiments through decoder-only finetuning on a small robot dataset. Across eleven bimanual tasks from two datasets, we find that the benefit of structured semantic--geometric representations over simpler sequence-based models grows with task variability in action ordering and object involvement. At deployment, a planner couples the action and motion predictions with learned Probabilistic Movement Primitives, achieving full task success on two real-robot bimanual tasks and outperforming graph ablations, Transformer, decoder-only, and finetuned vision-language model baselines.

A long-standing challenge in AI is to develop agents capable of solving a wide range of physical tasks and generalizing to new, unseen tasks and environments. A popular recent approach involves training a world model from state-action trajectories and subsequently use it with a planning algorithm to solve new tasks. Planning is commonly performed in the input space, but a recent family of methods has introduced planning algorithms that optimize in the learned representation space of the world model, with the promise that abstracting irrelevant details yields more efficient planning. In this work, we characterize models from this family as JEPA-WMs and investigate the technical choices that make algorithms from this class work. We propose a comprehensive study of several key components with the objective of finding the optimal approach within the family. We conducted experiments using both simulated environments and real-world robotic data, and studied how the model architecture, the training objective, and the planning algorithm affect planning success. We combine our findings to propose a model that outperforms two established baselines, DINO-WM and V-JEPA-2-AC, in both navigation and manipulation tasks. Code, data and checkpoints are available at https://github.com/facebookresearch/jepa-wms.

Video generation has achieved remarkable progress in visual fidelity and controllability, enabling conditioning on text, layout, or motion. Among these, motion control - specifying object dynamics and camera trajectories - is essential for composing complex, cinematic scenes, yet existing interfaces remain limited. We introduce LAMP that leverages large language models (LLMs) as motion planners to translate natural language descriptions into explicit 3D trajectories for dynamic objects and (relatively defined) cameras. LAMP defines a motion domain-specific language (DSL), inspired by cinematography conventions. By harnessing program synthesis capabilities of LLMs, LAMP generates structured motion programs from natural language, which are deterministically mapped to 3D trajectories. We construct a large-scale procedural dataset pairing natural text descriptions with corresponding motion programs and 3D trajectories. Experiments demonstrate LAMP's improved performance in motion controllability and alignment with user intent compared to state-of-the-art alternatives establishing the first framework for generating both object and camera motions directly from natural language specifications. Code, models and data are available on our project page.

Recent progress in contact-rich robotic manipulation has been striking, yet most deployed systems remain confined to simple, scripted routines. One of the barriers is the lack of motion planning algorithms that can provide verifiable guarantees for safety, efficiency and reliability. Constant-Time Motion Planning (CTMP) is a recent step toward such guarantees for collision-free motion in a priori known environments:: a preprocessing phase enables queries to be answered within a fixed, user-specified time budget (e.g., 10 milliseconds). However, CTMP certifies only reachability---a binary predicate---and ignores the manipulation behavior that completes the task, which is increasingly stochastic (e.g., a learned skill) and whose success no single offline rollout can establish, let alone certify. We introduce the Behavioral Constant-Time Motion Planner (B-CTMP), which extends CTMP to two-step manipulation tasks in semi-structured environments: a collision-free motion to a behavior initiation state, followed by execution of a behavior such as grasping or insertion. B-CTMP departs from prior CTMP in two ways: neighborhoods are constructed in object-pose space rather than robot configuration space, and coverage is established by statistical certification rather than a reachability check. A plan is cached only if repeated rollouts lower-bound its success rate above a user-specified threshold, and we prove these bounds hold simultaneously across the entire cache at a prescribed confidence level. For deterministic behaviors a single rollout suffices, recovering the binary check of prior CTMP as a special case. We evaluate B-CTMP on three manipulation tasks---shelf picking, plug insertion, and wheel replacement---in simulation and on real robots. B-CTMP's certified plans succeed consistently where baselines fail during behavior execution, and it rejects infeasible object poses in constant time.

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

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

Autonomous navigation in 3D is a fundamental problem for autonomy. Despite major advancements in terrestrial and aerial settings due to improved range sensors including LiDAR, compact sensors with similar capabilities for underwater robots have only recently become available, in the form of 3D sonars. This paper introduces a novel underwater 3D navigation pipeline, called SHRUMS (Sensor Hallucination for Robust Underwater Motion planning with 3D Sonar). To the best of the authors' knowledge, SHRUMS is the first underwater autonomous navigation stack to integrate a 3D sonar and demonstrate safe operation in complex 3D environments without reliance on visual input, thereby remaining independent of visibility conditions. To accommodate the intricacies of the novel sensor data stream while achieving real-time locally optimal performance, SHRUMS introduces the concept of hallucinating sensor measurements from non-existent sensors with convenient arbitrary parameters, tailored to application specific requirements. The proposed concepts are validated in simulations informed with real 3D sonar sensor data, in-pool clean-water trials, and field deployments in real conditions with significantly degraded visibility conditions.

Autonomous high-speed navigation through large, complex environments requires real-time generation of agile trajectories that are dynamically feasible, collision-free, and satisfy state or actuator constraints. Modern trajectory planning techniques primarily use numerical optimization, as they enable the systematic computation of high-quality, expressive trajectories that satisfy various constraints. However, stringent requirements on computation time and the risk of numerical instability can limit the use of optimization-based planners in safety-critical scenarios. This work presents an optimization-free planning framework called STITCHER that stitches short trajectory segments together with graph search to compute long-range, expressive, and near-optimal trajectories in real-time. STITCHER outperforms modern optimization-based planners through our innovative planning architecture and several algorithmic developments that make real-time planning possible. Extensive simulation testing is performed to analyze the algorithmic components that make up STITCHER, along with a thorough comparison with three state-of-the-art optimization planners. Simulation tests show that safe trajectories can be created within a few milliseconds for paths that span the entirety of two 50 m x 50 m environments. Hardware tests with a custom quadrotor verify that STITCHER can produce trackable paths in real-time while respecting nonconvex constraints, such as limits on tilt angle and motor forces, with flight speeds up to 63 km/h.

Generating large and diverse obstacle datasets to learn motion planning in environments with dynamic obstacles is challenging due to the vast space of possible obstacle trajectories. Inspired by hallucination-based data synthesis approaches, we propose Learning from Hallucinating Critical Points (LfH-CP), a self-supervised framework for creating rich dynamic obstacle datasets based on existing optimal motion plans without requiring expensive expert demonstrations or trial-and-error exploration. LfH-CP factorizes hallucination into two stages: first identifying when and where obstacles must appear in order to result in a near-optimal motion plan, i.e., the critical points, and then procedurally generating diverse trajectories that pass through these points while avoiding collisions. This factorization avoids generative failures such as mode collapse and ensures coverage of diverse dynamic behaviors. We further introduce a diversity metric to quantify dataset richness and show that LfH-CP produces substantially more varied training data than existing baseline. Experiments in simulation demonstrate that planners trained on a LfH-CP generated dataset achieves higher success rates compared to a prior hallucination method.

In this work, we introduce BBoE, a bidirectional, kinodynamic, sampling-based motion planner that consistently and quickly finds solutions in environments with varying obstacle clutter. The algorithm combines exploration and exploitation while relying on precomputed robot motions, resulting in efficient convergence towards the goal. Our key contributions include: i) a strategy to quickly find solutions in obstacle-rich spaces by leveraging a bundle of precomputed robot motions (i.e., edges); and ii) BBoE, a robust bidirectional kinodynamic planner that utilizes this strategy to produce fast and feasible solutions. Across four systems spanning 3D to 6D state spaces, BBoE is the only planner to achieve $\geq96\%$ success on every system while attaining the lowest planning time on three of four.

Unsteady aerodynamic effects can have a profound impact on aerial vehicle flight performance, especially during agile maneuvers and in complex aerodynamic environments. In this paper, we present a real-time planning and control approach capable of reasoning about unsteady aerodynamics. Our approach relies on a lightweight GPU-accelerated vortex particle model (VPM) and a sampling-based policy optimization strategy capable of leveraging the VPM for predictive reasoning. Through hardware experiments, we show that by replanning with our unsteady aerodynamics model, we can improve the performance of a post-stall fixed-wing perching maneuver in the presence of unsteady environmental flow disturbances.

Autonomous flight in GPS-denied indoor spaces requires trajectories that keep visual-localization error tightly bounded across varied missions. Map-based visual localization methods such as feature matching require computationally intensive map reconstruction and have feature-storage scalability issues, especially for large environments. Scene coordinate regression (SCR) provides an efficient learning-based alternative that directly predicts3D coordinates for every pixel, enabling absolute pose estimation with significant potential for onboard roboticsapplications. We present a perception-aware trajectory planner that couples an evidential learning-based SCR poseestimator with a receding-horizon trajectory optimizer. The optimizer steers the onboard camera toward reliablescene coordinates with low uncertainty, while a fixed-lag smoother fuses the low-rate SCR pose estimates with high-rate IMU data to provide a high-quality, high-rate pose estimate. In simulation, our planner reduces translationand rotation RMSE by at least 4.9% and 30.8% relative to baselines, respectively. Hardware-in-the-loop experiments validate the feasibility of our proposed trajectory planner under close-to-real deployment conditions.

Generative models have become increasingly powerful tools for robot motion generation, enabling flexible and multimodal trajectory generation across various tasks. Yet, most existing approaches remain limited in handling multiple types of constraints, such as collision avoidance, actuation limits, and dynamic consistency, which are typically addressed individually or heuristically. In this work, we propose UniConFlow, a unified constrained flow matching-based framework for trajectory generation that systematically incorporates both equality and inequality constraints. Moreover, UniConFlow introduces a novel prescribed-time zeroing function that shapes a time-varying guidance field during inference, allowing the generation process to adapt to varying system models and task requirements. Furthermore, to further address the computational challenges of long-horizon and high-dimensional trajectory generation, we propose two practical strategies for the terminal constraint enforcement and inference process: a violation-segment extraction protocol that precisely localizes and refines only the constraint-violating portions of trajectories, and a trajectory compression method that accelerates optimization in a reduced-dimensional space while preserving high-fidelity reconstruction after decoding. Empirical validation across three experiments, including a double inverted pendulum, a real-to-sim car racing task, and a sim-to-real manipulation task, demonstrates that UniConFlow outperforms state-of-the-art generative planners and conventional optimization baselines, achieving superior performance on certified motion planning metrics such as safety, kinodynamic consistency, and action feasibility. Project page is available at: https://uniconflow.github.io.

This article proposes a new path planning method for addressing multi-level terrain situations. The proposed method includes innovations in three aspects: 1) the pre-processing of point cloud maps with a multi-level skip-list structure and data-slimming algorithm for well-organized and simplified map formalization and management, 2) the direct acquisition of local traversability indexes through vehicle and point cloud interaction analysis, which saves work in surface fitting, and 3) the assignment of traversability indexes on a multi-level connectivity graph to generate a weighted traversability graph for generally search-based path planning. The A* algorithm is modified to utilize the traversability graph to generate a short and safe path. The effectiveness and reliability of the proposed method are verified through indoor and outdoor experiments conducted in various environments, including multi-floor buildings, woodland, and rugged mountainous regions. The results demonstrate that the proposed method can properly address 3D path planning problems for ground vehicles in a wide range of situations.

Autonomous mobile robots must operate smoothly in human-populated indoor environments, where onboard-only sensing suffers from limited perception range and occlusions, and socially compliant motion planning must account for human psychological comfort zones. These factors complicate accurate recognition of human intentions and the generation of comfortable, social-aware trajectories. To address these challenges, we propose SAP-CoPE, an indoor navigation framework that integrates cooperative infrastructure sensing, human pose estimation, and a social-aware MPC planner. In the perception module, an uncertainty-aware optimization problem is formulated to estimate 3D human poses from camera observations, with optional incorporation of sparse LiDAR point-cloud measurements, while considering human joint coherence. In the planning module, the estimated human pose and orientation are used to construct a personal-space (PS) field within an MPC framework, enabling the robot to account for both physical safety and psychological comfort. Extensive real-world evaluations demonstrate the effectiveness of the proposed approach in generating social-aware trajectories for autonomous systems.

Kinematic motion planners are among the most widely used control approaches in robotic applications. By modeling the robot as a first-order system, they generate a feedback-based desired velocity field that guides the robot toward a target while avoiding obstacles. However, extending such feedback planners to systems with higher-order dynamics, while preserving safety and stability properties, is not a straightforward task. In the present work, we propose an approach that adapts existing feedback-based kinematic motion planners to second-order autonomous systems while retaining their safety and almost global asymptotic stability guarantees. We consider two general classes of kinematic motion planners: those derived from navigation functions and those defined directly through desired velocity fields without relying on an underlying navigation function. To validate the proposed methodology, two feedback-based kinematic motion planners are adapted to second-order systems and evaluated both in simulations and experimentally.

This work presents an optimization method for generating kinodynamically feasible and collision-free multi-robot trajectories that exploits an incremental denoising scheme in diffusion models. Our key insight is that high-quality trajectories can be discovered merely by denoising noisy trajectories sampled from a distribution. This approach has no learning component, relying instead on only two ingredients: a dynamical model of the robots to obtain feasible trajectories via rollout, and a fitness function to guide denoising with Monte Carlo gradient approximation. The proposed framework iteratively optimizes a deformation for the previous trajectory with the current denoising process, allows anytime refinement as time permits, supports different dynamics, and benefits from GPU acceleration. Our evaluations for differential-drive and holonomic teams with up to 16 robots in 2D and 3D worlds show its ability to discover high-quality solutions faster than other black-box optimization methods such as MPPI. In a 2D holonomic case with 16 robots, it is almost twice as fast. As evidence for feasibility, we demonstrate zero-shot deployment of the planned trajectories on eight multirotors. Code and video: https://github.com/proroklab/d4orm

Ensuring safe driving while maintaining travel efficiency for autonomous vehicles in dynamic and occluded environments is a critical challenge. This paper proposes an occlusion-aware contingency safety-critical planning approach for real-time autonomous driving. Leveraging reachability analysis for risk assessment, forward reachable sets of phantom vehicles are used to derive risk-aware dynamic velocity boundaries. These velocity boundaries are incorporated into a biconvex nonlinear programming (NLP) formulation that formally enforces safety using spatiotemporal barrier constraints, while simultaneously optimizing exploration and fallback trajectories within a receding horizon planning framework. To enable real-time computation and coordination between trajectories, we employ the consensus alternating direction method of multipliers (ADMM) to decompose the biconvex NLP problem into low-dimensional convex subproblems. The effectiveness of the proposed approach is validated through simulations and real-world experiments in occluded intersections. Experimental results demonstrate enhanced safety and improved travel efficiency, enabling real-time safe trajectory generation in dynamic occluded intersections under varying obstacle conditions. The project page is available at https://zack4417.github.io/oacp-website/.

Off-road navigation is critical for a wide range of field robotics applications from planetary exploration to disaster response. However, it remains a longstanding challenge due to unstructured environments and the inherently complex terrain-vehicle interactions. Traditional physics-based methods struggle to accurately capture the nonlinear dynamics underlying these interactions, while purely data-driven approaches often overfit to specific motion patterns, vehicle geometries, or platforms, limiting their generalization in diverse, real-world scenarios. To address these limitations, we introduce AnyNav, a vision-based friction estimation and navigation framework grounded in neuro-symbolic principles. Our approach integrates neural networks for visual perception with symbolic physical models for reasoning about terrain-vehicle dynamics. To enable self-supervised learning in real-world settings, we adopt the imperative learning paradigm, employing bilevel optimization to train the friction network through physics-based optimization. This explicit incorporation of physical reasoning substantially enhances generalization across terrains, vehicle types, and operational conditions. Leveraging the predicted friction coefficients, we further develop a physics-informed navigation system capable of generating physically feasible, time-efficient paths together with corresponding speed profiles. We demonstrate that AnyNav seamlessly transfers from simulation to real-world robotic platforms, exhibiting strong robustness across different four-wheeled vehicles and diverse off-road environments.

Multi-agent path finding (MAPF) in large networks is computationally challenging. An approach for MAPF is prioritized planning (PP), in which agents plan sequentially according to their priority. Albeit a computationally efficient approach for MAPF, the solution quality strongly depends on the prioritization. Most prioritizations rely either on heuristics, which do not generalize well, or iterate to find adequate priorities, which costs computational effort. In this work, we show how agents can compute with multiple prioritizations simultaneously. Our approach is general as it does not rely on domain-specific knowledge. The context of this work is multi-agent motion planning (MAMP) with a receding horizon subject to computation time constraints. MAMP considers the system dynamics in more detail compared to MAPF. In numerical experiments on MAMP, we demonstrate that our approach achieves near-optimal prioritization and outperforms state-of-the-art methods with only a minor increase in computation time. We show real-time capability in an experiment on a road network with ten vehicles in our Cyber-Physical Mobility Lab.

Despite decades of research, the dynamic mechanisms governing human locomotion remain incompletely understood. Current motor control models only partially capture these complex dynamics, a theoretical gap that likely limits the clinical efficacy of modern lower-limb rehabilitation therapies. Recently, it has been proposed that human locomotion is planned in task-space by explicitly exploiting the gravitational pull acting on the Center of Mass (CoM). Building upon this concept, this paper presents a novel deterministic task-space planner for straight, constant-speed locomotion. Our model formulates the CoM trajectory in the transverse plane as a harmonic oscillator moving at a constant forward velocity, while demonstrating that the vertical CoM trajectory is actively regulated through localized ankle strategies. We derive closed-form equations that successfully generate human-like trajectories for both the CoM and the swing foot. The model's predictive accuracy is empirically validated against the kinematic behavior of 12 healthy subjects walking at three self-selected speeds. Furthermore, the proposed planner is mathematically compatible with an extended formulation of the Passive Motion Paradigm ($λ_0$-PMP), establishing a comprehensive theoretical foundation for decentralized, hierarchical architectures of semi-autonomous controllers in both human motor control and assistive robotics.