Maxim Likhachev

AAAI Conference 2026 Conference Paper

Dynamic Agent Grouping ECBS: Scaling Windowed Multi-Agent Path Finding with Completeness Guarantees

• Tiannan Zhang
• Rishi Veerapaneni
• Shao-Hung Chan
• Jiaoyang Li
• Maxim Likhachev

Multi-Agent Path Finding (MAPF) is the problem of finding a set of collision-free paths for a team of agents. Although several MAPF methods that solve full-horizon MAPF have completeness guarantees, very few MAPF methods that plan partial paths have completeness guarantees. Recent work introduced the Windowed Complete MAPF (WinC-MAPF) framework, which shows how windowed optimal MAPF solvers (e.g., SS-CBS) can use heuristic updates and disjoint agent groups to maintain completeness even when planning partial paths. A core limitation of WinC-MAPF is that it requires optimal MAPF solvers. Our main contribution is to extend WinC-MAPF by showing how we can use a bounded suboptimal solver while maintaining completeness. In particular, we design Dynamic Agent Grouping ECBS (DAG-ECBS) which dynamically creates and plans agent groups while maintaining that each agent group solution is bounded suboptimal. We prove how DAG-ECBS can maintain completeness in the WinC-MAPF framework and can improve scalability compared to windowed ECBS which does not have completeness guarantees. More broadly, our work serves as a blueprint for designing more MAPF methods that can use the WinC-MAPF framework

IJCAI Conference 2025 Conference Paper

Attractor-based Closed List Search: Sparsifying the Closed List for Efficient Memory-Constrained Planning

• Alvin Zou
• Muhammad Suhail Saleem
• Maxim Likhachev

Best-first search algorithms such as A* and Weighted A* are widely used tools. However, their high memory requirements often make them impractical for memory-constrained applications, such as on-board planning for interplanetary rovers, drones, and embedded systems. One popular strategy among memory-efficient approaches developed to address this challenge is to eliminate or sparsify the Closed list, a structure that tracks states explored by the search. However, such methods often incur substantial overhead in runtime, requiring recursive searches for solution reconstruction. In this work, we propose Attractor-based Closed List Search (ACLS), a novel framework that sparsely represents the Closed list using a small subset of states, termed attractors. ACLS intelligently identifies attractor states in a way that enables efficient solution reconstruction while preserving theoretical guarantees on the quality of the solution. Furthermore, we also introduce a lazy variant, Lazy-ACLS, which defers the computation of attractor states until necessary, substantially improving planning speed. We demonstrate the efficacy of ACLS used in conjunction with A*, Weighted A*, and Dijkstra’s searches across multiple domains including 2D and 3D navigation, Sliding Tiles, and Towers of Hanoi. Our experimental results demonstrate that ACLS significantly reduces memory usage, maintaining only 9% of the states typically stored in a Closed list, while achieving comparable planning times and outperforming state-of-the-art approaches. Source code can be found at github. com/alvin-ruihua-zou/ACLS.

AIJ Journal 2025 Journal Article

EMOA*: A framework for search-based multi-objective path planning

• Zhongqiang Ren
• Carlos Hernández
• Maxim Likhachev
• Ariel Felner
• Sven Koenig
• Oren Salzman
• Sivakumar Rathinam
• Howie Choset

In the Multi-Objective Shortest Path Problem (MO-SPP), one has to find paths on a graph that simultaneously minimize multiple objectives. It is not guaranteed that there exists a path that minimizes all objectives, and the problem thus aims to find the set of Pareto-optimal paths from the start to the goal vertex. A variety of multi-objective A*-based search approaches have been developed for this purpose. Typically, these approaches maintain a front set at each vertex during the search process to keep track of the Pareto-optimal paths that reach that vertex. Maintaining these front sets becomes burdensome and often slows down the search when there are many Pareto-optimal paths. In this article, we first introduce a framework for MO-SPP with the key procedures related to the front sets abstracted and highlighted, which provides a novel perspective for understanding the existing multi-objective A*-based search algorithms. Within this framework, we develop two different, yet closely related approaches to maintain these front sets efficiently during the search. We show that our approaches can find all cost-unique Pareto-optimal paths, and analyze their runtime complexity. We implement the approaches and compare them against baselines using instances with three, four and five objectives. Our experimental results show that our approaches run up to an order of magnitude faster than the baselines.

ICLR Conference 2025 Conference Paper

Multi-Robot Motion Planning with Diffusion Models

• Yorai Shaoul
• Itamar Mishani
• Shivam Vats
• Jiaoyang Li 0001
• Maxim Likhachev

Diffusion models have recently been successfully applied to a wide range of robotics applications for learning complex multi-modal behaviors from data. However, prior works have mostly been confined to single-robot and small-scale environments due to the high sample complexity of learning multi-robot diffusion models. In this paper, we propose a method for generating collision-free multi-robot trajectories that conform to underlying data distributions while using only single-robot data. Our algorithm, Multi-robot Multi-model planning Diffusion (MMD), does so by combining learned diffusion models with classical search-based techniques---generating data-driven motions under collision constraints. Scaling further, we show how to compose multiple diffusion models to plan in large environments where a single diffusion model fails to generalize well. We demonstrate the effectiveness of our approach in planning for dozens of robots in a variety of simulated scenarios motivated by logistics environments.

IROS Conference 2025 Conference Paper

RecoveryChaining: Learning Local Recovery Policies for Robust Manipulation

• Shivam Vats
• Devesh K. Jha
• Maxim Likhachev
• Oliver Kroemer
• Diego Romeres

Model-based planners and controllers are commonly used to solve complex manipulation problems as they can efficiently optimize diverse objectives and generalize to long horizon tasks. However, they often fail during deployment due to noisy actuation, partial observability and imperfect models. To enable a robot to recover from such failures, we propose to use hierarchical reinforcement learning to learn a recovery policy. The recovery policy is triggered when a failure is detected based on sensory observations and seeks to take the robot to a state from which it can complete the task using the nominal model-based controllers. Our approach, called RecoveryChaining, uses a hybrid action space, where the model-based controllers are provided as additional nominal options which allows the recovery policy to decide how to recover, when to switch to a nominal controller and which controller to switch to even with sparse rewards. We evaluate our approach in three multi-step manipulation tasks with sparse rewards, where it learns significantly more robust recovery policies than those learned by baselines. We successfully transfer recovery policies learned in simulation to a physical robot to demonstrate the feasibility of sim-to-real transfer with our method.

AAAI Conference 2025 Conference Paper

Windowed MAPF with Completeness Guarantees

• Rishi Veerapaneni
• Muhammad Suhail Saleem
• Jiaoyang Li
• Maxim Likhachev

Traditional multi-agent path finding (MAPF) methods try to compute entire collision free start-goal paths, with several algorithms offering completeness guarantees. However, computing partial paths offers significant advantages including faster planning, adaptability to changes, and enabling decentralized planning. Methods that compute partial paths employ a "windowed" approach and only try to find collision free paths for a limited timestep horizon. While this improves flexibility, this adaptation introduces incompleteness; all existing windowed approaches can become stuck in deadlock or livelock. Our main contribution is to introduce our framework, WinC-MAPF, for Windowed MAPF that enables completeness. Our framework leverages heuristic update insights from single-agent real-time heuristic search algorithms and agent independence ideas from MAPF algorithms. We also develop Single-Step Conflict Based Search (SS-CBS), an instantiation of this framework using a novel modification to CBS. We show how SS-CBS, which only plans a single step and updates heuristics, can effectively solve tough scenarios where existing windowed approaches fail.

ICRA Conference 2025 Conference Paper

Work Smarter Not Harder: Simple Imitation Learning with CS-PIBT Outperforms Large-Scale Imitation Learning for MAPF

• Rishi Veerapaneni
• Arthur Jakobsson
• Kevin Ren
• Samuel Kim
• Jiaoyang Li 0001
• Maxim Likhachev

Multi-Agent Path Finding (MAPF) is the problem of effectively finding efficient collision-free paths for a group of agents in a shared workspace. The MAPF community has largely focused on developing high-performance heuristic search methods. Recently, several works have applied various machine learning (ML) techniques to solve MAPF, usually involving sophisticated architectures, reinforcement learning techniques, and set-ups, but none using large amounts of high-quality supervised data. Our initial objective in this work was to show how simple large-scale imitation learning of high-quality heuristic search methods can lead to state-of-the-art ML MAPF performance. However, we find that, at least with our model architecture, simple large-scale (700k examples with hundreds of agents per example) imitation learning does not produce impressive results. Instead, we find that by using prior work that post-processes MAPF model predictions to resolve 1-step collisions (CS-PIBT), we can train a simple ML MAPF policy in minutes that dramatically outperforms existing ML MAPF policies. This has serious implications for all future ML MAPF policies (with local communication) which currently struggle to scale. In particular, this finding implies that future learnt policies should always (1) use smart 1-step collision shields (e. g, CS-PIBT) and (2) include the collision shield with greedy actions as a baseline (e. g. PIBT), as well as (3) motivates future models to focus on longer horizon / more complex planning as 1-step collisions can be efficiently resolved.

SoCS Conference 2024 Conference Paper

A Data Efficient Framework for Learning Local Heuristics

• Rishi Veerapaneni
• Jonathan Park
• Muhammad Suhail Saleem
• Maxim Likhachev

With the advent of machine learning, there have been several recent attempts to learn effective and generalizable heuristics. Local Heuristic A* (LoHA*) is one recent method that instead of learning the entire heuristic estimate, learns a "local" residual heuristic that estimates the cost to escape a region. LoHA*, like other supervised learning methods, collects a dataset of target values by querying an oracle on many planning problems (in this case, local planning problems). This data collection process can become slow as the size of the local region increases or if the domain requires expensive collision checks. Our main insight is that when an A* search solves a start-goal planning problem it inherently ends up solving multiple local planning problems. We exploit this observation to propose an efficient data collection framework that does

ICAPS Conference 2024 Conference Paper

Accelerating Search-Based Planning for Multi-Robot Manipulation by Leveraging Online-Generated Experiences

• Yorai Shaoul
• Itamar Mishani
• Maxim Likhachev
• Jiaoyang Li 0001

An exciting frontier in robotic manipulation is the use of multiple arms at once. However, planning concurrent motions is a challenging task using current methods. The high-dimensional composite state space renders many well-known motion planning algorithms intractable. Recently, Multi-Agent Path Finding (MAPF) algorithms have shown promise in discrete 2D domains, providing rigorous guarantees. However, widely used conflict-based methods in MAPF assume an efficient single-agent motion planner. This poses challenges in adapting them to manipulation cases where this assumption does not hold, due to the high dimensionality of configuration spaces and the computational bottlenecks associated with collision checking. To this end, we propose an approach for accelerating conflict-based search algorithms by leveraging their repetitive and incremental nature -- making them tractable for use in complex scenarios involving multi-arm coordination in obstacle-laden environments. We show that our method preserves completeness and bounded sub-optimality guarantees, and demonstrate its practical efficacy through a set of experiments with up to 10 robotic arms.

ICRA Conference 2024 Conference Paper

Constant-time Motion Planning with Anytime Refinement for Manipulation

• Itamar Mishani
• Hayden Feddock
• Maxim Likhachev

Robotic manipulators are essential for future autonomous systems, yet limited trust in their autonomy has confined them to rigid, task-specific systems. The intricate configuration space of manipulators, coupled with the challenges of obstacle avoidance and constraint satisfaction, often makes motion planning the bottleneck for achieving reliable and adaptable autonomy. Recently, a class of constant-time motion planners (CTMP) was introduced. These planners employ a preprocessing phase to compute data structures that enable online planning provably guarantee the ability to generate motion plans, potentially sub-optimal, within a user defined time bound. This framework has been demonstrated to be effective in a number of time-critical tasks. However, robotic systems often have more time allotted for planning than the online portion of CTMP requires, time that can be used to improve the solution. To this end, we propose an anytime refinement approach that works in combination with CTMP algorithms. Our proposed framework, as it operates as a constant time algorithm, rapidly generates an initial solution within a user-defined time threshold. Furthermore, functioning as an anytime algorithm, it iteratively refines the solution’s quality within the allocated time budget. This enables our approach to strike a balance between guaranteed fast plan generation and the pursuit of optimization over time. We support our approach by elucidating its analytical properties, showing the convergence of the anytime component towards optimal solutions. Additionally, we provide empirical validation through simulation and real-world demonstrations on a 6 degree-of-freedom robot manipulator, applied to an assembly domain.

SoCS Conference 2024 Conference Paper

From Space-Time to Space-Order: Directly Planning a Temporal Planning Graph by Redefining CBS (Extended Abstract)

• Yu Wu
• Rishi Veerapaneni
• Jiaoyang Li 0001
• Maxim Likhachev

The majority of multi-agent path finding (MAPF) methods compute collision-free space-time paths which require agents to be at a specific location at a specific discretized timestep. However, executing these space-time paths directly on robotic systems is infeasible due to real-time execution differences (e. g. delays) which can lead to collisions. To combat this, current methods translate the space-time paths into a temporal plan graph (TPG) that only requires that agents observe the order in which they navigate through locations where their paths cross. However, planning space-time paths and then post-processing them into a TPG does not reduce the required agent-to-agent coordination, which is fixed once the space-time paths are computed. To that end, we propose a novel algorithm Space-Order CBS that can directly plan a TPG and explicitly minimize coordination. Our main theoretical insight is our novel perspective on viewing a TPG as a set of space-visitation order paths where agents visit locations in relative orders (e. g. 1st vs 2nd) as opposed to specific timesteps. We redefine unique conflicts and constraints for adapting CBS for space-order planning. We experimentally validate how Space-Order CBS can return TPGs which significantly reduce coordination, thus subsequently reducing the amount of agent-agent communication and leading to more robustness to delays during execution.

ICRA Conference 2024 Conference Paper

Improved M4M: Faster and Richer Planning for Manipulation Among Movable Objects in Cluttered 3D Workspaces

• Dhruv Mauria Saxena
• Maxim Likhachev

We are interested in enabling robots to solve difficult pick-and-place manipulation tasks in cluttered and constrained environments. If the robot does not have collision-free access to the object-of-interest (OoI) which it intends to grasp and extract from the workspace, it must reason about which movable objects to rearrange, where to move them, and how it may do so. In recent work [1] we introduced E-M4M, a graph search-based solver for solving such Manipulation tasks Among Movable Objects (MAMO). In this paper we make several improvements to E-M4M – we introduce the use of prehensile or pick-and-place rearrangement actions in addition to pushes; we show that by running it as a depth-first search improves performance; we show how the search can be run "eagerly lazily" to only simulate actions in a physics-based simulator when necessary; finally we relax the assumption that we require perfect knowledge of the physical properties of objects (mass and coefficient of friction in particular). The improved version of E-M4M presented in this paper, I-M4M, is a faster and more versatile MAMO solver with a rich action space. We discuss the impact of the improvements we make in an extensive simulation study and show previously unachievable results on a real-world PR2 robot.

ICAPS Conference 2024 Conference Paper

Improving Learnt Local MAPF Policies with Heuristic Search

• Rishi Veerapaneni
• Qian Wang
• Kevin Ren
• Arthur Jakobsson
• Jiaoyang Li 0001
• Maxim Likhachev

Multi-agent path finding (MAPF) is the problem of finding collision-free paths for a team of agents to reach their goal locations. State-of-the-art classical MAPF solvers typically employ heuristic search to find solutions for hundreds of agents but are typically centralized and can struggle to scale when run with short timeouts. Machine learning (ML) approaches that learn policies for each agent are appealing as these could enable decentralized systems and scale well while maintaining good solution quality. Current ML approaches to MAPF have proposed methods that have started to scratch the surface of this potential. However, state-of-the-art ML approaches produce ``local" policies that only plan for a single timestep and have poor success rates and scalability. Our main idea is that we can improve a ML local policy by using heuristic search methods on the output probability distribution to resolve deadlocks and enable full horizon planning. We show several model-agnostic ways to use heuristic search with learnt policies that significantly improve the policies

ICAPS Conference 2024 Conference Paper

MAPF in 3D Warehouses: Dataset and Analysis

• Qian Wang
• Rishi Veerapaneni
• Yu Wu
• Jiaoyang Li 0001
• Maxim Likhachev

Recent works have made significant progress in multi-agent path finding (MAPF), with modern methods being able to scale to hundreds of agents, handle unexpected delays, work in groups, etc. The vast majority of these methods have focused on 2D "grid world" domains. However, modern warehouses often utilize multi-agent robotic systems that can move in 3D, enabling dense storage but resulting in a more complex multi-agent planning problem. Motivated by this, we introduce and experimentally analyze the application of MAPF to 3D warehouse management, and release the first (see http: //mapf. info/index. php/Main/Benchmarks) open-source 3D MAPF dataset. We benchmark two state-of-the-art MAPF methods, EECBS and MAPF-LNS2, and show how different hyper-parameters affect these methods across various 3D MAPF problems. We also investigate how the warehouse structure itself affects MAPF performance. Based on our experimental analysis, we find that a fast low-level search is critical for 3D MAPF, EECBS

IROS Conference 2024 Conference Paper

PINSAT: Parallelized Interleaving of Graph Search and Trajectory Optimization for Kinodynamic Motion Planning

• Ramkumar Natarajan
• Shohin Mukherjee
• Howie Choset
• Maxim Likhachev

Trajectory optimization is a widely used technique in robot motion planning for letting the dynamics of the system shape and synthesize complex behaviors. Several previous works have shown its benefits in high-dimensional continuous state spaces and under differential constraints. However, long time horizons and planning around obstacles in non-convex spaces pose challenges in guaranteeing convergence or finding optimal solutions. As a result, discrete graph search planners and sampling-based planers are preferred when facing obstacle-cluttered environments. A recently developed algorithm called INSAT effectively combines graph search in the low-dimensional subspace and trajectory optimization in the full-dimensional space for global kinodynamic planning over long horizons. Although INSAT successfully reasoned about and solved complex planning problems, the numerous expensive calls to an optimizer resulted in large planning times, thereby limiting its practical use. Inspired by the recent work on edge-based parallel graph search, we present PINSAT, which introduces systematic parallelization in INSAT to achieve lower planning times and higher success rates, while maintaining significantly lower costs over relevant baselines. We demonstrate PINSAT by evaluating it on 6 DoF kinodynamic manipulation planning with obstacles. We demonstrate PINSAT by evaluating it on two kinodynamic manipulation planning scenarios: (i) a single ball blocking task among obstacles using a 6 DoF ABB arm, and (ii) a multi-ball blocking task where the balls are separated by short time intervals using a 7 DoF KUKA LBR iiwa arm with obstacles.

ICRA Conference 2024 Conference Paper

Preprocessing-based Kinodynamic Motion Planning Framework for Intercepting Projectiles using a Robot Manipulator

• Ramkumar Natarajan
• Hanlan Yang
• Qintong Xie
• Yash Oza
• Manash Pratim Das
• Fahad Islam 0002
• Muhammad Suhail Saleem
• Howie Choset

We are interested in studying sports with robots and starting with the problem of intercepting a projectile moving toward a robot manipulator equipped with a shield. To successfully perform this task, the robot needs to (i) detect the incoming projectile, (ii) predict the projectile’s future motion, (iii) plan a minimum-time rapid trajectory that can evade obstacles and intercept the projectile, and (iv) execute the planned trajectory. These four steps must be performed under the manipulator’s dynamic limits and extreme time constraints (≤ 350ms in our setting) to successfully intercept the projectile. In addition, we want these trajectories to be smooth to reduce the robot’s joint torques and the impulse on the platform on which it is mounted. To this end, we propose a kinodynamic motion planning framework that preprocesses smooth trajectories offline to allow real-time collision-free executions online. We present an end-to-end pipeline along with our planning framework, including perception, prediction, and execution modules. We evaluate our framework experimentally in simulation and show that it has a higher blocking success rate than the baselines. Further, we deploy our pipeline on a robotic system comprising an industrial arm (ABB IRB-1600) and an onboard stereo camera (ZED 2i), which achieves a 78% success rate in projectile interceptions.

SoCS Conference 2024 Conference Paper

Unconstraining Multi-Robot Manipulation: Enabling Arbitrary Constraints in ECBS with Bounded Sub-Optimality

• Yorai Shaoul
• Rishi Veerapaneni
• Maxim Likhachev
• Jiaoyang Li 0001

Multi-Robot-Arm Motion Planning (M-RAMP) is a challenging problem featuring complex single-agent planning and multi-agent coordination. Recent advancements in extending the popular Conflict-Based Search (CBS) algorithm have made large strides in solving Multi-Agent Path Finding (MAPF) problems. However, fundamental challenges remain in applying CBS to M-RAMP. A core challenge is the existing reliance of the CBS framework on conservative "complete" constraints. These constraints ensure solution guarantees but often result in slow pruning of the search space -- causing repeated expensive single-agent planning calls. Therefore, even though it is possible to leverage domain knowledge and design incomplete M-RAMP-specific CBS constraints to more efficiently prune the search, using these constraints would render the algorithm itself incomplete. This forces practitioners to choose between efficiency and completeness. In light of these challenges, we propose a novel algorithm, Generalized ECBS, aimed at removing the burden of choice between completeness and efficiency in MAPF algorithms. Our approach enables the use of arbitrary constraints in conflict-based algorithms while preserving completeness and bounding sub-optimality. This enables practitioners to capitalize on the benefits of arbitrary constraints and opens a new space for constraint design in MAPF that has not been explored. We provide a theoretical analysis of our algorithms, propose new "incomplete" constraints, and demonstrate their effectiveness through experiments in M-RAMP.

SoCS Conference 2023 Conference Paper

A-ePA*SE: Anytime Edge-Based Parallel A* for Slow Evaluations

• Hanlan Yang
• Shohin Mukherjee
• Maxim Likhachev

Anytime search algorithms are useful for planning problems where a solution is desired under a limited time budget. Anytime algorithms first aim to provide a feasible solution quickly and then attempt to improve it until the time budget expires. On the other hand, parallel search algorithms utilize the multithreading capability of modern processors to speed up the search. One such algorithm, ePA*SE (Edge-Based Parallel A* for Slow Evaluations), parallelizes edge evaluations to achieve faster planning and is especially useful in domains with expensive-to-compute edges. In this work, we propose an extension that brings the anytime property to ePA*SE, resulting in A-ePA*SE. We evaluate A-ePA*SE experimentally and show that it is significantly more efficient than other anytime search methods. The open-source code for A-ePA*SE, along with the baselines, is available here: https: //github. com/shohinm/parallel_search

AAAI Conference 2023 Conference Paper

Effective Integration of Weighted Cost-to-Go and Conflict Heuristic within Suboptimal CBS

• Rishi Veerapaneni
• Tushar Kusnur
• Maxim Likhachev

Conflict-Based Search (CBS) is a popular multi-agent path finding (MAPF) solver that employs a low-level single agent planner and a high-level constraint tree to resolve conflicts. The vast majority of modern MAPF solvers focus on improving CBS by reducing the size of this tree through various strategies with few methods modifying the low level planner. Typically low level planners in existing CBS methods use an unweighted cost-to-go heuristic, with suboptimal CBS methods also using a conflict heuristic to help the high level search. In this paper, we show that, contrary to prevailing CBS beliefs, a weighted cost-to-go heuristic can be used effectively alongside the conflict heuristic in two possible variants. In particular, one of these variants can obtain large speedups, 2-100x, across several scenarios and suboptimal CBS methods. Importantly, we discover that performance is related not to the weighted cost-to-go heuristic but rather to the relative conflict heuristic weight's ability to effectively balance low-level and high-level work. Additionally, to the best of our knowledge, we show the first theoretical relation of prioritized planning and bounded suboptimal CBS and demonstrate that our methods are their natural generalization.

ICRA Conference 2023 Conference Paper

Efficient Recovery Learning using Model Predictive Meta-Reasoning

• Shivam Vats
• Maxim Likhachev
• Oliver Kroemer

Operating under real world conditions is challenging due to the possibility of a wide range of failures induced by execution errors and state uncertainty. In relatively benign settings, such failures can be overcome by retrying or executing one of a small number of hand-engineered recovery strategies. By contrast, contact-rich sequential manipulation tasks, like opening doors and assembling furniture, are not amenable to exhaustive hand-engineering. To address this issue, we present a general approach for robustifying manipulation strategies in a sample-efficient manner. Our approach incrementally improves robustness by first discovering the failure modes of the current strategy via exploration in simulation and then learning additional recovery skills to handle these failures. To ensure efficient learning, we propose an online algorithm called Meta-Reasoning for Skill Learning (MetaReSkill) that monitors the progress of all recovery policies during training and allocates training resources to recoveries that are likely to improve the task performance the most. We use our approach to learn recovery skills for door-opening and evaluate them both in simulation and on a real robot with little fine-tuning. Compared to open-loop execution, our experiments show that even a limited amount of recovery learning improves task success substantially from 71% to 92. 4% in simulation and from 75% to 90% on a real robot.

SoCS Conference 2023 Conference Paper

GePA*SE: Generalized Edge-Based Parallel A* for Slow Evaluations

• Shohin Mukherjee
• Maxim Likhachev

Parallel search algorithms have been shown to improve planning speed by harnessing the multithreading capability of modern processors. One such algorithm PA*SE achieves this by parallelizing state expansions, whereas another algorithm ePA*SE achieves this by effectively parallelizing edge evaluations. ePA*SE targets domains in which the action space comprises actions with expensive but similar evaluation times. However, in a number of robotics domains, the action space is heterogenous in the computational effort required to evaluate the cost of an action and its outcome. Motivated by this, we introduce GePA*SE: Generalized Edge-based Parallel A* for Slow Evaluations, which generalizes the key ideas of PA*SE and ePA*SE, i. e. , parallelization of state expansions and edge evaluations, respectively. This extends its applicability to domains that have actions requiring varying computational effort to evaluate them. The open-source code for GePA*SE, along with the baselines, is available here: https: //github. com/shohinm/parallel_search

ICAPS Conference 2023 Conference Paper

Learning Local Heuristics for Search-Based Navigation Planning

• Rishi Veerapaneni
• Muhammad Suhail Saleem
• Maxim Likhachev

Graph search planning algorithms for navigation typically rely heavily on heuristics to efficiently plan paths. As a result, while such approaches require no training phase and can directly plan long horizon paths, they often require careful hand designing of informative heuristic functions. Recent works have started bypassing hand designed heuristics by using machine learning to learn heuristic functions that guide the search algorithm. While these methods can learn complex heuristic functions from raw input, they i) require significant training and ii) do not generalize well to new maps and longer horizon paths. Our contribution is showing that instead of learning a global heuristic estimate, we can define and learn local heuristics which results in a significantly smaller learning problem and improves generalization. We show that using such local heuristics can reduce node expansions by 2-20x while maintaining bounded suboptimality, are easy to train, and generalize to new maps & long horizon plans.

ICRA Conference 2023 Conference Paper

Operating with Inaccurate Models by Integrating Control-Level Discrepancy Information into Planning

• Ellis Ratner
• Claire J. Tomlin
• Maxim Likhachev

Typical robotic systems rely on models for planning. Therefore, the quality of the robot's behavior is heavily dependent on how accurately the model can predict the outcome of the robot's actions in the environment. A challenge, however, is that no model is perfect; moreover, we often do not know where discrepancies between the model's prediction and the actual outcome occur prior to observing executions in the real-world. One way to address this is to bias the planner away from these discrepancies by inflating the cost of states and actions where we previously observed the model to be inaccurate. Making such decisions about where and how to bias purely at the planning-level, however, neglects valuable information from the control-level, which gives a more fine-grained understanding of where and how the model went wrong during execution. Based on this observation, our key idea is to first infer a statistical model over discrepancies in the control-level's model. Then, we translate this model to the planning-level, where we use it to more informatively bias the planner away from states and actions where the model's predicted outcome is likely to be inaccurate. We demonstrate that our framework enables a robot to complete tasks, despite an inaccurate planning model, with greater efficiency than existing approaches. We do so through an experimental evaluation in simulation and real-robot experiments on NASA's Astrobee free-flyer.

ICRA Conference 2023 Conference Paper

Planning for Complex Non-prehensile Manipulation Among Movable Objects by Interleaving Multi-Agent Pathfinding and Physics-Based Simulation

• Dhruv Mauria Saxena
• Maxim Likhachev

Real-world manipulation problems in heavy clutter require robots to reason about potential contacts with objects in the environment. We focus on pick-and-place style tasks to retrieve a target object from a shelf where some ‘movable’ objects must be rearranged in order to solve the task. In particular, our motivation is to allow the robot to reason over and consider non-prehensile rearrangement actions that lead to complex robot-object and object-object interactions where multiple objects might be moved by the robot simultaneously, and objects might tilt, lean on each other, or topple. To support this, we query a physics-based simulator to forward simulate these interaction dynamics which makes action evaluation during planning computationally very expensive. To make the planner tractable, we establish a connection between the domain of Manipulation Among Movable Objects and Multi-Agent Pathfinding that lets us decompose the problem into two phases our M4M algorithm iterates over. First we solve a multi-agent planning problem that reasons about the configurations of movable objects but does not forward simulate a physics model. Next, an arm motion planning problem is solved that uses a physics-based simulator but does not search over possible configurations of movable objects. We run simulated and real-world experiments with the PR2 robot and compare against relevant baseline algorithms. Our results highlight that M4M generates complex 3D interactions, and solves at least twice as many problems as the baselines with competitive performance.

ICAPS Conference 2023 Conference Paper

Planning for Manipulation among Movable Objects: Deciding Which Objects Go Where, in What Order, and How

• Dhruv Mauria Saxena
• Maxim Likhachev

We are interested in pick-and-place style robot manipulation tasks in cluttered and confined 3D workspaces among movable objects that may be rearranged by the robot and may slide, tilt, lean or topple. A recently proposed algorithm, M4M, determines which objects need to be moved and where by solving a Multi-Agent Pathfinding (MAPF) abstraction of this problem. It then utilises a nonprehensile push planner to compute actions for how the robot might realise these rearrangements and a rigid body physics simulator to check whether the actions satisfy physics constraints encoded in the problem. However, M4M greedily commits to valid pushes found during planning, and does not reason about orderings over pushes if multiple objects need to be rearranged. Furthermore, M4M does not reason about other possible MAPF solutions that lead to different rearrangements and pushes. In this paper, we extend M4M and present Enhanced-M4M (E-M4M) -- a systematic graph search-based solver that searches over orderings of pushes for movable objects that need to be rearranged and different possible rearrangements of the scene. We introduce several algorithmic optimisations to circumvent the increased computational complexity, discuss the space of problems solvable by E-M4M and show that experimentally, both on the real robot and in simulation, it significantly outperforms the original M4M algorithm, as well as other state-of-the-art alternatives when dealing with complex scenes.

ICAPS Conference 2023 Conference Paper

Runahead A*: Speculative Parallelism for A* with Slow Expansions

• Mohammad Bakhshalipour
• Mohamad Qadri
• Dominic Guri
• Seyed Borna Ehsani
• Maxim Likhachev
• Phillip B. Gibbons

A* suffers from limited parallelism. The maximum level of traditional parallelism in A* is the same as the degree of the search graph nodes, which is too small in many applications. As such, A* cannot fully leverage the multithreading capabilities of modern processors. In this paper, we go beyond traditional parallelism and introduce speculative parallelism for A*. We observe that A*

ICRA Conference 2023 Conference Paper

Torque-Limited Manipulation Planning through Contact by Interleaving Graph Search and Trajectory Optimization

• Ramkumar Natarajan
• Garrison L. H. Johnston
• Nabil Simaan
• Maxim Likhachev
• Howie Choset

Robots often have to perform manipulation tasks in close proximity to people (Fig 1). As such, it is desirable to use a robot arm that has limited joint torques so as to not injure the nearby person. Unfortunately, these limited torques then limit the payload capability of the arm. By using contact with the environment, robots can expand their reachable workspace that, otherwise, would be inaccessible due to exceeding actuator torque limits. We adapt our recently developed INSAT algorithm [1] to tackle the problem of torque-limited whole arm manipulation planning through contact. INSAT requires no prior over contact mode sequence and no initial template or seed for trajectory optimization. INSAT achieves this by interleaving graph search to explore the manipulator joint configuration space with incremental trajectory optimizations seeded by neighborhood solutions to find a dynamically feasible trajectory through contact. We demonstrate our results on a variety of manipulators and scenarios in simulation. We also experimentally show our planner exploiting robot-environment contact for the pick and place of a payload using a Kinova Gen3 robot. In comparison to the same trajectory running in free space, we experimentally show that the utilization of bracing contacts reduces the overall torque required to execute the trajectory.

ICRA Conference 2022 Conference Paper

AMRA*: Anytime Multi-Resolution Multi-Heuristic A

• Dhruv Mauria Saxena
• Tushar Kusnur
• Maxim Likhachev

Heuristic search-based motion planning algorithms typically discretise the search space in order to solve the shortest path problem. Their performance is closely related to this discretisation. A fine discretisation allows for better approximations of the continuous search space, but makes the search for a solution more computationally costly. A coarser resolution might allow the algorithms to find solutions quickly at the expense of quality. For large state spaces, it can be beneficial to search for solutions across multiple resolutions even though defining the discretisations is challenging. The recently proposed algorithm Multi-Resolution A* (MRA*) searches over multiple resolutions. It traverses large areas of obstacle-free space and escapes local minima at a coarse resolution. It can also navigate so-called narrow passageways at a finer resolution. In this work, we develop AMRA*, an anytime version of MRA*, AMRA* tries to find a solution quickly using the coarse resolution as much as possible. It then refines the solution by relying on the fine resolution to discover better paths that may not have been available at the coarse resolution. In addition to being anytime, AMRA* can also leverage information sharing between multiple heuristics. We prove that AMRA* is complete and optimal (in-the-limit of time) with respect to the finest resolution. We show its performance on 2D grid navigation and 4D kinodynamic planning problems.

SoCS Conference 2022 Conference Paper

Effectively Incorporating Weighted Cost-to-go Heuristic in Suboptimal CBS (Extended Abstract)

• Rishi Veerapaneni
• Tushar Kusnur
• Maxim Likhachev

Conflict-Based Search (CBS) is a popular multi-agent path finding (MAPF) solver that employs a low-level single agent planner and a high-level constraint tree to resolve conflicts. The majority of modern MAPF solvers focus on improving CBS by reducing the size of this tree through various strategies with few methods modifying the low level planner. All low level planners in existing CBS methods use an unweighted cost-to-go heuristic, with suboptimal CBS methods also using a conflict heuristic to help the high level search. Contrary to prevailing beliefs, we show that the cost-to-go heuristic can be used significantly more effectively by weighting it in a specific manner alongside the conflict heuristic. We introduce two variants of doing so and demonstrate that this change can lead to 2-100x speedups in certain scenarios. Additionally, we show the first theoretical relation of prioritized planning and bounded suboptimal CBS and demonstrate that our methods are their natural generalization.

SoCS Conference 2022 Conference Paper

Enhanced Multi-Objective A* Using Balanced Binary Search Trees

• Zhongqiang Ren
• Richard Zhan
• Sivakumar Rathinam
• Maxim Likhachev
• Howie Choset

This work addresses a Multi-Objective Shortest Path Problem (MO-SPP) on a graph where the goal is to find a set of Pareto-optimal solutions from a start node to a destination in the graph. A family of approaches based on MOA* have been developed to solve MO-SPP in the literature. Typically, these approaches maintain a "frontier" set at each node during the search process to keep track of the non-dominated, partial paths to reach that node. This search process becomes computationally expensive when the number of objectives increases as the number of Pareto-optimal solutions becomes large. In this work, we introduce a new method to efficiently maintain these frontiers for multiple objectives by incrementally constructing balanced binary search trees within the MOA* search framework. We first show that our approach correctly finds the Pareto-optimal front, and then provide extensive simulation results for problems with three, four and five objectives to show that our method runs faster than existing techniques by up to an order of magnitude.

SoCS Conference 2022 Conference Paper

ePA*SE: Edge-Based Parallel A* for Slow Evaluations

• Shohin Mukherjee
• Sandip Aine
• Maxim Likhachev

Parallel search algorithms harness the multithreading capability of modern processors to achieve faster planning. One such algorithm is PA*SE (Parallel A* for Slow Expansions), which parallelizes state expansions to achieve faster planning in domains where state expansions are slow. In this work, we propose ePA*SE (Edge-Based Parallel A* for Slow Evaluations) that improves on PA*SE by parallelizing edge evaluations instead of state expansions. This makes ePA*SE more efficient in domains where edge evaluations are expensive and need varying amounts of computational effort, which is often the case in robotics. On the theoretical front, we show that ePA*SE provides rigorous optimality guarantees. In addition, ePA*SE can be trivially extended to handle an inflation weight on the heuristic resulting in a bounded suboptimal algorithm w-ePA*SE (Weighted ePA*SE) that trades off optimality for faster planning. On the experimental front, we validate the proposed algorithm in two different planning domains: 1) motion planning for 3D humanoid navigation and 2) task and motion planning for a dual-arm robotic assembly task. We show that ePA*SE can be significantly more efficient than PA*SE and other alternatives. The open-source code for ePA*SE along with the baselines is available here: https: //github. com/shohinm/parallel_search

ICAPS Conference 2022 Conference Paper

Euclidean Distance-Optimal Post-processing of Grid-Based Paths

• Guru Koushik
• Sandip Aine
• Maxim Likhachev

Paths planned over grids can often be suboptimal in an Euclidean space and contain a large number of unnecessary turns. Consequently, researchers have looked into post-processing techniques to improve the paths after they are planned. In this paper, we propose a novel post-processing technique, called Homotopic Visibility Graph Planning (HVG) which differentiates itself from existing post-processing methods in that it is guaranteed to shorten the path such that it is at least as short as the provably shortest path that lies within the same topological class as the initially computed path. We propose the algorithm, provide proofs and compare it experimentally against other post-processing methods and any-angle planning algorithms.

ICRA Conference 2022 Conference Paper

Improved Soft Duplicate Detection in Search-Based Motion Planning

• Nader Maray
• Anirudh Vemula
• Maxim Likhachev

Search-based techniques have shown great success in motion planning problems such as robotic navigation by discretizing the state space and precomputing motion primitives. However in domains with complex dynamic constraints, constructing motion primitives in a discretized state space is non-trivial. This requires operating in continuous space which can be challenging for search-based planners as they can get stuck in local minima regions. Previous work [1] on planning in continuous spaces introduced soft duplicate detection which requires search to compute the duplicity of a state with respect to previously seen states to avoid exploring states that are likely to be duplicates, especially in local minima regions. They propose a simple metric utilizing the Euclidean distance between states, and proximity to obstacles to compute the duplicity. In this paper, we improve upon this metric by introducing a kinodynamically informed metric, subtree overlap, between two states as the similarity between their successors that can be reached within a fixed time horizon using kinodynamic motion primitives. This captures the intuition that, due to robot dynamics, duplicate states can be far in Euclidean distance and result in very similar successor states, while non-duplicate states can be close and result in widely different successors. Our approach computes the new metric offline for a given robot dynamics, and stores the subtree overlap value for all possible relative state configurations. During search, the planner uses these precomputed values to speed up duplicity computation, and achieves fast planning times in continuous spaces in addition to completeness and sub-optimality guarantees. Empirically, we show that our improved metric for soft duplicity detection in search-based planning outperforms previous approaches in terms of planning time, by a factor of 1. 5 to 2× on 3D and 5D planning domains with highly constrained dynamics.

SoCS Conference 2022 Conference Paper

MA3: Model-Accuracy Aware Anytime Planning with Simulation Verification for Navigating Complex Terrains

• Manash Pratim Das
• Damon M. Conover
• Sungmin Eum
• Heesung Kwon
• Maxim Likhachev

Off-road and unstructured environments often contain complex patches of various types of terrain, rough elevation changes, deformable objects, etc. An autonomous ground vehicle traversing such environments experiences physical interactions that are extremely hard to model at scale and thus very hard to predict. Nevertheless, planning a safely traversable path through such an environment requires the ability to predict the outcomes of these interactions instead of avoiding them. One approach to doing this is to learn the interaction model offline based on collected data. Unfortunately, though, this requires large amounts of data and can often be brittle. Alternatively, models using physics-based simulators can generate large data and provide a reliable prediction. However, they are very slow to query online within the planning loop. This work proposes an algorithmic framework that utilizes the combination of a learned model and a physics-based simulation model for fast planning. Specifically, it uses the learned model as much as possible to accelerate planning while sparsely using the physics-based simulator to verify the feasibility of the planned path. We provide a theoretical analysis of the algorithm and its empirical evaluation showing a significant reduction in planning times.

ICRA Conference 2022 Conference Paper

Synergistic Scheduling of Learning and Allocation of Tasks in Human-Robot Teams

• Shivam Vats
• Oliver Kroemer
• Maxim Likhachev

We consider the problem of completing a set of $n$ tasks with a human-robot team using minimum effort. In many domains, teaching a robot to be fully autonomous can be counterproductive if there are finitely many tasks to be done. Rather, the optimal strategy is to weigh the cost of teaching a robot and its benefit- how many new tasks it allows the robot to solve autonomously. We formulate this as a planning problem where the goal is to decide what tasks the robot should do autonomously (act), what tasks should be delegated to a human (delegate) and what tasks the robot should be taught (learn) so as to complete all the given tasks with minimum effort. This planning problem results in a search tree that grows expo-nentially with $n$ - making standard graph search algorithms intractable. We address this by converting the problem into a mixed integer program that can be solved efficiently using off-the-shelf solvers with bounds on solution quality. To predict the benefit of learning, we use an approximate simulation model of the tasks to train a precondition model that is parameterized by the training task. Finally, we evaluate our approach on peg insertion and Lego stacking tasks- both in simulation and real-world, showing substantial savings in human effort.

ICRA Conference 2021 Conference Paper

Affordable Autonomy through Cooperative Sensing and Planning

• Paritosh Kelkar
• Parth Chopra
• Savio Pereira
• Dan DeLano
• Aaron Miller
• Kyungzun Rim
• Samer Rajab
• Jonathan Butzke

It is widely anticipated that Level-4 4 (L4) autonomous vehicles - vehicles capable of autonomously performing all driving functions under certain conditions - will eventually become available to the public. However, these vehicles are likely to be expensive due to the additional requirements on hardware, making them potentially inaccessible to the average consumer. This paper introduces the concept of Affordable Autonomy through Cooperative Sensing and Planning (AACSP). This concept enables a vehicle equipped with lower-end sensors such as a Level-2 6 vehicle - vehicle capable of limited lateral and longitudinal control but requiring a driver to stay engaged - to enter into an autonomous mode whenever it is in proximity with a Level-4 vehicle traveling along the same route. The paper describes the system along with the technical components of cooperative planning & state coordination in detail while giving an overview of the cooperative perception and localization modules that are required to implement the AACSP concept. The paper also presents results from simulation and real vehicles.

ICRA Conference 2021 Conference Paper

Alternative Paths Planner (APP) for Provably Fixed-time Manipulation Planning in Semi-structured Environments

• Fahad Islam 0002
• Chris Paxton 0001
• Clemens Eppner
• Bryan Peele
• Maxim Likhachev
• Dieter Fox

In many applications, including logistics and manufacturing, robot manipulators operate in semi-structured environments alongside humans or other robots. These environments are largely static, but they may contain some movable obstacles that the robot must avoid. Manipulation tasks in these applications are often highly repetitive, but require fast and reliable motion planning capabilities, often under strict time constraints. Existing preprocessing-based approaches are beneficial when the environments are highly-structured, but their performance degrades in the presence of movable obstacles, since these are not modelled a priori. We propose a novel preprocessing-based method called Alternative Paths Planner (APP) that provides provably fixed-time planning guarantees in semi-structured environments. APP plans a set of alternative paths offline such that, for any configuration of the movable obstacles, at least one of the paths from this set is collision-free. During online execution, a collision-free path can be looked up efficiently within a few microseconds. We evaluate APP on a 7 DoF robot arm in semi-structured domains of varying complexity and demonstrate that APP is several orders of magnitude faster than state-of-the-art motion planners for each domain. We further validate this approach with real-time experiments on a robotic manipulator.

AAAI Conference 2021 Conference Paper

CMAX++: Leveraging Experience in Planning and Execution using Inaccurate Models

• Anirudh Vemula
• J. Andrew Bagnell
• Maxim Likhachev

Given access to accurate dynamical models, modern planning approaches are effective in computing feasible and optimal plans for repetitive robotic tasks. However, it is difficult to model the true dynamics of the real world before execution, especially for tasks requiring interactions with objects whose parameters are unknown. A recent planning approach, CMAX, tackles this problem by adapting the planner online during execution to bias the resulting plans away from inaccurately modeled regions. CMAX, while being provably guaranteed to reach the goal, requires strong assumptions on the accuracy of the model used for planning and fails to improve the quality of the solution over repetitions of the same task. In this paper we propose CMAX++, an approach that leverages real-world experience to improve the quality of resulting plans over successive repetitions of a robotic task. CMAX++ achieves this by integrating model-free learning using acquired experience with model-based planning using the potentially inaccurate model. We provide provable guarantees on the completeness and asymptotic convergence of CMAX++ to the optimal path cost as the number of repetitions increases. CMAX++ is also shown to outperform baselines in simulated robotic tasks including 3D mobile robot navigation where the track friction is incorrectly modeled, and a 7D pick-and-place task where the mass of the object is unknown leading to discrepancy between true and modeled dynamics.

IROS Conference 2021 Conference Paper

Disruption-Limited Planning for Robot Navigation in Dynamic Environments

• Sandip Aine
• Yash Oza
• Maxim Likhachev

Path planning in the presence of dynamic obstacles is a fundamental problem in robotics with widespread applications. A typical approach to such problems is that a robot predicts the trajectories of dynamic obstacles, and plans its path while avoiding them. Such a formulation becomes limiting though for scenarios where an agent cannot complete its task efficiently, without disrupting the movement of dynamic obstacles. For example, when merging in heavy traffic or navigating through crowded corridors. In this paper, we propose a paradigm for planning in dynamic environments, called Disruption-Limited Planning (DLP), that allows a robot to disrupt the motions of dynamic obstacles in order to accomplish its task. DLP builds on the premise that while a robot may have to disrupt others’ trajectories to achieve its goals, it should try to limit the disruption. DLP assumes that it can estimate others’ response to its own actions/plans, and plans its own path while ensuring that no other agents’ disrupted trajectory cost gets worse than w-times their initial trajectory costs. While our formulation is motivated by the Stackelberg competitions, we show that DLP can be both more expressive and computationally more efficient compared to a Stackelberg planner. We present DLP paradigm, develop its efficient implementation based on A*, analyze its theoretical properties, and apply it to multiple planning in dynamic environment problems, including x, y, time planning, planning for self-driving, and planning for arm manipulation. We compare DLP with purely altruistic, purely egocentric, and optimal Stackelberg planners, demonstrating the efficacy of DLP over these alternatives.

SoCS Conference 2021 Conference Paper

Fast Bounded Suboptimal Probabilistic Planning with Clear Preferences on Missing Information

• Ishani Chatterjee 0001
• Tushar Kusnur
• Maxim Likhachev

In the real-world, robots must often plan despite the environment being partially known. This frequently necessitates planning under uncertainty over missing information about the environment. Unfortunately, the computational expense of such planning often precludes its scalability to real-world problems. The Probabilistic Planning with Clear Preferences (PPCP) framework focuses on a specific subset of such planning problems wherein there exist clear preferences over the actual values of missing information (Likhachev and Stenz 2009). PPCP exploits the existence and knowledge of these preferences to perform provably optimal planning via a series of deterministic A*-like searches over particular instantiations of the environment. Such decomposition leads to much better scalability with respect to both the size of a problem and the amount of missing information in it. The run-time of PPCP however is a function of the number of searches it has to run until convergence. In this paper, we make a key observation that the number of searches PPCP has to run can be dramatically decreased if each search computes a plan that minimizes the amount of missing information it relies upon. To that end, we introduce Fast-PPCP, a novel planning algorithm that computes a provably bounded suboptimal policy using significantly lesser number of searches than that required to find an optimal policy. We present Fast-PPCP with its theoretical analysis, compare with common alternative approaches to planning under uncertainty over missing information, and experimentally show that Fast-PPCP provides substantial gain in runtime over other approaches while incurring little loss in solution quality.

ICRA Conference 2021 Conference Paper

Manipulation Planning Among Movable Obstacles Using Physics-Based Adaptive Motion Primitives

• Dhruv Mauria Saxena
• Muhammad Suhail Saleem
• Maxim Likhachev

Robot manipulation in cluttered scenes often requires contact-rich interactions with objects. It can be more economical to interact via non-prehensile actions, for example, push through other objects to get to the desired grasp pose, instead of deliberate prehensile rearrangement of the scene. For each object in a scene, depending on its properties, the robot may or may not be allowed to make contact with, tilt, or topple it. To ensure that these constraints are satisfied during non-prehensile interactions, a planner can query a physics-based simulator to evaluate the complex multi-body interactions caused by robot actions. Unfortunately, it is infeasible to query the simulator for thousands of actions that need to be evaluated in a typical planning problem as each simulation is time-consuming. In this work, we show that (i) manipulation tasks (specifically pick-and-place style tasks from a tabletop or a refrigerator) can often be solved by restricting robot-object interactions to adaptive motion primitives in a plan, (ii) these actions can be incorporated as subgoals within a multi-heuristic search framework, and (iii) limiting interactions to these actions can help reduce the time spent querying the simulator during planning by up to 40× in comparison to baseline algorithms. Our algorithm is evaluated in simulation and in the real-world on a PR2 robot using PyBullet as our physics-based simulator. Supplementary video: https://youtu.be/ABQc7JbeJPM.

IROS Conference 2021 Conference Paper

Reactive Long Horizon Task Execution via Visual Skill and Precondition Models

• Shohin Mukherjee
• Chris Paxton 0001
• Arsalan Mousavian
• Adam Fishman
• Maxim Likhachev
• Dieter Fox

Zero-shot execution of unseen robotic tasks is important to allowing robots to perform a wide variety of tasks in human environments, but collecting the amounts of data necessary to train end-to-end policies in the real-world is often infeasible. We describe an approach for sim-to-real training that can accomplish unseen robotic tasks using models learned in simulation to ground components of a simple task planner. We learn a library of parameterized skills, along with a set of predicates-based preconditions and termination conditions, entirely in simulation. We explore a block-stacking task because it has a clear structure, where multiple skills must be chained together, but our methods are applicable to a wide range of other problems and domains, and can transfer from simulation to the real-world with no fine tuning. The system is able to recognize failures and accomplish long-horizon tasks from perceptual input, which is critical for real-world execution. We evaluate our proposed approach in both simulation and in the real-world, showing an increase in success rate from 91. 6% to 98% in simulation and from 10% to 80% success rate in the real-world as compared with naive baselines. For experiment videos including both real-world and simulation, see: https://www.youtube.com/playlist? list=PL-oD0xHUngeLfQmpngYkGFZarstfPOXqX

IROS Conference 2021 Conference Paper

Search-based Path Planning for a High Dimensional Manipulator in Cluttered Environments Using Optimization-based Primitives

• Muhammad Suhail Saleem
• Raghav Sood
• Sho Onodera
• Rohit Arora
• Hiroyuki Kanazawa
• Maxim Likhachev

In this work we tackle the path planning problem for a 21-dimensional snake robot-like manipulator, navigating a cluttered gas turbine for the purposes of inspection. Heuristic search based approaches are effective planning strategies for common manipulation domains. However, their performance on high dimensional systems is heavily reliant on the effectiveness of the action space and the heuristics chosen. The complex nature of our system, reachability constraints, and highly cluttered turbine environment renders naive choices of action spaces and heuristics ineffective. To this extent we have developed i) a methodology for dynamically generating actions based on online optimization that help the robot navigate narrow spaces, ii) a technique for lazily generating these computationally expensive optimization actions to effectively utilize resources, and iii) heuristics that reason about the homotopy classes induced by the blades of the turbine in the robot workspace and a Multi-Heuristic framework which guides the search along the relevant classes. The impact of our contributions is presented through an experimental study in simulation, where the 21 DOF manipulator navigates towards regions of inspection within a turbine.

ICRA Conference 2021 Conference Paper

Search-based Planning for Active Sensing in Goal-Directed Coverage Tasks

• Tushar Kusnur
• Dhruv Mauria Saxena
• Maxim Likhachev

Path planning for robotic coverage is the task of determining a collision-free robot trajectory that observes all points of interest in an environment. Robots employed for such tasks are often capable of exercising active control over onboard observational sensors during navigation. We address the problem of planning robot and sensor trajectories that maximize information gain in such tasks, where the robot needs to cover points of interest with its sensor footprint. Search-based planners in general guarantee completeness and provable bounds on sub-optimality with respect to an underlying graph discretization. However, searching for kinodynamically feasible paths in the joint space of robot and sensor state variables with standard search is computationally expensive. We propose two alternative search-based approaches to this problem. The first solves for robot and sensor trajectories independently in decoupled state spaces while maintaining a history of sensor headings during the search. The second is a two-step approach that first quickly computes a solution in decoupled state spaces and then refines it by searching its local neighborhood in the joint space for a better solution. We evaluate our approaches in simulation with a kinodynamically constrained unmanned aerial vehicle performing coverage over a 2D environment and show their benefits.

IROS Conference 2021 Conference Paper

Search-based Planning with Learned Behaviors for Navigation among Pedestrians

• Ishani Chatterjee 0001
• Yash Oza
• Maxim Likhachev
• Manuela Veloso

Agent control among pedestrians is often approached in one of the three following ways: using predefined behaviors for agent navigation, learning navigation behaviors from data, or search-based planning on a graph where each edge is a feasible action chosen from a set of predefined actions. While the first approach often produces natural looking motions and the second learns and utilizes complex interactions with pedestrians, both lack global reasoning about how to sequence these behaviors to achieve the overall goal. The third approach, namely search-based planning, does incorporate global reasoning but relies on predefined actions that do not involve any interactions with pedestrians or assume predefined interactions that cannot model complex interactions. This is a significant drawback since many situations such as going through a doorway blocked by other people require complex interactions in order to avoid highly suboptimal behaviors or not being able to get to the goal at all. To this end, we propose a search-based planning framework that constructs and searches a graph wherein each edge can be either a predefined action or a learned behavior. We further extend it to deal with the uncertainty arising from introducing learned behaviors. We present the algorithm, go over its theoretical analysis, and present experimental results.

ICAPS Conference 2021 Conference Paper

Speeding Up Search-Based Motion Planning using Expansion Delay Heuristics

• Jasmeet Kaur
• Ishani Chatterjee 0001
• Maxim Likhachev

Suboptimal search algorithms are a popular way to find solutions to planning problems faster by trading off solution optimality for search time. This is often achieved with the help of inadmissible heuristics. Prior work has explored ways to learn such inadmissible heuristics. However, it has focused on learning the heuristic value as an estimate of the cost to reach a goal. In this paper, we present a different approach that computes inadmissible heuristics by learning Expansion Delay for transitions in the state space. Expansion Delay is defined as the number of states expanded during the search between two consecutive states. Expansion Delay can be used as a measure of the depth of local minima regions i. e. , regions where the heuristic(s) are weakly correlated with the true cost-to-goal (Vats, Narayanan and Likhachev 2017). Our key idea is to learn this measure in order to guide the search such that it reduces the total Expansion delay for reaching the goal and hence, avoid local minima regions in the state space. We analyze our method on 3D (x, y, theta) planning and Humanoid footstep planning. We find that the heuristics computed using our technique result in finding feasible plans faster.

ICRA Conference 2020 Conference Paper

Cooperative Perception and Localization for Cooperative Driving

• Aaron Miller
• Kyungzun Rim
• Parth Chopra
• Paritosh Kelkar
• Maxim Likhachev

Fully autonomous vehicles are expected to share the road with less advanced vehicles for a significant period of time. Furthermore, an increasing number of vehicles on the road are equipped with a variety of low-fidelity sensors which provide some perception and localization data, but not at a high enough quality for full autonomy. In this paper, we develop a perception and localization system that allows a vehicle with low-fidelity sensors to incorporate high-fidelity observations from a vehicle in front of it, allowing both vehicles to operate with full autonomy. The resulting system generates perception and localization information that is both low-noise in regions covered by high-fidelity sensors and avoids false negatives in areas only observed by low-fidelity sensors, while dealing with latency and dropout of the communication link between the two vehicles. At its core, the system uses a set of Extended Kalman filters which incorporate observations from both vehicles' sensors and extrapolate them using information about the road geometry. The perception and localization algorithms are evaluated both in simulation and on real vehicles as part of a full cooperative driving system.

ICRA Conference 2020 Conference Paper

Driving in Dense Traffic with Model-Free Reinforcement Learning

• Dhruv Mauria Saxena
• Sangjae Bae
• Alireza Nakhaei
• Kikuo Fujimura
• Maxim Likhachev

Traditional planning and control methods could fail to find a feasible trajectory for an autonomous vehicle to execute amongst dense traffic on roads. This is because the obstacle-free volume in spacetime is very small in these scenarios for the vehicle to drive through. However, that does not mean the task is infeasible since human drivers are known to be able to drive amongst dense traffic by leveraging the cooperativeness of other drivers to open a gap. The traditional methods fail to take into account the fact that the actions taken by an agent affect the behaviour of other vehicles on the road. In this work, we rely on the ability of deep reinforcement learning to implicitly model such interactions and learn a continuous control policy over the action space of an autonomous vehicle. The application we consider requires our agent to negotiate and open a gap in the road in order to successfully merge or change lanes. Our policy learns to repeatedly probe into the target road lane while trying to find a safe spot to move in to. We compare against two model-predictive control-based algorithms and show that our policy outperforms them in simulation. As part of this work, we introduce a benchmark for driving in dense traffic for use by the community.

AIJ Journal 2020 Journal Article

Effective footstep planning using homotopy-class guidance

• Vinitha Ranganeni
• Sahit Chintalapudi
• Oren Salzman
• Maxim Likhachev

Planning the motion for humanoid robots is a computationally-complex task due to the high dimensionality of the system. Thus, a common approach is to first plan in the low-dimensional space induced by the robot's feet—a task referred to as footstep planning. This low-dimensional plan is then used to guide the full motion of the robot. One approach that has proven successful in footstep planning is using search-based planners such as A* and its many variants. To do so, these search-based planners have to be endowed with effective heuristics to efficiently guide them through the search space. However, designing effective heuristics is a time-consuming task that requires the user to have good domain knowledge. Thus, our goal is to be able to effectively plan the footstep motions taken by a humanoid robot while obviating the burden on the user to carefully design local-minima free heuristics. To this end, we propose to use user-defined homotopy classes in the workspace that are intuitive to define. These homotopy classes are used to automatically generate heuristic functions that efficiently guide the footstep planner. Additionally, we present an extension to homotopy classes such that they are applicable to complex multi-level environments. We compare our approach for footstep planning with a standard approach that uses a heuristic common to footstep planning. In simple scenarios, the performance of both algorithms is comparable. However, in more complex scenarios our approach allows for a speedup in planning of several orders of magnitude when compared to the standard approach.

ICAPS Conference 2020 Conference Paper

Efficient Robot Planning for Achieving Multiple Independent Partially Observable Tasks That Evolve over Time

• Anahita Mohseni-Kabir
• Manuela Veloso
• Maxim Likhachev

We focus on domains where a robot is required to accomplish a set of tasks that are partially observable and evolve independently of each other according to their dynamics. An example domain is a restaurant setting where a robot waiter should take care of an ongoing stream of tasks, namely serving a number of tables, including delivering food to the tables and checking on customers. An action that the robot should take next at any point of time typically depends on the duration of possible actions, the state of each table, and how these tables evolve over time, e. g. , the food becomes cold after a few time steps. As most of these domains are dynamic and tasks are frequently being added and removed, the robot typically needs to plan for a short h-step horizon to quickly decide on the next action and replans at each time step. A conventional approach to deal with this problem is to combine all the tasks' states and robot actions into one large model and to compute an h-step optimal policy for this combined model. For the problems that we are interested in, the number of tasks, e. g. , the number of tables in the restaurant domain, can be large making this planning approach computationally impractical. The observation that we make however is that in many domains the number of tasks that the robot can accomplish within h-steps is very limited. We present an algorithm that exploits this observation and decomposes the problem into a series of much smaller planning problems, the solution to which gives us an optimal solution. We demonstrate the efficiency of our algorithm on the restaurant domain.

IROS Conference 2020 Conference Paper

Learning to Use Adaptive Motion Primitives in Search-Based Planning for Navigation

• Raghav Sood
• Shivam Vats
• Maxim Likhachev

Heuristic-based graph search algorithms like A* are frequently used to solve motion planning problems in many domains. For most practical applications, it is infeasible and unnecessary to pre-compute the graph representing the whole search space. Instead, these algorithms generate the graph incrementally by applying a fixed set of actions (frequently called motion primitives) to find the successors of every node that they need to evaluate. In many domains, it is possible to define actions (called adaptive motion primitives) that are not pre-computed but generated on the fly. The generation and validation of these adaptive motion primitives is usually quite expensive compared to pre-computed motion primitives. However, they have been shown to drastically speed up search if used judiciously. In prior work, ad hoc techniques like fixed thresholds have been used to limit unsuccessful evaluations of these actions. In this paper, we propose a learning-based approach to make more intelligent decisions about when to evaluate them. We do a thorough empirical evaluation of our model on a 3 degree-of-freedom (dof) motion planning problem for navigation using the Reeds-Shepp path as an adaptive motion primitive. Our experiments show that using our approach in conjunction with search algorithms leads to over 2x speedup in planning time.

SoCS Conference 2020 Conference Paper

Multi-Resolution A

• Wei Du
• Fahad Islam 0002
• Maxim Likhachev

Heuristic search-based planning techniques are commonly used for motion planning on discretized spaces. The performance of these algorithms is heavily affected by the resolution at which the search space is discretized. Typically a fixed resolution is chosen for a given domain. While a finer resolution allows better maneuverability, it exponentially increases the size of the state space, and hence demands more search efforts. On the contrary, a coarser resolution gives a fast exploratory behavior but compromises on maneuverability and the completeness of the search. To effectively leverage the advantages of both high and low resolution discretizations, we propose Multi-Resolution A* (MRA*) algorithm, that runs multiple weighted-A*(WA*) searches with different resolution levels simultaneously and combines the strengths of all of them. In addition to these searches, MRA* uses one anchor search to control expansions of other searches. We show that MRA* is bounded suboptimal with respect to the anchor resolution search space and resolution complete. We performed experiments on several motion planning domains including 2D, 3D grid planning and 7 DOF manipulation planning and compared our approach with several search-based and sampling-based baselines.

IROS Conference 2020 Conference Paper

PERCH 2. 0: Fast and Accurate GPU-based Perception via Search for Object Pose Estimation

• Aditya Agarwal
• Yupeng Han
• Maxim Likhachev

Pose estimation of known objects is fundamental to tasks such as robotic grasping and manipulation. The need for reliable grasping imposes stringent accuracy requirements on pose estimation in cluttered, occluded scenes in dynamic environments. Modern methods employ large sets of training data to learn features in order to find correspondence between 3D models and observed data. However these methods require extensive annotation of ground truth poses. An alternative is to use algorithms that search for the best explanation of the observed scene in a space of possible rendered scenes. A recently developed algorithm, PERCH (PErception Via SeaRCH) does so by using depth data to converge to a globally optimum solution using a search over a specially constructed tree. While PERCH offers strong guarantees on accuracy, the current formulation suffers from low scalability owing to its high runtime. In addition, the sole reliance on depth data for pose estimation restricts the algorithm to scenes where no two objects have the same shape. In this work, we propose PERCH 2. 0, a novel perception via search strategy that takes advantage of GPU acceleration and RGB data. We show that our approach can achieve a speedup of 100x over PERCH, as well as better accuracy than the state-of-the-art data-driven approaches on 6-DoF pose estimation without the need for annotating ground truth poses in the training data. Our code and video are available at https://sbpl-cruz.github.io/perception/.

ICRA Conference 2020 Conference Paper

Planning with Selective Physics-based Simulation for Manipulation Among Movable Objects

• Muhammad Suhail Saleem
• Maxim Likhachev

Use of physics-based simulation as a planning model enables a planner to reason and generate plans that involve non-trivial interactions with the world. For example, grasping a milk container out of a cluttered refrigerator may involve moving a robot manipulator in between other objects, pushing away the ones that are moveable and avoiding interactions with certain fragile containers. A physics-based simulator allows a planner to reason about the effects of interactions with these objects and to generate a plan that grasps the milk container successfully. The use of physics-based simulation for planning however is underutilized. One of the reasons for it being that physics-based simulations are typically way too slow for being used within a planning loop that typically requires tens of thousands of actions to be evaluated within a matter of a second or two. In this work, we develop a planning algorithm that tries to address this challenge. In particular, it builds on the observation that only a small number of actions actually need to be simulated using physics, and the remaining set of actions, such as moving an arm around obstacles, can be evaluated using a much simpler internal planning model, e. g. , a simple collision-checking model. Motivated by this, we develop an algorithm called Planning with Selective Physics-based Simulation that automatically discovers what should be simulated with physics and what can utilize an internal planning model for pick-and-place tasks.

ICRA Conference 2020 Conference Paper

Planning, Learning and Reasoning Framework for Robot Truck Unloading

• Fahad Islam 0002
• Anirudh Vemula
• Sung-Kyun Kim
• Andrew Dornbush
• Oren Salzman
• Maxim Likhachev

We consider the task of autonomously unloading boxes from trucks using an industrial manipulator robot. There are multiple challenges that arise: (1) real-time motion planning for a complex robotic system carrying two articulated mechanisms, an arm and a scooper, (2) decision-making in terms of what action to execute next given imperfect information about boxes such as their masses, (3) accounting for the sequential nature of the problem where current actions affect future state of the boxes, and (4) real-time execution that interleaves high-level decision-making with lower level motion planning. In this work, we propose a planning, learning, and reasoning framework to tackle these challenges, and describe its components including motion planning, belief space planning for offline learning, online decision-making based on offline learning, and an execution module to combine decision-making with motion planning. We analyze the performance of the framework on real-world scenarios. In particular, motion planning and execution modules are evaluated in simulation and on a real robot, while offline learning and online decision-making are evaluated in simulated real-world scenarios.

SoCS Conference 2019 Conference Paper

A-MHA*: Anytime Multi-Heuristic A

• Ramkumar Natarajan
• Muhammad Suhail Saleem
• Sandip Aine
• Maxim Likhachev
• Howie Choset

Designing good heuristic functions for graph search requires adequate domain knowledge. It is often easy to design heuristics that perform well and correlate with the underlying true cost-to-go values in certain parts of the search space but these may not be admissible throughout the domain thereby affecting the optimality guarantees of the search. Bounded suboptimal search using several of such partially good but inadmissible heuristics was developed in Multi-Heuristic A* (MHA*). Although MHA* leverages multiple inadmissible heuristics to potentially generate a faster suboptimal solution, the original version does not improve the solution over time. It is an one shot algorithm that requires careful setting of inflation factors to obtain a desired one time solution. In this work, we tackle this issue by extending MHA* to an anytime version that finds a feasible suboptimal solution quickly and continually improve it until time runs out. Our work is inspired from the Anytime Repairing A* (ARA*) algorithm. We prove that our precise adaptation of ARA* concepts in the MHA* framework preserves the original suboptimal and completeness guarantees and enhances MHA* to perform in an anytime fashion. Furthermore, we report the performance of A-MHA* in 3-D path planning domain and sliding tiles puzzle and compare against MHA* and other anytime algorithms.

IROS Conference 2019 Conference Paper

Bidirectional Heuristic Search for Motion Planning with an Extend Operator

• Allen Cheng
• Dhruv Mauria Saxena
• Maxim Likhachev

Sampling-based approaches are often favored in robotics for high-dimensional motion planning for their fast exploration of the search space. However, at best they offer asymptotic guarantees on solution quality due to their inherent stochasticity. While planning, the majority of effort is often spent near the start and goal configurations with a large amount of free space in between. Bidirectional approaches such as RRT-Connect exploit this fact by greedily extending and connecting search frontiers that simultaneously propagate from the start and goal configurations of a planning problem. In this work, we use such an extend operator for bidirectional heuristic search-based planners, which typically struggle with high-dimensionality. In doing so, we address the difficulty that these bidirectional planners face with connecting frontiers of both search efforts while providing suboptimality bounds on solution quality. We validate our simple approach on high-dimensional manipulation tasks, demonstrating significantly reduced search effort when compared against other popular bidirectional algorithms, both search-based and sampling. Our algorithm maintains theoretical guarantees on suboptimality and completeness for a given resolution. In addition, the solutions found by our planner are of higher quality compared to those found by the other baseline algorithms.

IROS Conference 2019 Conference Paper

Escaping Local Minima in Search-Based Planning using Soft Duplicate Detection

• Wei Du
• Sung-Kyun Kim
• Oren Salzman
• Maxim Likhachev

Search-based planning for relatively low-dimensional motion-planning problems such as for autonomous navigation and autonomous flight has been shown to be very successful. Such framework relies on laying a grid over a state-space and constructing a set of actions (motion primitives) that connect the centers of cells. However, in some cases such as kinodynamic motion planning, planning for bipedal robots with high balance requirements, computing these actions can be highly non-trivial and often impossible depending on the dynamic constraints. In this paper, we explore a soft version of discretization, wherein the state-space remains to be continuous but the search tries to avoid exploring states that are likely to be duplicates of states that have already been explored. We refer to this property of the search as soft duplicate detection and view it as a relaxation of the standard notion of duplicate detection. Empirically, we show that the search can efficiently compute paths in highly-constrained settings and outperforms alternatives on several domains.

SoCS Conference 2019 Conference Paper

Intuitive, Reliable Plans with Contingencies: Planning with Safety Nets for Landmark-Based Routing

• Kalyan Vasudev Alwala
• Margarita Safonova
• Oren Salzman
• Maxim Likhachev

We are interested in the problem of providing intuitive instructions for human agents to enable reliable navigation in unknown environments. Since the advent of GPS and digital maps, a common approach is to visually provide a planned path on a digital map defined in terms of actions to take at specific junctions. However, this approach relies on the agent to constantly and accurately localize itself. Furthermore, it comes in stark contrast to the way humans provide instructions—by leveraging known landmarks in the environment to both augment the description of the planned path as well as to allow to detect when the agent deviated from the planned path. Hence, there is need for assurable means of localization, an intuitive way of compactly conveying directions to agents and a systematic approach to account for human errors. To this end, our key insight is to employ known landmarks in the environment to overcome these challenges. We formally model this intuitive way to use landmarks for conveying instructions and for creating contingency plans. We present experiments demonstrating the efficacy of our approach both on synthetic environments as well as on realworld maps, computed using a smart-phone iOS application that we developed.

ICAPS Conference 2019 Conference Paper

POMHDP: Search-Based Belief Space Planning Using Multiple Heuristics

• Sung-Kyun Kim
• Oren Salzman
• Maxim Likhachev

Robots operating in the real world encounter substantial uncertainty that cannot be modeled deterministically before the actual execution. This gives rise to the necessity of robust motion planning under uncertainty also known as belief space planning. Belief space planning can be formulated as Partially Observable Markov Decision Processes (POMDPs). However, computing optimal policies for non-trivial POMDPs is computationally intractable. Building upon recent progress from the search community, we propose a novel anytime POMDP solver, Partially Observable Multi-Heuristic Dynamic Programming (POMHDP), that leverages multiple heuristics to efficiently compute high-quality solutions while guaranteeing asymptotic convergence to an optimal policy. Through iterative forward search, POMHDP utilizes domain knowledge to solve POMDPs with specific goals and an infinite horizon. We demonstrate the efficacy of our proposed framework on a real-world, highly-complex, truck unloading application.

ICAPS Conference 2019 Conference Paper

Provable Indefinite-Horizon Real-Time Planning for Repetitive Tasks

• Fahad Islam 0002
• Oren Salzman
• Maxim Likhachev

In many robotic manipulation scenarios, robots often have to perform highly-repetitive tasks in structured environments e. g. sorting mail in a mailroom or pick and place objects on a conveyor belt. In this work we are interested in settings where the tasks are similar, yet not identical (e. g. , due to uncertain orientation of objects) and motion planning needs to be extremely fast. Preprocessing-based approaches prove to be very beneficial in these settings—they analyze the configuration-space offline to generate some auxiliary information which can then be used in the query phase to speedup planning times. Typically, the tighter the requirement is on query times the larger the memory footprint will be. In particular, for high-dimensional spaces, providing real-time planning capabilities is extremely challenging. While there are planners that guarantee real-time performance by limiting the planning horizon, we are not aware of general-purpose planners capable of doing it for indefinite horizon (i. e. , planning to the goal). To this end, we propose a preprocessingbased method that provides provable bounds on the query time while incurring only a small amount of memory overhead in the query phase. We evaluate our method on a 7-DOF robot arm and show a speedup of over tenfold in query time when compared to the PRM algorithm.

ICAPS Conference 2019 Conference Paper

Speeding Up Search-Based Motion Planning via Conservative Heuristics

• Ishani Chatterjee 0001
• Maxim Likhachev
• Ashwin Khadke
• Manuela Veloso

Weighted A* search (wA*) is a popular tool for robot motionplanning. Its efficiency however depends on the quality of heuristic function used. In fact, it has been shown that the correlation between the heuristic function and the true costto-goal significantly affects the efficiency of the search, when used with a large weight on the heuristics. Motivated by this observation, we investigate the problem of computing heuristics that explicitly aim to minimize the amount of search efforts in finding a feasible plan. The key observation we exploit is that while heuristics tries to guide the search along what looks like an optimal path towards the goal, there are other paths that are clearly sub-optimal yet are much easier to compute. For example, in motion planning domains like footstep-planning for humanoids, a heuristic that guides the search along a path away from obstacles is less likely to encounter local minima compared with the heuristics that guides the search along an optimal but close-to-obstacles path. We utilize this observation to define the concept of conservative heuristics and propose a simple algorithm for computing such a heuristic function. Experimental analysis on (1) humanoid footstep planning (simulation), (2) path planning for a UAV (simulation), and a real-world experiment in footstep-planning for a NAO robot shows the utility of the approach.

ICRA Conference 2018 Conference Paper

A Single-Planner Approach to Multi-Modal Humanoid Mobility

• Andrew Dornbush
• Karthik Vijayakumar
• Sameer Bardapurkar
• Fahad Islam 0002
• Masayuki Ito
• Maxim Likhachev

In this work, we present an approach to planning for humanoid mobility. Humanoid mobility is a challenging problem, as the configuration space for a humanoid robot is intractably large, especially if the robot is capable of performing many types of locomotion. For example, a humanoid robot may be able to perform such tasks as bipedal walking, crawling, and climbing. Our approach is to plan for all these tasks within a single search process. This allows the search to reason about all the capabilities of the robot at any point, and to derive the complete solution such that the plan is guaranteed to be feasible. A key observation is that we often can roughly decompose a mobility task into a sequence of smaller tasks, and focus planning efforts to reason over much smaller search spaces. To this end, we leverage the results of a recently developed framework for planning with adaptive dimensionality, and incorporate the capabilities of available controllers directly into the planning process. The resulting planner can also be run in an interleaved fashion alongside execution so that time spent idle is much reduced.

ICAPS Conference 2018 Conference Paper

Effective Footstep Planning for Humanoids Using Homotopy-Class Guidance

• Vinitha Ranganeni
• Oren Salzman
• Maxim Likhachev

Planning the motion for humanoid robots is a computationally-complex task due to the high dimensionality of the system. Thus, a common approach is to first plan in the low-dimensional space induced by the robot’s feet—a task referred to as footstep planning. This low-dimensional plan is then used to guide the full motion of the robot. One approach that has proven successful in footstep planning is using search-based planners such as A* and its many variants. To do so, these search-based planners have to be endowed with effective heuristics to efficiently guide them through the search space. However, designing effective heuristics is a time-consuming task that requires the user to have good domain knowledge. Thus, our goal is to be able to effectively plan the footstep motions taken by a humanoid robot while obviating the burden on the user to carefully design local-minima free heuristics. To this end, we propose to use user-defined homotopy classes in the workspace that are intuitive to define. These homotopy classes are used to automatically generate heuristic functions that efficiently guide the footstep planner. We compare our approach for footstep planning with a standard approach that uses a heuristic common to footstep planning. In simple scenarios, the performance of both algorithms is comparable. However, in more complex scenarios our approach allows for a speedup in planning of several orders of magnitude when compared to the standard approach.

IROS Conference 2018 Conference Paper

FOCS: Planning by Fusion of Optimal Control & Search and its Application to Navigation

• Piero Micelli
• Maxim Likhachev

Both Optimal Control and Search-based Planning are used extensively for path planning and have their own set of advantages and disadvantages. In this paper, we propose an algorithm FOCS (Fusion of Optimal Control and Search) that combines these two classes of approaches together. FOCS finds a path exploiting the advantages of both approaches while providing a bound on the sub-optimality of its solution. The returned path is a concatenation of the path found in the implicit graph constructed by search and the path generated by following the negative gradient of the value function obtained as a solution of the Hamilton-Jacobi-Bellman equation. We analyze the algorithm and illustrate its effectiveness in finding a minimum-time path for a car-like vehicle in different environments.

IROS Conference 2018 Conference Paper

Motion Planning for an Underwater Mobile Manipulator by Exploiting Loose Coupling

• Dina Youakim
• Andrew Dornbush
• Maxim Likhachev
• Pere Ridao Rodriguez

Intervention Autonomous Underwater Vehicle or I-AUV has recently started to grab researchers attention in the last 20 years. Only three I-AUVs have demonstrated autonomous manipulation skills: ALIVE, SAUVIM and GIRONA 500. While prior systems rely on variations of the task-priority redundancy control framework, our recent research showed preliminary results using motion planning for floating-based intervention in the presence of obstacles. With the increasing need for autonomously performing more complex manipulation tasks, two main challenges need to be addressed: the high-dimensionality of the system, and the motion coordination between the mobile base and the working arm. The latter challenge is of high importance if accurate execution is required, especially considering the floating nature of the AUV and the control challenges that come with it. Our approach relies on exploiting the loose coupling between the AUV and the arm. In particular we present an approach based on MR-MHA * (Multi-Representation, Multi-Heuristic A*), and we show how it can generate efficient trajectories by exploiting decoupling. We show for the first time the use of a search-based planner on a high-dimensional underwater manipulator. In addition, we support our claims with experimental analysis of the generated trajectories with respect to various metrics in different environments. Furthermore, we demonstrate the ability of our approach to conduct a full intervention mission in a realistic simulated underwater intervention environment.

IJCAI Conference 2018 Conference Paper

Online, Interactive User Guidance for High-dimensional, Constrained Motion Planning

• Fahad Islam
• Oren Salzman
• Maxim Likhachev

We consider the problem of planning a collision-free path for a high-dimensional robot. Specifically, we suggest a planning framework where a motion-planning algorithm can obtain guidance from a user. In contrast to existing approaches that try to speed up planning by incorporating experiences or demonstrations ahead of planning, we suggest to seek user guidance only when the planner identifies that it ceases to make significant progress towards the goal. Guidance is provided in the form of an intermediate configuration q^, which is used to bias the planner to go through q^. We demonstrate our approach for the case where the planning algorithm is Multi-Heuristic A* (MHA*) and the robot is a 34-DOF humanoid. We show that our approach allows to compute highly-constrained paths with little domain knowledge. Without our approach, solving such problems requires carefully-crafted domain-dependent heuristics.

ICRA Conference 2017 Conference Paper

Deliberative object pose estimation in clutter

• Venkatraman Narayanan
• Maxim Likhachev

A fundamental robot perception task is that of identifying and estimating the poses of objects with known 3D models in RGB-D data. While feature-based and discriminative approaches have been traditionally used for this task, recent work on deliberative approaches such as PERCH and D2P have shown improved robustness in handling scenes with severe inter-object occlusions. These deliberative approaches work by treating multi-object pose estimation as a combinatorial search over the space of possible rendered scenes of the objects, thereby inherently being able to predict and account for occlusions. However, these methods have so far been restricted to scenes comprising only of known objects, and have been unable to handle extraneous clutter — a common occurrence in many real-world settings. This work significantly increases the practical relevance of deliberative perception methods by developing a formulation that: i) accounts for extraneous unmodeled clutter in scenes, and ii) provides object pose uncertainty estimates. Our algorithm is complete and provides bounded suboptimality guarantees for the cost function chosen to be optimized. Empirically, we demonstrate successful object recognition and uncertainty-aware localization in challenging scenes with unmodeled clutter, where previous deliberative methods perform unsatisfactorily. In addition, this work was used as part of the perception system by Carnegie Mellon University's Team HARP in the 2016 Amazon Picking Challenge.

ICAPS Conference 2017 Conference Paper

Heuristic Search on Graphs with Existence Priors for Expensive-to-Evaluate Edges

• Venkatraman Narayanan
• Maxim Likhachev

We address the problem of finding shortest paths in graphs where some edges have a prior probability of existence, and their existence can be verified during planning with time- consuming operations. Our work is motivated by real-world robot motion planning, where edge existence is often expensive to verify (typically involves time-consuming collision-checking between the robot and world models), but edge existence probabilities are readily available. The goal then, is to develop an anytime algorithm that can return good solutions quickly by somehow leveraging the existence probabilities, and continue to return better-quality solutions or provide tighter suboptimality bounds with more time. While our motivation is fast and high-quality motion planning for robots, this work presents two fundamental contributions applicable to generic graphs with probabilistic edges. They are: a) an algorithm for efficiently computing all relevant shortest paths in a graph with probabilistic edges, and as a by-product the expected shortest path cost, and b) an anytime algorithm for evaluating (verifying existence of) edges in a collection of paths, which is optimal in expectation under a chosen distribution of the algorithm interruption time. Finally, we provide a practical approach to integrate a) and b) in the context of robot motion planning and demonstrate significant improvements in success rate and planning time for a 11 degree-of-freedom mobile manipulation planning problem. We also conduct additional evaluations on a 2D grid navigation domain to study our algorithm’s behavior.

ICAPS Conference 2017 Conference Paper

Learning to Avoid Local Minima in Planning for Static Environments

• Shivam Vats
• Venkatraman Narayanan
• Maxim Likhachev

In many robot motion planning problems such as manipulation planning for a personal robot in a kitchen or an industrial manipulator in a warehouse, all motion planning queries are in an environment that is largely static. Consequently, one should be able to improve the performance of a planning algorithm by training on this static environment ahead of operation time. In this work, we propose a method to improve the performance of heuristic search-based motion planners in such environments. The first, learning, phase of our proposed method analyzes search performance on multiple planning episodes to infer local minima zones, that is, regions where the existing heuristic(s) are weakly correlated with the true cost-to-go. Then, in the planning phase of the method, the learnt local minima are used to modify the original search graph in a way that improves search performance. We prove that our method preserves guarantees on completeness and bounded suboptimality with respect to the original search graph. Experimentally, we observe significant improvements in success rate and planning time for challenging 11 degree-of-freedom mobile manipulation problems.

ICRA Conference 2017 Conference Paper

Parts assembly planning under uncertainty with simulation-aided physical reasoning

• Sung-Kyun Kim
• Maxim Likhachev

Parts assembly, in a broad sense, is to make multiple objects to be in specific relative poses in contact with each other. One of the major reasons that make it difficult is uncertainty. Because parts assembly involves physical contact between objects, it requires higher precision than other manipulation tasks like collision avoidance. The key idea of this paper is to use simulation-aided physical reasoning while planning with the goal of finding a robust motion plan for parts assembly. Specifically, in the proposed approach, a) uncertainty between object poses is represented as a distribution of particles, b) the motion planner estimates the transition of particles for unit actions (motion primitives) through physics-based simulation, and c) the performance of the planner is sped up using Multi-Heuristic A* (MHA*) search that utilizes multiple inadmissible heuristics that lead to fast uncertainty reduction. To demonstrate the benefits of our framework, motion planning and physical robot experiments for several parts assembly tasks are provided.

SoCS Conference 2017 Conference Paper

Search Reduction through Conservative Abstract-Space Based Heuristic

• Ishani Chatterjee 0001
• Maxim Likhachev
• Manuela Veloso

The efficiency of heuristic search depends dramatically on the quality of the heuristic function. For an optimal heuristic search, heuristics that estimate cost-to-goal better typically lead to faster searches. For a sub-optimal heuristic search such as weighted A*, the search speed depends more on the correlation between the heuristic and the true cost-to-goal. In this extended abstract, we discuss our preliminary work on computing heuristic functions that exploit this fact. In particular, we introduce a many-to-one mapping from an original search space to a conservative abstract space. Edges in the abstract space capture reachability among all corresponding nodes in the original space. We compute a heuristic in the conservative abstract space which when used by the search in the original space reduces the number of searched nodes. Our preliminary results on 3D navigation show that in more complex scenarios the speedup can be dramatic.

ICRA Conference 2016 Conference Paper

A*-Connect: Bounded suboptimal bidirectional heuristic search

• Fahad Islam 0002
• Venkatraman Narayanan
• Maxim Likhachev

The benefits of bidirectional planning over the unidirectional version are well established for motion planning in high-dimensional configuration spaces. While bidirectional approaches have been employed with great success in the context of sampling-based planners such as in RRT-Connect, they have not enjoyed popularity amongst search-based methods such as A*. The systematic nature of search-based algorithms, which often leads to consistent and high-quality paths, also enforces strict conditions for the connection of forward and backward searches. Admissible heuristics for the connection of forward and backward searches have been developed, but their computational complexity is a deterrent. In this work, we leverage recent advances in search with inadmissible heuristics to develop an algorithm called A*-Connect, much in the spirit of RRT-Connect. A*-Connect uses a fast approximation of the classic front-to-front heuristic from literature to lead the forward and backward searches towards each other, while retaining theoretical guarantees on completeness and bounded suboptimality. We validate A*-Connect on manipulation as well as navigation domains, comparing with popular sampling-based methods as well as state-of-the-art bidirectional search algorithms. Our results indicate that A*-Connect can provide several times speedup over unidirectional search while maintaining high solution quality.

ICRA Conference 2016 Conference Paper

PERCH: Perception via search for multi-object recognition and localization

• Venkatraman Narayanan
• Maxim Likhachev

In many robotic domains such as flexible automated manufacturing or personal assistance, a fundamental perception task is that of identifying and localizing objects whose 3D models are known. Canonical approaches to this problem include discriminative methods that find correspondences between feature descriptors computed over the model and observed data. While these methods have been employed successfully, they can be unreliable when the feature descriptors fail to capture variations in observed data; a classic cause being occlusion. As a step towards deliberative reasoning, we present PERCH: PErception via SeaRCH, an algorithm that seeks to find the best explanation of the observed sensor data by hypothesizing possible scenes in a generative fashion. Our contributions are: i) formulating the multi-object recognition and localization task as an optimization problem over the space of hypothesized scenes, ii) exploiting structure in the optimization to cast it as a combinatorial search problem on what we call the Monotone Scene Generation Tree, and iii) leveraging parallelization and recent advances in multi-heuristic search in making combinatorial search tractable. We prove that our system can guaranteedly produce the best explanation of the scene under the chosen cost function, and validate our claims on real world RGB-D test data. Our experimental results show that we can identify and localize objects under heavy occlusion— cases where state-of-the-art methods struggle.

ICRA Conference 2016 Conference Paper

Planning for a ground-air robotic system with collaborative localization

• Jonathan Butzke
• Kalin Gochev
• Benjamin Holden
• Eui-Jung Jung
• Maxim Likhachev

Robots are increasingly being used in situations such as search and rescue that require robust navigation capabilities, potentially in areas with little or no GPS or other high-quality localization information. As more robots are used in these scenarios, it becomes viable to collaborate between heterogeneous types of robots to leverage their individual strengths while minimizing their weaknesses. More specifically, in a scenario involving unmanned ground and aerial vehicles (UGV, UAV), the ground robot can contribute its high payload capacity to provide computational resources and high accuracy sensors while the aerial robot can bring its high mobility and capability to traverse obstacles to the team. However, in order for the team to benefit from these capabilities, it must be capable of generating a plan for both robots that allows them to collaboratively localize when necessary. Our approach to this problem is to combine a recently developed state lattice planner using controller-based motion primitives (SLC) with planning using adaptive dimensionality (PAD). The SLC planner allows for robust navigation using a wide variety of sensors including in areas with no or limited high-quality localization information while the PAD planner allows us to expand beyond a single robot and generate plans for a team of robots operating in a high dimensional space. We present our results to this combined approach for a UGV/UAV team operating indoors in areas with limited visual features.

ICRA Conference 2016 Conference Paper

Planning for grasp selection of partially occluded objects

• Sung-Kyun Kim
• Maxim Likhachev

In a cluttered scene, an object is often occluded by other objects, and a robot cannot figure out what the object is and perceive its pose exactly. We assume that the robot is equipped with a depth sensor and given a database of 3D object models and their grasping poses, but yet there is uncertainty about object's class and pose. In this paper, we study the problem of how to predict the class and pose of an occluded object by carefully taking a sequence of observations. To find the best sequence of viewpoints by the robot, we construct hypotheses of the states of the target and occluding objects, and update our belief state as new observations come in. Every time selecting the next robot pose, we greedily choose the one that is expected to reduce the uncertainty the most. Based on the theoretical analysis of adaptive submodular maximization problems, this process is guaranteed to find a near-optimal sequence of robot poses in terms of observation and traverse costs. To validate the proposed method, we present simulation and robot experiments using a PR2.

ICAPS Conference 2016 Conference Paper

Search Portfolio with Sharing

• Sandip Aine
• Maxim Likhachev

Over the years, a number of search algorithms have been proposed in AI literature, ranging from best-first to depth-first searches, from incomplete to optimal searches, from linear memory to unbounded memory searches; each having their strengths and weaknesses. The variability in performance of these algorithms makes algorithm selection a hard problem, especially for performance critical domains. Algorithm portfolios alleviate this problem by simultaneously running multiple algorithms to solve a given problem instance, exploiting their diversity. In general, the portfolio methods do not share information among candidate algorithms. Our work is based on the observation that if the algorithms within a portfolio can share information, it may significantly enhance the performance, as one algorithm can now utilize partial results computed by other algorithms. To this end, we introduce a new search framework, called Search Portfolio with Sharing (SP-S), which uses multiple algorithms to explore a given state-space in an integrated manner, seamlessly combining the partial solutions, while preserving the constraints/characteristics of the candidate algorithms. In addition, SP-S can be easily adopted to guarantee theoretical properties like completeness, bounded sub-optimality, and bounded re-expansions. We describe the basics of the SP-S framework and explain how different classes of search algorithms can be integrated in SP-S. We discuss its theoretical properties and present experimental results for multiple domains, demonstrating the utility of such a shared approach.

AIJ Journal 2016 Journal Article

Truncated incremental search

• Sandip Aine
• Maxim Likhachev

Incremental heuristic search algorithms reuse their previous search efforts whenever these are available. As a result, they can often solve a sequence of similar planning problems faster than planning from scratch. State-of-the-art incremental heuristic searches (such as LPA*, D* and D* Lite) work by propagating cost changes to all the states in the search tree whose g values (the costs of computed paths from the start state) are no longer optimal. This work is based on the observation that while a complete propagation of cost changes is essential to ensure optimality, the propagations can be stopped earlier if we are looking close-to-optimal solutions instead of the optimal one. We develop a framework called Truncated Incremental Search that builds on this observation and uses a target suboptimality bound to efficiently restrict cost propagations. We present two truncation based algorithms, Truncated LPA* (TLPA*) and Truncated D* Lite (TD* Lite), for bounded suboptimal planning and navigation in dynamic graphs. We also develop an anytime replanning algorithm, Anytime Truncated D* (ATD*), that combines the inflated heuristic search with truncation, in an anytime manner. We discuss the theoretical properties of these algorithms proving their correctness and efficiency, and present experimental results on 2D and 3D (x, y, heading) path planning domains evaluating their performance. The empirical results show that the truncated incremental searches can provide significant improvement in runtime over existing incremental search algorithms, especially when searching for close-to-optimal solutions in large, dynamic graphs.

IROS Conference 2015 Conference Paper

3-D exploration with an air-ground robotic system

• Jonathan Butzke
• Andrew Dornbush
• Maxim Likhachev

Exploration of unknown environments is an important aspect to fielding teams of robots. Without the ability to determine on their own where to go in the environment, the full potential of robotic teams is limited to the abilities of human operators to deploy them for search and rescue, mapping, or other tasks that are predicated on gaining knowledge from the environment. This is of particular importance in real-world 3-Dimensional (3-D) environments where simple planar assumptions can lead to incomplete exploration, for example, real-world environments have areas underneath overhangs or inside caves. As an additional challenge, when the teams of robots have vastly different capabilities, the planning system must take those into account to efficiently utilize the available assets. In this paper, we present a combined air-ground system for conducting 3-D exploration in cluttered environments. We first describe the hardware and software components of the system. We then present our algorithm for planning 3-D goal locations for a heterogeneous team of robots to efficiently explore a previously unknown environment and demonstrate its applicability in real-world experiments.

ICRA Conference 2015 Conference Paper

A web-based infrastructure for recording user demonstrations of mobile manipulation tasks

• Ellis Ratner
• Benjamin J. Cohen
• Mike Phillips
• Maxim Likhachev

Learning from demonstration (LfD) is a common technique applied to many problems in robotics, such as populating grasp databases, training for reinforcement learning of high-level skill sets and bootstrapping motion planners. While such approaches are generally highly valued, they rely on the often time-consuming process of gathering user demonstrations, and hence it becomes difficult to attain a sizeable dataset. In this paper, we present a tool capable of recording large numbers of high-dimensional demonstrations of mobile manipulation tasks provided by non-experts in the field. Our tool accomplishes this via a web interface that requires no additional software to be installed beyond a web browser, as well as a scalable architecture that is capable of supporting 10 concurrent demonstrators on a single server. Our architecture employs a lightweight simulation environment to reduce unnecessary computations and improve performance. Furthermore, we show how our tool can be used to gather a large set of demonstrations of a mobile manipulation task by leveraging existing crowdsource platforms. The data set collected has been made available to the robotics community. We also present experiments in which we apply demonstrations collected through our infrastructure to teach a robot how to grasp, to teach a robot how to perform dexterous manipulation tasks such as scooping and to accelerate motion planning for full-body manipulation tasks.

ICRA Conference 2015 Conference Paper

Dynamic Multi-Heuristic A*

• Fahad Islam 0002
• Venkatraman Narayanan
• Maxim Likhachev

Many motion planning problems in robotics are high dimensional planning problems. While sampling-based motion planning algorithms handle the high dimensionality very well, the solution qualities are often hard to control due to the inherent randomization. In addition, they suffer severely when the configuration space has several ‘narrow passages’. Search-based planners on the other hand typically provide good solution qualities and are not affected by narrow passages. However, in the absence of a good heuristic or when there are deep local minima in the heuristic, they suffer from the curse of dimensionality. In this work, our primary contribution is a method for dynamically generating heuristics, in addition to the original heuristic(s) used, to guide the search out of local minima. With the ability to escape local minima easily, the effect of dimensionality becomes less pronounced. On the theoretical side, we provide guarantees on completeness and bounds on suboptimality of the solution found. We compare our proposed method with the recently published Multi-Heuristic A* search, and the popular RRT-Connect in a full-body mobile manipulation domain for the PR2 robot, and show its benefits over these approaches.

IJCAI Conference 2015 Conference Paper

Efficient Search with an Ensemble of Heuristics

• Mike Phillips
• Venkatraman Narayanan
• Sandip Aine
• Maxim Likhachev

Recently, a number of papers have shown that for many domains, using multiple heuristics in independent searches performs better than combining them into a single heuristic. Furthermore, using a large number of “weak” heuristics could potentially eliminate the need for the careful design of a few. The standard approach to distribute computation in these multi-heuristic searches is to rotate through the heuristics in a round-robin fashion. However, this strategy can be inefficient especially in the case when only a few of the heuristics are leading to progress. In this paper, we present two principled methods to adaptively distribute computation time among the different searches of the Multi- Heuristic A* algorithm. The first method, Meta-A*, constructs and searches a meta-graph, which represents the problem of finding the best heuristic as the problem of minimizing the total number of expansions. The second treats the scheduling of searches with different heuristics as a multi-armed bandit problem. It applies Dynamic Thompson Sampling (DTS) to keep track of what searches are making progress the most and continuously re-computes the schedule of searches based on this information. We provide a theoretical analysis and compare our new strategies with the round-robin method on a 12-DOF full-body motion planning problem and on sliding tile puzzle problems. In these experiments, we used up to 20 heuristics and observed a several times speedup without loss in solution quality.

SoCS Conference 2015 Conference Paper

Improved Multi-Heuristic A* for Searching with Uncalibrated Heuristics

• Venkatraman Narayanan
• Sandip Aine
• Maxim Likhachev

Recently, several researchers have brought forth the benefits of searching with multiple (and possibly inadmissible) heuristics, arguing how different heuristics could be independently useful in different parts of the state space. However, algorithms that use inadmissible heuristics in the traditional best-first sense, such as the recently developed Multi-Heuristic A* (MHA*), are subject to a crippling calibration problem: they prioritize nodes for expansion by additively combining the cost-to-come and the inadmissible heuristics even if those heuristics have no connection with the cost-to-go (e. g. , the heuristics are uncalibrated). For instance, if the inadmissible heuristic were an order of magnitude greater than the perfect heuristic, an algorithm like MHA* would simply reduce to a weighted A* search with one consistent heuristic. In this work, we introduce a general multi-heuristic search framework that solves the calibration problem and as a result a) facilitates the effective use of multiple uncalibrated inadmissible heuristics, and b) provides significantly better performance than MHA* whenever tighter sub-optimality bounds on solution quality are desired. Experimental evaluations on a complex full-body robotics motion planning problem and large sliding tile puzzles demonstrate the benefits of our framework.

ICRA Conference 2015 Conference Paper

Lazy validation of Experience Graphs

• Victor Hwang
• Mike Phillips
• Siddhartha S. Srinivasa
• Maxim Likhachev

Many robot applications involve lifelong planning in relatively static environments e. g. assembling objects or sorting mail in an office building. In these types of scenarios, the robot performs many tasks over a long period of time. Thus, the time required for computing a motion plan becomes a significant concern, prompting the need for a fast and efficient motion planner. Since these environments remain similar in between planning requests, planning from scratch is wasteful. Recently, Experience Graphs (E-Graphs) were proposed to accelerate the planning process by reusing parts of previously computed paths to solve new motion planning queries more efficiently. This work describes a method to improve planning times with E-Graphs given changes in the environment by lazily evaluating the validity of past experiences during the planning process. We show the improvements with our method in a single-arm manipulation domain with simulations on the PR2 robot.

SoCS Conference 2015 Conference Paper

Learning to Search More Efficiently from Experience: A Multi-Heuristic Approach

• Sandip Aine
• Charupriya Sharma
• Maxim Likhachev

Learning from experience can significantly improve the performance of search based planners, especially for challenging problems like high-dimensional planning. Experience Graph (E-Graph) is a recently developed framework that encodes experiences, obtained from solving instances in the past, into a single bounded-admissible heuristic, and uses it to guide the search. While the E-Graph approach was shown to be very useful for repetitive problems, it suffers from two issues. First, computing the E-Graph heuristic is time consuming as it maintains the bounded admissibility constraints. Second, a single heuristic can get stuck in a local minimum, and thereby, degrade the performance. In this work, we present an alternative approach to improving the runtime of search from experience, based on a recently developed search algorithm Multi-heuristic A* (MHA*). This framework provides an improvement over the E-Graph planner for two reasons: a) MHA* uses multiple heuristics simultaneously to explore the search space, which reduces the probability of getting stuck in a local minimum, and b) the heuristics in MHA* can be arbitrarily inadmissible, which makes it very easy to compute them. The paper describes the framework, explains how to compute these (inadmissible) heuristics through offline and online processing and presents experimental analysis on two domains, motion planning for a 6D planar arm and large sliding tile puzzles.

IROS Conference 2015 Conference Paper

Path planning for a tethered robot using Multi-Heuristic A* with topology-based heuristics

• Soonkyum Kim
• Maxim Likhachev

In this paper, we solve the path planning problem for a tethered mobile robot, which is connected to a fixed base by a cable of length L. The reachable space of the robot is restricted by the length of the cable and obstacles. The reachable space of the tethered robot can be computed by considering the topology class of the cable. However, it is computationally too expensive to compute this space a-priori. Instead, in this paper, we show how we can plan using a recently-developed variant of A* search, called Multi-Heuristic A*. Normally, the Multi-Heuristic A* algorithm takes in a fixed set of heuristic functions. In our problem, however, the heuristics represent length of paths to the goal along different topology classes, and there can be too many of them and not all the topology classes are useful. To deal with this, we adapt Multi-Heuristic A* to work with a dynamically generated set of heuristic functions. It starts out as a normal weighted A*. Whenever the search gets trapped in a local minimum, we find the proper topology class of the path to escape from it and add the corresponding new heuristic function into the set of heuristic functions considered by the search. We present experimental analysis comparing our approach with weighted A* on planning for a tethered robot in simulation.

ICRA Conference 2015 Conference Paper

Planning for multi-agent teams with leader switching

• Siddharth Swaminathan
• Mike Phillips
• Maxim Likhachev

Follow-the-leader based approaches have been popular for the control of multi-robot teams for their ability to drive many with few. Typically, in these methods you select a single leader, generate a plan for it, while all other agents follow this leader using their individual controllers. However, there are many scenarios where this approach can lead to highly suboptimal behavior or even failure in the presence of clutter. In this work, we present a planning approach that automatically figures out when to switch leaders on the way to the goal while minimizing a given cost function that penalizes leader switching and deviations from the desired formation. To deal with the increased dimensionality of the problem we show how a recently developed algorithm, MHA* (multi-heuristic A*) can be extended to support planning for a team of robots. We also provide explicit cost minimization and guarantee that paths found are within a user- chosen factor of optimality with respect to the graph modeling the planning problem. Experimentally, we found that allowing for dynamic leader-switching leads to a significant increase in finding feasible plans for multi-robot teams ranging up to 21 robots.

SoCS Conference 2015 Conference Paper

Planning Single-Arm Manipulations with N-Arm Robots

• Benjamin J. Cohen
• Mike Phillips
• Maxim Likhachev

Many robotic systems are comprised of two or more arms. Such systems range from dual-arm household manipulators to factory floors populated with a multitude of industrial robotic arms. While the use of multiple arms increases the productivity of the system and extends dramatically its workspace, it also introduces a number of challenges. One such challenge is planning the motion of the arm(s) required to relocate an object from one location to another. This problem is challenging because it requires reasoning over which arms and in which order should manipulate the object, finding a sequence of valid handoff locations between the consecutive arms and finally choosing the grasps that allow for successful handoffs. In this paper, we show how to exploit the characteristics of this problem in order to construct a planner that can solve it effectively. We analyze our approach experimentally on a number of simulated examples ranging from a 2-arm system operating at a table to a 3-arm system working at a bar and to a 4-arm system in a factory setting.

ICRA Conference 2015 Conference Paper

Speeding up heuristic computation in planning with Experience Graphs

• Mike Phillips
• Maxim Likhachev

Experience Graphs have been shown to accelerate motion planning using parts of previous paths in an A* framework. Experience Graphs work by computing a new heuristic for weighted A* search on top of the domain's original heuristic and the edges in an Experience Graph. The new heuristic biases the search toward relevant prior experience and uses the original heuristic for guidance otherwise. In previous work, Experience Graphs were always built on top of domain heuristics which were computed by dynamic programming (a lower dimensional version of the original planning problem). When the original heuristic is computed this way the Experience Graph heuristic can be computed very efficiently. However, there are many commonly used heuristics in planning that are not computed in this fashion, such as euclidean distance. While the Experience Graph heuristic can be computed using these heuristics, it is not efficient, and in many cases the heuristic computation takes much of the planning time. In this work, we present a more efficient way to use these heuristics for motion planning problems by making use of popular nearest neighbor algorithms. Experimentally, we show an average 8 times reduction in heuristic computation time, resulting in overall planning time being reduced by 66%. with no change in the expanded states or resulting path.

ICRA Conference 2015 Conference Paper

Task-oriented planning for manipulating articulated mechanisms under model uncertainty

• Venkatraman Narayanan
• Maxim Likhachev

Personal robots need to manipulate a variety of articulated mechanisms as part of day-to-day tasks. These tasks are often specific, goal-driven, and permit very little bootstrap time for learning the articulation type. In this work, we address the problem of purposefully manipulating an articulated object, with uncertainty in the type of articulation. To this end, we provide two primary contributions: first, an efficient planning algorithm that, given a set of candidate articulation models, is able to correctly identify the underlying model and simultaneously complete a task; and second, a representation for articulated objects called the Generalized Kinematic Graph (GK-Graph), that allows for modeling complex mechanisms whose articulation varies as a function of the state space. Finally, we provide a practical method to auto-generate candidate articulation models from RGB-D data and present extensive results on the PR2 robot to demonstrate the utility of our representation and the efficiency of our planner.

SoCS Conference 2014 Conference Paper

Anytime Tree-Restoring Weighted A* Graph Search

• Kalin Gochev
• Alla Safonova
• Maxim Likhachev

Incremental graph search methods reuse information from previous searches in order to minimize redundant computation and to find solutions to series of similar search queries much faster than it is possible by solving each query from scratch. In this work, we present a simple, but very effective, technique for performing incremental weighted A* graph search in an anytime fashion. On the theoretical side, we show that our anytime incremental algorithm preserves the strong theoretical guarantees provided by the weighted A* algorithm, such as completeness and bounds on solution cost sub-optimality. We also show that our algorithm can handle a variety of changes to the underlying graph, such as both increasing and decreasing edge costs, and changes in the heuristic. On the experimental side, we demonstrate the effectiveness of our algorithm in the context of (x, y, z, yaw) navigation planning for an unmanned aerial vehicle and compare our algorithm to popular incremental and anytime graph search algorithms.

ICRA Conference 2014 Conference Paper

Motion planning for robotic manipulators with independent wrist joints

• Kalin Gochev
• Venkatraman Narayanan
• Benjamin J. Cohen
• Alla Safonova
• Maxim Likhachev

Advanced modern humanoid robots often have complex manipulators with a large number of degrees of freedom. Thus, motion planning for such manipulators is a very computationally challenging problem. However, often robotic manipulators allow the wrist degrees of freedom to be controlled independently from the configuration of the rest of the arm. In this paper we show how to split the high dimensional planning problem into two lower-dimensional sub-problems — planning for the main arm joints and planning for the wrist joints, without losing guarantees on completeness. This approach is an extension of our previously developed framework for planning with adaptive dimensionality. Experimentally, we show that this approach is very effective in speeding up planning for robotic arms on Willow Garage's PR2 platform. We compare our algorithm with several popular alternative approaches for performing motion planning for robotic arms. The results we observe illustrate that our algorithm provides a good balance between planning time, planning success rate, path consistency, and path quality.

ICRA Conference 2014 Conference Paper

Motion planning for smooth pickup of moving objects

• Arjun Menon
• Benjamin J. Cohen
• Maxim Likhachev

Kinematic planning for robotic arms has been shown to be capable of planning for robust manipulation of static objects. However, this approach falls short when manipulating moving objects such as picking up a jar off of a conveyor belt at a bottling plant. The challenge in carefully picking up moving objects is that these actions require motions that do not involve large decelerations, to avoid jerking the object, as well as figuring out the proper time in which the object can be picked up. We present a search-based kinodynamic motion planning algorithm that generates a time-parameterized trajectory for both the arm and end-effector, capable of carefully picking up the object at the earliest feasible point in its trajectory. To combat the high-dimensionality of the time-parameterized kinodynamic planning problem, our approach employs informative heuristics and adaptive dynamic motion primitives. To validate our approach, we used a 7DOF manipulator on Willow Garage's PR2 robot to pickup objects off of a conveyor belt. We also provide a detailed set of results that demonstrate the planner's ability to generate consistent, low cost trajectories for manipulation.

SoCS Conference 2014 Conference Paper

Multi-Heuristic A

• Sandip Aine
• Siddharth Swaminathan
• Venkatraman Narayanan
• Victor Hwang
• Maxim Likhachev

We present a novel heuristic search framework, called Multi-Heuristic A* (MHA*), that simultaneously uses multiple, arbitrarily inadmissible heuristic functions and one consistent heuristic to search for complete and bounded suboptimal solutions. This simplifies the de- sign of heuristics and enables the search to effectively combine the guiding powers of different heuristic func- tions. We support these claims with experimental results on full-body manipulation for PR2 robots.

ICAPS Conference 2014 Conference Paper

PA*SE: Parallel A* for Slow Expansions

• Mike Phillips
• Maxim Likhachev
• Sven Koenig

Planners need to become faster as we seek to tackle increasingly complicated problems. Much of the recent improvements in computer speed is due to multi-core processors. For planners to take advantage of these types of architectures, we must adapt algorithms for parallel processing. There are a number of planning domains where state expansions are slow. One example is robot motion planning, where most of the time is devoted to collision checking. In this work, we present PA*SE, a novel, parallel version of A* (and weighted A*) which parallelizes state expansions by taking advantage of this property. While getting close to a linear speedup in the number of cores, we still preserve completeness and optimality of A* (bounded sub-optimality of weighted A*). PA*SE applies to any planning problem in which significant time is spent on generating successor states and computing transition costs. We present experimental results on a robot navigation domain (x, y, heading) which requires expensive 3D collision checking for the PR2 robot. We also provide an in-depth analysis of the algorithm’s performance on a 2D navigation problem as we vary the number of cores (up to 32) as well as the time it takes to collision check successors during state expansions.

IROS Conference 2014 Conference Paper

Robotic handwriting: Multi-contact manipulation based on Reactional Internal Contact Hypothesis

• Sung-Kyun Kim
• Joonhee Jo
• Yonghwan Oh
• Sang-Rok Oh
• Siddhartha S. Srinivasa
• Maxim Likhachev

When one uses a hand-held tool, the fingers often make the tool to be in contact with the palm in the form of multi-contact manipulation. Multi-contact manipulation is useful for object-environment interaction tasks because it can provide both powerful grasping of the object body and dexterous manipulation of the object end-effector. However, dealing with the internal link contact with the object is not trivial. In this paper, we propose Reactional Internal Contact Hypothesis that regards the internal contact force as a reaction force so that the desired finger force can be reduced. By taking a handwriting task as an example, optimal configuration search and grasping force computation problems are addressed based on this hypothesis and validated via dynamic simulation.

ICAPS Conference 2014 Conference Paper

Single- and Dual-Arm Motion Planning with Heuristic Search

• Benjamin J. Cohen
• Sachin Chitta
• Maxim Likhachev

Heuristic searches such as A* search are a popular means of finding least-cost plans due to their generality, strong theoretical guarantees on completeness and optimality, simplicity in implementation and consistent behavior. In planning for robotic manipulation, however, these techniques are commonly thought of as impractical due to the high-dimensionality of the planning problem. In this paper, we present a heuristic search-based approach to motion planning for manipulation that does deal effectively with the high-dimensionality of the problem. The paper presents a summary of the approach along with applications to single-arm and dual-arm motion planning with upright constraints on a PR2 robot operating in non-trivial cluttered spaces. An extensive experimental analysis in both simulation and on a physical PR2 shows that, in terms of runtime, our approach is on par with other most common sampling-based approaches and due to its deterministic cost-minimization, the computed motions are of good quality and are consistent, i. e. the resulting plans tend to be similar for similar tasks.

IROS Conference 2014 Conference Paper

State lattice with controllers: Augmenting lattice-based path planning with controller-based motion primitives

• Jonathan Butzke
• Krishna Sapkota
• Kush Prasad
• Brian MacAllister
• Maxim Likhachev

State lattice-based planning has been used in navigation for ground, water, aerial and space robots. State lattices are typically constructed of simple motion primitives connecting one state to another. There are situations where these metric motions may not be available, such as in GPS-denied areas. In many of these cases, however, the robot may have some additional sensing capability that is not being fully utilized by the planner. For example, if the robot has a camera it may be able to use simple visual servoing techniques to navigate through a GPS-denied region. Likewise, a LIDAR may allow the robot to skirt along an environmental feature even if there is not enough information to generate an accurate pose estimate. In this paper we present an expansion of the state lattice framework that allows us to incorporate controller-based motion primitives and external perceptual triggers directly into the planning process. We provide a formal description of our method of constructing the search graph in these cases as well as presenting real-world and simulated testing data showing the practical application of this approach.

ICRA Conference 2013 Conference Paper

A single planner for a composite task of approaching, opening and navigating through non-spring and spring-loaded doors

• Steven Gray 0003
• Sachin Chitta
• Vijay Kumar 0001
• Maxim Likhachev

Opening and navigating through doors remains a challenging problem, particularly in cluttered environments and for spring-loaded doors. Passing through doors, especially spring-loaded doors, requires making and breaking contacts with the door and preventing the door from closing while passing through. In this work, we present a planning framework that handles non-spring and spring-loaded doors, in cluttered or confined workspaces, planning the approach to the door, pushing or pulling it open, and passing through. Because the problem is solved in a combined search space, the planner yields an overall least-cost path. The planner is able to insert a transition between robot-door contacts at any point along the plan. We utilize a compact graph-based representation of the problem to keep planning times low. We precompute the force workspace of the end-effectors to eliminate checks against joint torque limits at plan time. We have validated our solution in both simulation and real-world experiments on the PR2 mobile manipulation platform; the robot is able to successfully open a variety of spring-loaded and non-spring-loaded doors by pushing and pulling.

ICRA Conference 2013 Conference Paper

Anytime incremental planning with E-Graphs

• Mike Phillips
• Andrew Dornbush
• Sachin Chitta
• Maxim Likhachev

Robots operating in real world environments need to find motion plans quickly. Robot motion should also be efficient and, when operating among people, predictable. Minimizing a cost function, e. g. path length, can produce short, reasonable paths. Anytime planners are ideal for this since they find an initial solution quickly and then improve solution quality as time permits. In previous work, we introduced the concept of Experience Graphs, which allow search-based planners to find paths with bounded sub-optimality quickly by reusing parts of previous paths where relevant. Here we extend planning with Experience Graphs to work in an anytime fashion so a first solution is found quickly using prior experience. As time allows, the dependence on this experience is reduced in order to produce closer to optimal solutions. We also demonstrate how Experience Graphs provide a new way of approaching incremental planning as they naturally reuse information when the environment, the starting configuration of the robot or the goal configuration change. Experimentally, we demonstrate the anytime and incremental properties of our algorithm on mobile manipulation tasks in both simulation and on a real PR2 robot.

SoCS Conference 2013 Conference Paper

Anytime Truncated D*: Anytime Replanning with Truncation

• Sandip Aine
• Maxim Likhachev

Incremental heuristic searches reuse their previous search efforts to speed up the current search. Anytime search algorithms iteratively tune the solutions based on available search time. Anytime D* (AD*) is an incremental anytime search algorithm that combines these two approaches. AD* uses an inflated heuristic to produce bounded suboptimal solutions and improves the solution by iteratively decreasing the inflation factor. If the environment changes, AD* recomputes a new solution by propagating the new costs. Recently, a different approach to speed up replanning (TLPA*/TD* Lite) was proposed that relies on selective truncation of cost propagations instead of heuristic inflation. In this work, we present an algorithm called Anytime Truncated D* (ATD*) that combines heuristic inflation with truncation in an anytime fashion. We develop truncation rules that can work with an inflated heuristic without violating the completeness/suboptimality guarantees, and show how these rules can be applied in conjunction with heuristic inflation to iteratively refine the replanning solutions with minimal reexpansions. We explain ATD*, discuss its analytical properties and present experimental results for 2D and 3D (x, y, heading) path planning demonstrating its efficacy for anytime replanning.

ICAPS Conference 2013 Conference Paper

Incremental Planning with Adaptive Dimensionality

• Kalin Gochev
• Alla Safonova
• Maxim Likhachev

Path planning is often a high-dimensional computationally-expensive planning problem as it requires reasoning about the kinodynamic constraints of the robot and collisions of the robot with the environment. However, large regions of the environment are typically benign enough that a much faster low-dimensional planning combined with a local path following controller suffice. Planning with Adaptive Dimensionality that was recently developed makes use of this observation and iteratively constructs and searches a state-space consisting of mainly low-dimensional states. It only introduces regions of high-dimensional states into the state-space where they are necessary to ensure completeness and bounds on sub-optimality. However, due to its iterative nature, the approach relies on running a series of weighted A* searches. In this paper, we introduce and apply to Planning with Adaptive Dimensionality a simple but very effective incremental version of weighted A* that reuses its previously generated search tree if available. On the theoretical side, the new algorithm preserves guarantees on completeness and bounds on sub-optimality. On the experimental side, it speeds up 3D (x, y, heading) path planning with a full-body collision checking by up to a factor of 5. Our results also show that it tends to be much faster than applying alternative incremental graph search techniques such as D* to Planning with Adaptive Dimensionality.

ICRA Conference 2013 Conference Paper

Path planning for non-circular micro aerial vehicles in constrained environments

• Brian MacAllister
• Jonathan Butzke
• Aleksandr Kushleyev
• Harsh Pandey
• Maxim Likhachev

Operating micro aerial vehicles (MAVs) outside of the bounds of a rigidly controlled lab environment, specifically one that is unstructured and contains unknown obstacles, poses a number of challenges. One of these challenges is that of quickly determining an optimal (or nearly so) path from the MAVs current position to a designated goal state. Past work in this area using full-size unmanned aerial vehicles (UAVs) has predominantly been performed in benign environments. However, due to their small size, MAVs are capable of operating in indoor environments which are more cluttered. This requires planners to account for the vehicle heading in addition to its spatial position in order to successfully navigate. In addition, due to the short flight times of MAVs along with the inherent hazards of operating in close proximity to obstacles, we desire the trajectories to be as cost-optimal as possible. Our approach uses an anytime planner based on A* that performs a graph search on a four-dimensional (4-D) (x, y, z, heading) lattice. This allows for the generation of close-to-optimal trajectories based on a set of precomputed motion primitives along with the capability to provide trajectories in real-time allowing for on-the-fly re-planning as new sensor data is received. We also account for arbitrary vehicle shapes, permitting the use of a non-circular footprint during the planning process. By not using the overly conservative circumscribed circle for collision checking, we are capable of successfully finding optimal paths through cluttered environments including those with narrow hallways. Analytically, we show that our planner provides bounds on the sub-optimality of the solution it finds. Experimentally, we show that the planner can operate in real-time in both a simulated and real-world cluttered environments.

IROS Conference 2013 Conference Paper

Perception and motion planning for pick-and-place of dynamic objects

• Anthony Cowley
• Benjamin J. Cohen
• William Marshall
• Camillo J. Taylor
• Maxim Likhachev

Mobile manipulators have brought a new level of flexibility to traditional automation tasks such as tabletop manipulation, but are not yet capable of the same speed and reliability as industrial automation. We present approaches to 3D perception and manipulator motion planning that enable a general purpose robotic platform to recognize and manipulate a variety of objects at a rate of one pick-and-place operation every 6. 7 s, and work with a conveyor belt carrying objects at a speed of 33 cm /s.

IROS Conference 2013 Conference Paper

Planning for opportunistic surveillance with multiple robots

• Dinesh Thakur
• Maxim Likhachev
• James Keller 0002
• Vijay Kumar 0001
• Vladimir N. Dobrokhodov
• Kevin D. Jones
• Jeff Wurz
• Isaac Kaminer

We are interested in the multiple robot surveillance problem where robots must allocate waypoints to be visited among themselves and plan paths through different waypoints while avoiding obstacles. Furthermore, the robots are allocated specific times to reach their respective goal locations and as a result they have to decide which robots have to visit which waypoints. Such a problem has the challenge of computing the allocation of waypoints across robots, ordering for these waypoints and dynamical feasibility of the paths between waypoints. We present an algorithm that runs a series of graph searches to solve the problem and provide theoretical analysis that our approach yields an optimal solution. We present simulated results as well as experiments on two UAVs that validate the capability of our algorithm. For a single robot, we can solve instances having 10–15 waypoints and for multiple robots, instances having five robots and 10 waypoints can be solved.

ICRA Conference 2013 Conference Paper

Planning under topological constraints using beam-graphs

• Venkatraman Narayanan
• Paul Vernaza
• Maxim Likhachev
• Steven M. LaValle

We present a framework based on graph search for navigation in the plane with a variety of topological constraints. The method is based on modifying a standard graph-based navigation approach to keep an additional state variable that encodes topological information about the path. The topological information is represented by a sequence of virtual sensor beam crossings. By considering classes of beam crossing sequences to be equivalent under certain equivalence relations, we obtain a general method for planning with topological constraints that subsumes existing approaches while admitting more favorable representational characteristics. We provide experimental results that validate the approach and show how the planner can be used to find loop paths for autonomous surveillance problems, simultaneously satisfying minimum-cost objectives and in dynamic environments. As an additional application, we demonstrate the use of our planner on the PR2 robot for automated building of 3D object models.

ICRA Conference 2013 Conference Paper

Planning with approximate preferences and its application to disambiguating human intentions in navigation

• Bradford Neuman
• Maxim Likhachev

This paper addresses the problem of planning in the presence of humans modeled as dynamic obstacles with multiple hypotheses on their trajectories and actions which can disambiguate between the hypotheses. To solve this problem, we develop and analyze a generalization to the PPCP (Probabilistic Planning with Clear Preferences) algorithm that allows us to efficiently solve problems with approximate preferences on missing information. The approach finds policies with bounded suboptimal expected cost and scales well with the number of people, only disambiguating between the trajectories of people when necessary. We present simulated results as well as experiments on two different physical robots demonstrating the capability of this planner.

AAAI Conference 2013 Conference Paper

Truncated Incremental Search: Faster Replanning by Exploiting Suboptimality

• Sandip Aine
• Maxim Likhachev

Incremental heuristic searches try to reuse their previous search efforts whenever these are available. As a result, they can often solve a sequence of similar planning problems much faster than planning from scratch. State-of-the-art incremental heuristic searches such as LPA*, D* and D* Lite all work by propagating cost changes to all the states on the search tree whose gvalues (the costs of computed paths from the start) are no longer optimal. While such a complete propagation of cost changes is required to ensure optimality, the propagations can be stopped much earlier if we are looking for solutions within a given suboptimality bound. We present a framework called Truncated Incremental Search that builds on this observation, and uses a target suboptimality bound to efficiently restrict the cost propagations. Using this framework, we develop two algorithms, Truncated LPA* (TLPA*) and Truncated D* Lite (TD* Lite). We discuss their analytical properties and present experimental results for 2D and 3D (x, y, heading) path planning that show significant improvement in runtime over existing incremental heuristic searches when searching for close-to-optimal solutions. In addition, unlike typical incremental searches, Truncated Incremental Search is much less dependent on the proximity of the cost changes to the goal of the search due to the early termination of the cost change propagation.

IROS Conference 2012 Conference Paper

Anytime Safe Interval Path Planning for dynamic environments

• Venkatraman Narayanan
• Mike Phillips
• Maxim Likhachev

Path planning in dynamic environments is significantly more difficult than navigation in static spaces due to the increased dimensionality of the problem, as well as the importance of returning good paths under time constraints. Anytime planners are ideal for these types of problems as they find an initial solution quickly and then improve it as time allows. In this paper, we develop an anytime planner that builds off of Safe Interval Path Planning (SIPP), which is a fast A*-variant for planning in dynamic environments that uses intervals instead of timesteps to represent the time dimension of the problem. In addition, we introduce an optional time-horizon after which the planner drops time as a dimension. On the theoretical side, we show that in the absence of time-horizon our planner can provide guarantees on completeness as well as bounds on the sub-optimality of the solution with respect to the original space-time graph. We also provide simulation experiments for planning for a UAV among 50 dynamic obstacles, where we can provide safe paths for the next 15 seconds of execution within 0. 05 seconds. Our results provide a strong evidence for our planner working under real-time constraints.

ICRA Conference 2012 Conference Paper

Combining global and local planning with guarantees on completeness

• Haojie Zhang
• Jonathan Butzke
• Maxim Likhachev

Planning with kinodynamic constraints is often required for mobile robots operating in cluttered, complex environments. A common approach is to use a two-dimensional (2-D) global planner for long range planning, and a short range higher dimensional planner or controller capable of satisfying all of the constraints on motion. However, this approach is incomplete and can result in oscillations and the inability to find a path to the goal. In this paper we present an approach to solving this problem by combining the global and local path planning problem into a single search using a combined 2-D and higher dimensional state-space.

SoCS Conference 2012 Conference Paper

E-Graphs: Bootstrapping Planning with Experience Graphs

• Mike Phillips
• Benjamin J. Cohen
• Sachin Chitta
• Maxim Likhachev

In this paper, we develop an online motion planning approach which learns from its planning episodes (experiences) a graph, an Experience Graph. On the theoretical side, we show that planning with Experience graphs is complete and provides bounds on suboptimality with respect to the graph that represents the original planning problem. Experimentally, we show in simulations and on a physical robot that our approach is particularly suitable for higher-dimensional motion planning tasks such as planning for two armed mobile manipulation.

SoCS Conference 2012 Conference Paper

Efficiently Finding Optimal Winding-Constrained Loops in the Plane: Extended Abstract

• Paul Vernaza
• Venkatraman Narayanan
• Maxim Likhachev

We present a method to efficiently find winding-constrained loops in the plane that are optimal with respect to a minimum- cost objective and in the presence of obstacles. Our approach is similar to a typical graph-based search for an optimal path in the plane, but with an additional state variable that encodes information about path homotopy. Upon finding a loop, the value of this state corresponds to a line integral over the loop that indicates how many times it winds around each obstacle, enabling us to reduce the problem of finding paths satisfying winding constraints to that of searching for paths to suitable states in this augmented state space. We give an intuitive in- terpretation of the method based on fluid mechanics and show how this yields a way to perform the necessary calculations efficiently. Results are given in which we use our method to find optimal routes for autonomous surveillance and intruder containment.

AAAI Conference 2012 Conference Paper

Heuristic Search Comes of Age

• Nathan Sturtevant
• Ariel Felner
• Maxim Likhachev
• Wheeler Ruml

In looking back on the last five to ten years of work in heuristic search a few trends emerge. First, there has been a broadening of research topics studied. Second, there has been a deepened understanding of the theoretical foundations of search. Third, and finally, there have been increased connections with work in other fields. This paper, corresponding to a AAAI 2012 invited talk on recent work in heuristic search, highlights these trends in a number of areas of heuristic search. It is our opinion that the sum of these trends reflects the growth in the field and the fact that heuristic search has come of age.

ICRA Conference 2012 Conference Paper

Navigation in three-dimensional cluttered environments for mobile manipulation

• Armin Hornung
• Mike Phillips
• Edward Gil Jones
• Maren Bennewitz
• Maxim Likhachev
• Sachin Chitta

Collision-free navigation in cluttered environments is essential for any mobile manipulation system. Traditional navigation systems have relied on a 2D grid map projected from a 3D representation for efficiency. This approach, however, prevents navigation close to objects in situations where projected 3D configurations are in collision within the 2D grid map even if actually no collision occurs in the 3D environment. Accordingly, when using such a 2D representation for planning paths of a mobile manipulation robot, the number of planning problems which can be solved is limited and suboptimal robot paths may result. We present a fast, integrated approach to solve path planning in 3D using a combination of an efficient octree-based representation of the 3D world and an anytime search-based motion planner. Our approach utilizes a combination of multi-layered 2D and 3D representations to improve planning speed, allowing the generation of almost real-time plans with bounded sub-optimality. We present extensive experimental results with the two-armed mobile manipulation robot PR2 carrying large objects in a highly cluttered environment. Using our approach, the robot is able to efficiently plan and execute trajectories while transporting objects, thereby often moving through demanding, narrow passageways.

ICRA Conference 2012 Conference Paper

Planning with adaptive dimensionality for mobile manipulation

• Kalin Gochev
• Alla Safonova
• Maxim Likhachev

Mobile manipulation planning is a hard problem composed of multiple challenging sub-problems, some of which require searching through large high-dimensional state-spaces. The focus of this work is on computing a trajectory to safely maneuver an object through an environment, given the start and goal configurations. In this work we present a heuristic search-based deterministic mobile manipulation planner, based on our recently-developed algorithm for planning with adaptive dimensionality. Our planner demonstrates reasonable performance, while also providing strong guarantees on completeness and suboptimality bounds with respect to the graph representing the problem.

AAAI Conference 2012 Conference Paper

Search-Based Path Planning with Homotopy Class Constraints in 3D

• Subhrajit Bhattacharya
• Maxim Likhachev
• Vijay Kumar

ICRA Conference 2012 Conference Paper

Search-based planning for dual-arm manipulation with upright orientation constraints

• Benjamin J. Cohen
• Sachin Chitta
• Maxim Likhachev

Dual-arm manipulation is an increasingly important skill for robots operating in home, retail and industrial environments. Dual-arm manipulation is especially essential for tasks involving large objects which are harder to grasp and manipulate using a single arm. In this work, we address dual-arm manipulation of objects in indoor environments. We are particularly focused on tasks that involve an upright orientation constraint on the grasped object. Such constraints are often present in human environments, e. g. when manipulating a tray of food or a container with fluids. In this paper, we present a search-based approach that is capable of planning dual-arm motions, often within one second, in cluttered environments while adhering to the orientation constraints. Our approach systematically constructs a graph in task space and generates motions that are consistent across runs with similar start/goal configurations and are low-cost. These motions come with guarantees on completeness and bounds on the suboptimality with respect to the graph that encodes the planning problem. For many problems, the consistency of the generated motions is important as it helps make the actions of the robot more predictable for a human interacting with the robot.

ICRA Conference 2012 Conference Paper

Using state dominance for path planning in dynamic environments with moving obstacles

• Juan Pablo Gonzalez
• Andrew Dornbush
• Maxim Likhachev

Path planning in dynamic environments with moving obstacles is computationally complex since it requires modeling time as an additional dimension. While in other domains there are state dominance relationships that can significantly reduce the complexity of the search, in dynamic environments such relationships do not exist. This paper presents a novel state dominance relationship tailored specifically for dynamic environments, and presents a planner that uses that property to plan paths over ten times faster than without using state dominance.

ICRA Conference 2011 Conference Paper

Cart pushing with a mobile manipulation system: Towards navigation with moveable objects

• Jonathan Scholz
• Sachin Chitta
• Bhaskara Marthi
• Maxim Likhachev

Robust navigation in cluttered environments has been well addressed for mobile robotic platforms, but the problem of navigating with a moveable object like a cart has not been widely examined. In this work, we present a planning and control approach to navigation of a humanoid robot while pushing a cart. We show how immediate information about the environment can be integrated into this approach to achieve safer navigation in the presence of dynamic obstacles. We demonstrate the robustness of our approach through long-running experiments with the PR2 mobile manipulation robot in a typical indoor office environment, where the robot faced narrow and high-traffic passageways with very limited clearance.

SoCS Conference 2011 Conference Paper

Path Planning with Adaptive Dimensionality

• Kalin Gochev
• Benjamin J. Cohen
• Jonathan Butzke
• Alla Safonova
• Maxim Likhachev

Path planning quickly becomes computationally hard as the dimensionality of the state-space increases. In this paper, we present a planning algorithm intended to speed up path planning for high-dimensional state-spaces such as robotic arms. The idea behind this work is that while planning in a high-dimensional state-space is often necessary to ensure the feasibilityof the resulting path, large portions of the path have a lower-dimensional structure. Based on this observation, our algorithm iteratively constructs a state-space of an adaptive dimensionality--a state-space that is high-dimensional only where the higher dimensionality is absolutely necessary for finding a feasible path. This often reduces drastically the size of the state-space, and as a result, the planning time and memory requirements. Analytically, we show that our method is complete and is guaranteed to find a solution if one exists, within a specified suboptimality bound. Experimentally, we apply the approach to 3D vehicle navigation (x, y, heading), and to a 7 DOF robotic arm on the Willow Garage's PR2 robot. The results from our experiments suggest that ourmethod can be substantially faster than some of the state-of-the-art planning algorithms optimized for those tasks.

IROS Conference 2011 Conference Paper

Planning for landing site selection in the aerial supply delivery

• Aleksandr Kushleyev
• Brian MacAllister
• Maxim Likhachev

In the aerial supply delivery problem, an unmanned aircraft needs to deliver supplies as close as possible to the desired location. This involves choosing, flying to, sensing, and landing at a safe landing site that is most accessible from the goal. The problem is often complicated by the fact that the availability of these landing sites may be unknown before the mission begins. Therefore, the aircraft needs to compute a sequence of actions that will minimize the expected value of the objective function. The problem of computing this sequence corresponds to planning under uncertainty in the environment. In this paper, we show how it can be solved efficiently via a recently developed probabilistic planning framework, called Probabilistic Planning with Clear Preferences (PPCP). We show that the problem satisfies the Clear Preferences assumption required by PPCP, and therefore all the theoretical guarantees continue to hold. The experimental results in simulation demonstrate that our approach can solve large-scale problems in realtime while experiments on our custom quad-rotor helicopter provide a proof of concept for the planner.

SoCS Conference 2011 Conference Paper

Planning for Landing Site Selection in the Aerial Supply Delivery

• Aleksandr Kushleyev
• Brian MacAllister
• Maxim Likhachev

In the aerial supply delivery problem, an un-manned aircraft needs to deliver supplies as close as possible to the desired goal location. This involves choosing and landing at a landing site that is closest to or most accessible from the desired goal location. The problem is complicated by the fact that the status of candidate landing sites is unknown before the mission begins, and instead the aircraft needs to compute a sequence according to which it flies and senses the candidate landing sites in order to land as quickly as possible. The problem of computing this sequence corresponds to planning under uncertainty about environment. In this paper, we show how it can be solved efficiently via a recently developed probabilistic planning framework, called Probabilistic Planning with Clear Preferences (PPCP). We show that the problem satisfies the Clear Preferences assumption required by PPCP, and therefore all the theoretical guarantees continue to hold. The experimental results in simulation show that our approachcan solve large-scale problems in real-time while experiments on a physical quad-rotor provide proof of concept.

ICRA Conference 2011 Conference Paper

Planning for Manipulation with Adaptive Motion Primitives

• Benjamin J. Cohen
• Gokul Subramania
• Sachin Chitta
• Maxim Likhachev

In this paper, we present a search-based motion planning algorithm for manipulation that handles the high dimensionality of the problem and minimizes the limitations associated with employing a strict set of pre-defined actions. Our approach employs a set of adaptive motion primitives comprised of static motions with variable dimensionality and on-the-fly motions generated by two analytical solvers. This method results in a slimmer, multi-dimensional lattice and offers the ability to satisfy goal constraints with precision. To validate our approach, we used a 7DOF manipulator to perform experiments on a real mobile manipulation platform (Willow Garage's PR2). Our results demonstrate the effectiveness of the planner in efficiently navigating cluttered spaces; the method generates consistent, low-cost motion trajectories, and guarantees the search is complete with bounds on the suboptimality of the solution.

IROS Conference 2011 Conference Paper

Planning for multi-robot exploration with multiple objective utility functions

• Jonathan Butzke
• Maxim Likhachev

With the continued improvements in portable computing power and sensing systems it is becoming more common for groups of robots to cooperate to achieve a goal. When the robots are operating in an initially unknown environment, the most natural form of cooperation is multi-robot exploration. For many years frontier based approaches have been commonly used to assign target points for each of the robots in the group based on expected information gain and distance to travel. In this paper we present an expansion to these approaches allowing for the incorporation of multiple objective utility functions that allow adjustment of the exploration priorities both for the individual robots and the group as a whole. In addition, we discuss real world results of our algorithm including our first place finish at the Old Ram Shed Challenge and second place at the MAGIC2010 main competition.

SoCS Conference 2011 Conference Paper

Planning in Domains with Cost Function Dependent Actions

• Mike Phillips
• Maxim Likhachev

In a number of graph search-based planning problems, the value of the cost function that is being minimized also affects the set of possible actions at some or all the states in the graph. In such planning problems, the cost function typically becomes one of the state variables thereby increasing the dimensionality of the planning problem, and consequently the size of the graph that represents the problem. In this paper, we show how to avoid this increase in the dimensionality for weighted search (with bounded suboptimality) whenever the availability of the actions is monotonically non-increasing with the increase in the cost function.

AAAI Conference 2011 Conference Paper

Planning in Domains with Cost Function Dependent Actions

• Mike Phillips
• Maxim Likhachev

In a number of graph search-based planning problems, the value of the cost function that is being minimized also affects the set of possible actions at some or all the states in the graph. For example, in path planning for a robot with a limited battery power, a common cost function is energy consumption, whereas the level of remaining energy affects the navigational capabilities of the robot. Similarly, in path planning for a robot navigating dynamic environments, a total traversal time is a common cost function whereas the timestep affects whether a particular transition is valid. In such planning problems, the cost function typically becomes one of the state variables thereby increasing the dimensionality of the planning problem, and consequently the size of the graph that represents the problem. In this paper, we show how to avoid this increase in the dimensionality for the planning problems whenever the availability of the actions is monotonically non-increasing with the increase in the cost function. We present three variants of A* search for dealing with such planning problems: a provably optimal version, a suboptimal version that scales to larger problems while maintaining a bound on suboptimality, and finally a version that relaxes our assumption on the relationship between the cost function and action space. Our experimental analysis on several domains shows that the presented algorithms achieve up to several orders of magnitude speed up over the alternative approaches to planning.

SoCS Conference 2011 Conference Paper

Search-Based Planning with Provable Suboptimality Bounds for Continuous State Spaces

• Juan Pablo Gonzalez
• Maxim Likhachev

Search-based planning is widely used for mobile robot motion planning because of its guarantees of optimality and completeness. In continuous state-spaces, however, most existing approaches have significant limitations in terms of optimality and completeness because of the underlying grid used. We propose an approach that eliminates the dependency on grids by using more general equivalence classes to quickly find an initial solution and instead of pruning states that fall within an equivalence class and have higher cost, we use an inflated heuristic to lower the priority of these states in the search. In further iterations, we reduce the inflated heuristic in a principled way, thus providing fast solutions with provable suboptimality bounds that can be improved as time allows. The proposed approach produces smooth paths with the resolution dictated by the action set. Finer action sets produce higher resolution paths that are more computationally intensive to calculate and coarser action sets produce lower resolution paths that are faster to compute. To the best of our knowledge, this is the first algorithm that is able to plan in continuous state-spaces with provable guarantees on completeness and bounds on suboptimality for a given action set. Experimental results on 3D (x, y, theta) path planning show that, on average, this approach is able to find paths in less than two seconds that are within 2% of the optimal path cost in worlds of up to 1000x1000 m with a minimum step size of one meter.

ICRA Conference 2011 Conference Paper

SIPP: Safe interval path planning for dynamic environments

• Mike Phillips
• Maxim Likhachev

Robotic path planning in static environments is a thoroughly studied problem that can typically be solved very efficiently. However, planning in the presence of dynamic obstacles is still computationally challenging because it requires adding time as an additional dimension to the search-space explored by the planner. In order to avoid the increase in the dimensionality of the planning problem, most real-time approaches to path planning treat dynamic obstacles as static and constantly re-plan as dynamic obstacles move. Although gaining efficiency, these approaches sacrifice optimality and even completeness. In this paper, we develop a planner that builds on the observation that while the number of safe timesteps in any configuration may be unbounded, the number of safe time intervals in a configuration is finite and generally very small. A safe interval is a time period for a configuration with no collisions and if it were extended one timestep in either direction, it would then be in collision. The planner exploits this observation and constructs a search-space with states defined by their configuration and safe interval, resulting in a graph that generally only has a few states per configuration. On the theoretical side, we show that our planner can provide the same optimality and completeness guarantees as planning with time as an additional dimension. On the experimental side, in simulation tests with up to 200 dynamic obstacles, we show that our planner is significantly faster, making it feasible to use in real-time on robots operating in large dynamic environments. We also ran several real robot trials on the PR2, a mobile manipulation platform.

ICRA Conference 2010 Conference Paper

High-dimensional planning on the GPU

• Joseph T. Kider Jr.
• Mark Henderson
• Maxim Likhachev
• Alla Safonova

Optimal heuristic searches such as A* search are commonly used for low-dimensional planning such as 2D path finding. These algorithms however, typically do not scale well to high-dimensional planning problems such as motion planning for robotic arms, computing motion trajectories for non-holonomic robotic vehicles and motion synthesis for humanoid characters. A recently developed randomized version of A* search, called R* search, scales to higher-dimensional planning problems by trading off deterministic optimality guarantees of A* for probabilistic sub-optimality guarantees. In this paper, we show that in addition to its scalability, R* lends itself well to a parallel implementation. In particular, we demonstrate how R* can be implemented on the GPU. On the theoretical side, the GPU version of R*, called R*GPU, preserves all the theoretical properties of R* including its probabilistic bounds on sub-optimality. On the experimental side, we show that R*GPU consistently produces lower cost solutions, scales better in terms of memory, and runs faster than R*. These results hold for both motion planning for a 6DOF robot arm planar as well as 2D path finding.

ICRA Conference 2010 Conference Paper

Multi-agent path planning with multiple tasks and distance constraints

• Subhrajit Bhattacharya
• Maxim Likhachev
• Vijay Kumar 0001

The DPC algorithm developed in our previous work is an efficient way of computing optimal trajectories for multiple robots in a distributed fashion with time-parameterized constraints on the distances between pairs of robots. In the present work we extend DPC to the problem of multiple task execution. While this extended problem inherits all the objectives, complexities and constraints of the basic DPC algorithm, each robot is also given an unordered set of tasks that it has to execute before it reaches its goal. There is no specific order imposed on the tasks assigned to a particular robot. The algorithm decides the order of execution of the tasks such that an optimal solution is attained while the time-parametrized distance constraints are satisfied along with successful execution of the tasks. We solve this problem by designing a “State-task Graph” that represents a product of the state-space graph and the task graph. We then develop an efficient heuristic function for performing searches in this graph.

ICRA Conference 2010 Conference Paper

Planning for autonomous door opening with a mobile manipulator

• Sachin Chitta
• Benjamin J. Cohen
• Maxim Likhachev

Computing a motion that enables a mobile manipulator to open a door is challenging because it requires tight coordination between the motions of the arm and the base. Hard-coding the motion, on the other hand, is infeasible since doors vary widely in their sizes and types, some doors are opened by pulling and others by pushing, and indoor spaces often contain obstacles that limit the freedom of the mobile manipulator and the degree to which the doors open up. In this paper, we show how to overcome the high-dimensionality of the planning problem by identifying a graph-based representation that is small enough for efficient planning yet rich enough to contain feasible motions that open doors. The use of graph search-based motion planning enables us to handle consistently the wide variance of conditions under which doors need to be open. We demonstrate our approach on the PR2 robot - a mobile manipulator with an omnidirectional base and a 7 degree of freedom arm. The robot was successful in opening a variety of doors both by pulling and pushing.

SoCS Conference 2010 Conference Paper

Search-Based Path Planning with Homotopy Class Constraints

• Subhrajit Bhattacharya
• Vijay Kumar 0001
• Maxim Likhachev

Goal-directed path planning is one of the basic and widely studied problems in the field of mobile robotics. Homotopy classes of trajectories, arising due to the presence of obstacles, are defined as sets of trajectories that can be transformed into each other by gradual bending and stretching without colliding with obstacles. The problem of finding least-cost paths restricted to a specific homotopy class or finding least-cost paths that do not belong to certain homotopy classes arises frequently in such applications as predicting paths for dynamic entities and computing heuristics for path planning with dynamic constraints. In the present work, we develop a compact way of representing homotopy classes and propose an efficient method of graph search-based optimal path planning with constraints on homotopy classes. The method is based on representing the environment of the robot as a complex plane and making use of the Cauchy Integral Theorem. We prove optimality of the method and show its efficiency experimentally.

ICRA Conference 2010 Conference Paper

Search-based planning for manipulation with motion primitives

• Benjamin J. Cohen
• Sachin Chitta
• Maxim Likhachev

Heuristic searches such as A* search are highly popular means of finding least-cost plans due to their generality, strong theoretical guarantees on completeness and optimality and simplicity in the implementation. In planning for robotic manipulation however, these techniques are commonly thought of as impractical due to the high-dimensionality of the planning problem. In this paper, we present a heuristic search-based manipulation planner that does deal effectively with the high-dimensionality of the problem. The planner achieves the required efficiency due to the following three factors: (a) its use of informative yet fast-to-compute heuristics; (b) its use of basic (small) motion primitives as atomic actions; and (c) its use of ARA* search which is an anytime heuristic search with provable bounds on solution suboptimality. Our experimental analysis on a real mobile manipulation platform with a 7-DOF robotic manipulator shows the ability of the planner to solve manipulation in cluttered spaces by generating consistent, low-cost motion trajectories while providing guarantees on completeness and bounds on suboptimality.

IJCAI Conference 2009 Conference Paper

• Alex Nash
• Sven Koenig
• Maxim Likhachev

We study path planning on grids with blocked and unblocked cells. Any-angle path-planning algorithms find short paths fast because they propagate information along grid edges without constraining the resulting paths to grid edges. Incremental pathplanning algorithms solve a series of similar pathplanning problems faster than repeated single-shot searches because they reuse information from the previous search to speed up the next one. In this paper, we combine these ideas by making the anyangle path-planning algorithm Basic Theta* incremental. This is non-trivial because Basic Theta* does not fit the standard assumption that the parent of a vertex in the search tree must also be its neighbor. We present Incremental Phi* and show experimentally that it can speed up Basic Theta* by about one order of magnitude for path planning with the freespace assumption.

IROS Conference 2009 Conference Paper

Efficient cost computation in cost map planning for non-circular robots

• Jennifer E. King
• Maxim Likhachev

For a robot with a circular footprint, obstacles in a map can be inflated by the radius of the footprint, and planning can treat the robot as a point robot. Many robotic vehicles however have non-circular footprints. When operating in cluttered spaces it therefore becomes important to evaluate the footprint of these robots against a cost map. This evaluation is one of the major computational burdens in planning for robots whose footprints can not be assumed to be circular. In this paper, we propose an efficient method for evaluating a footprint of the robot against a cost map. Our method involves a transformation of the set of points covered by the footprint of the robot into two sets of points: points that should be evaluated against the cost map with inflated obstacles, and points that should be evaluated against the original cost map. The cumulative number of these points is much smaller than the number of points in the original footprint of the robot. Moreover, the method automatically reduces the robot to a single point when its footprint is circular. On the theoretical side, the paper proves the correctness of our method. On the experimental side, the paper shows that the method results in a significant speedup.

AIJ Journal 2009 Journal Article

Probabilistic planning with clear preferences on missing information

• Maxim Likhachev
• Anthony Stentz

For many real-world problems, environments at the time of planning are only partially-known. For example, robots often have to navigate partially-known terrains, planes often have to be scheduled under changing weather conditions, and car route-finders often have to figure out paths with only partial knowledge of traffic congestions. While general decision-theoretic planning that takes into account the uncertainty about the environment is hard to scale to large problems, many such problems exhibit a special property: one can clearly identify beforehand the best (called clearly preferred) values for the variables that represent the unknowns in the environment. For example, in the robot navigation problem, it is always preferred to find out that an initially unknown location is traversable rather than not, in the plane scheduling problem, it is always preferred for the weather to remain a good flying weather, and in route-finding problem, it is always preferred for the road of interest to be clear of traffic. It turns out that the existence of the clear preferences can be used to construct an efficient planner, called PPCP (Probabilistic Planning with Clear Preferences), that solves these planning problems by running a series of deterministic low-dimensional A*-like searches. In this paper, we formally define the notion of clear preferences on missing information, present the PPCP algorithm together with its extensive theoretical analysis, describe several useful extensions and optimizations of the algorithm and demonstrate the usefulness of PPCP on several applications in robotics. The theoretical analysis shows that once converged, the plan returned by PPCP is guaranteed to be optimal under certain conditions. The experimental analysis shows that running a series of fast low-dimensional searches turns out to be much faster than solving the full problem at once since memory requirements are much lower and deterministic searches are orders of magnitude faster than probabilistic planning.

ICRA Conference 2009 Conference Paper

Search-based planning for a legged robot over rough terrain

• Paul Vernaza
• Maxim Likhachev
• Subhrajit Bhattacharya
• Sachin Chitta
• Aleksandr Kushleyev
• Daniel D. Lee

We present a search-based planning approach for controlling a quadrupedal robot over rough terrain. Given a start and goal position, we consider the problem of generating a complete joint trajectory that will result in the legged robot successfully moving from the start to the goal. We decompose the problem into two main phases: an initial global planning phase, which results in a footstep trajectory; and an execution phase, which dynamically generates a joint trajectory to best execute the footstep trajectory. We show how R* search can be employed to generate high-quality global plans in the high-dimensional space of footstep trajectories. Results show that the global plans coupled with the joint controller result in a system robust enough to deal with a variety of terrains.

ICRA Conference 2009 Conference Paper

Time-bounded lattice for efficient planning in dynamic environments

• Aleksandr Kushleyev
• Maxim Likhachev

For vehicles navigating initially unknown cluttered environments, current state-of-the-art planning algorithms are able to plan and re-plan dynamically-feasible paths efficiently and robustly. It is still a challenge, however, to deal well with the surroundings that are both cluttered and highly dynamic. Planning under these conditions is more difficult for two reasons. First, tracking and predicting the trajectories of moving objects (i. e. , cars, humans) is very noisy. Second, the planning process is computationally more expensive because of the increased dimensionality of the state-space, with time as an additional variable. Moreover, re-planning needs to be invoked more often since the trajectories of moving obstacles need to be constantly re-estimated. In this paper, we develop a path planning algorithm that addresses these challenges. First, we choose a representation of dynamic obstacles that efficiently models their predicted trajectories and the uncertainty associated with the predictions. Second, to provide real-time guarantees on the performance of planning with dynamic obstacles, we propose to utilize a novel data structure for planning - a time-bounded lattice - that merges together short-term planning in time with longterm planning without time. We demonstrate the effectiveness of the approach in both simulations with up to 30 dynamic obstacles and on real robots.

AIJ Journal 2008 Journal Article

Anytime search in dynamic graphs

• Maxim Likhachev
• Dave Ferguson
• Geoff Gordon
• Anthony Stentz
• Sebastian Thrun

Agents operating in the real world often have limited time available for planning their next actions. Producing optimal plans is infeasible in these scenarios. Instead, agents must be satisfied with the best plans they can generate within the time available. One class of planners well-suited to this task are anytime planners, which quickly find an initial, highly suboptimal plan, and then improve this plan until time runs out. A second challenge associated with planning in the real world is that models are usually imperfect and environments are often dynamic. Thus, agents need to update their models and consequently plans over time. Incremental planners, which make use of the results of previous planning efforts to generate a new plan, can substantially speed up each planning episode in such cases. In this paper, we present an A∗-based anytime search algorithm that produces significantly better solutions than current approaches, while also providing suboptimality bounds on the quality of the solution at any point in time. We also present an extension of this algorithm that is both anytime and incremental. This extension improves its current solution while deliberation time allows and is able to incrementally repair its solution when changes to the world model occur. We provide a number of theoretical and experimental results and demonstrate the effectiveness of the approaches in a robot navigation domain involving two physical systems. We believe that the simplicity, theoretical properties, and generality of the presented methods make them well suited to a range of search problems involving dynamic graphs.

ICRA Conference 2008 Conference Paper

Information value-driven approach to path clearance with multiple scout robots

• Maxim Likhachev
• Anthony Stentz

In the path clearance problem the robot needs to reach its goal as quickly as possible without being detected by enemies. The robot does not know the precise locations of enemies, but has a list of their possible locations. These locations can be sensed, and the robot can go through them if no enemy is present or has to take a detour otherwise. We have previously developed an approach to the path clearance problem when the robot itself had to sense possible enemy locations. In this paper we investigate the problem of path clearance when the robot can use multiple scout robots to sense the possible enemy locations. This becomes a high-dimensional planning under uncertainty problem. We propose an efficient and scalable approach to it. While the approach requires centralized planning, it can scale to very large environments and to a large number of scouts and allows the scouts to be heterogenous. The experimental results show the benefits of using our approach when multiple scout robots are available.

IROS Conference 2008 Conference Paper

Motion planning in urban environments: Part I

• Dave Ferguson 0001
• Thomas M. Howard
• Maxim Likhachev

We present the motion planning framework for an autonomous vehicle navigating through urban environments. Such environments present a number of motion planning challenges, including ultra-reliability, high-speed operation, complex inter-vehicle interaction, parking in large unstructured lots, and constrained maneuvers. Our approach combines a model-predictive trajectory generation algorithm for computing dynamically-feasible actions with two higher-level planners for generating long range plans in both on-road and unstructured areas of the environment. In this Part I of a two-part paper, we describe the underlying trajectory generator and the on-road planning component of this system. We provide examples and results from ldquoBossrdquo, an autonomous SUV that has driven itself over 3000 kilometers and competed in, and won, the Urban Challenge.

IROS Conference 2008 Conference Paper

Motion planning in urban environments: Part II

• Dave Ferguson 0001
• Thomas M. Howard
• Maxim Likhachev

We present the motion planning framework for an autonomous vehicle navigating through urban environments. Such environments present a number of motion planning challenges, including ultra-reliability, high-speed operation, complex inter-vehicle interaction, parking in large unstructured lots, and constrained maneuvers. Our approach combines a model-predictive trajectory generation algorithm for computing dynamically-feasible actions with two higher-level planners for generating long range plans in both on-road and unstructured areas of the environment. In this Part II of a two-part paper, we describe the unstructured planning component of this system used for navigating through parking lots and recovering from anomalous on-road scenarios. We provide examples and results from ldquoBossrdquo, an autonomous SUV that has driven itself over 3000 kilometers and competed in, and won, the Urban Challenge.

AAAI Conference 2008 Conference Paper

R* Search

• Maxim Likhachev

Optimal heuristic searches such as A* search are widely used for planning but can rarely scale to large complex problems. The suboptimal versions of heuristic searches such as weighted A* search can often scale to much larger planning problems by trading off the quality of the solution for efficiency. They do so by relying more on the ability of the heuristic function to guide them well towards the goal. For complex planning problems, however, the heuristic function may often guide the search into a large local minimum and make the search examine most of the states in the minimum before proceeding. In this paper, we propose a novel heuristic search, called R* search, which depends much less on the quality of the heuristic function. The search avoids local minima by solving the whole planning problem with a series of short-range and easy-to-solve searches, each guided by the heuristic function towards a randomly chosen goal. In addition, R* scales much better in terms of memory because it can discard a search state-space after each of its searches. On the theoretical side, we derive probabilistic guarantees on the sub-optimality of the solution returned by R*. On the experimental side, we show that R* can scale to large complex problems.

IROS Conference 2007 Conference Paper

Goal directed navigation with uncertainty in adversary locations

• Maxim Likhachev
• Anthony Stentz

This paper addresses the problem of planning for goal directed navigation in the environment that contains a number of possible adversary locations. It first shows that commonly used approaches such as assumptive planning can result in very long and costly robot traverses. It then shows how one can solve the same problem using a general probabilistic planner we have recently developed called PPCP (Probabilistic Planning with Clear Preferences). The paper also introduces two optimizations to the PPCP algorithm that make it run up to five times faster for our domain. The experimental results show that solving the problem with PPCP can substantially reduce the expected execution cost as compared to assumptive planning.

AAMAS Conference 2007 Conference Paper

Speeding up Moving-Target Search

• Sven Koenig
• Maxim Likhachev
• Xiaoxun Sun

In this paper, we study moving-target search, where an agent (=hunter) has to catch a moving target (=prey). The agent does not necessarily know the terrain initially but can observe it within a certain sensor range around itself. It uses the strategy to always move on a shortest presumed unblocked path toward the target, which is a reasonable strategy for computer-controlled characters in video games. We study how the agent can find such paths faster by exploiting the fact that it performs A * searches repeatedly. To this end, we extend Adaptive A *, an incremental heuristic search method, to moving-target search and demonstrate experimentally that the resulting MT-Adaptive A * is faster than isolated A * searches and, in many situations, also D * Lite, a state-of-the-art incremental heuristic search method. In particular, it is faster than D * Lite by about one order of magnitude for moving-target search in known and initially unknown mazes if both search methods use the same informed heuristics.

ICAPS Conference 2006 Conference Paper

A New Principle for Incremental Heuristic Search: Theoretical Results

• Sven Koenig
• Maxim Likhachev

Planning is often not a one-shot task because either the world or the agent's knowledge of the world changes. In this paper, we introduce a new principle that can be used to solve a series of similar search tasks faster with heuristic search methods than running individual searches in isolation, by updating the heuristics over time to make them more informed and thus future searches more focused. This principle is simple and easy to integrate into heuristic search methods, and it is easy to prove the correctness of the resulting heuristic search methods.

AAAI Conference 2006 Conference Paper

PPCP: Efficient Probabilistic Planning with Clear Preferences in Partially-Known Environments

• Maxim Likhachev

For most real-world problems the agent operates in only partially-known environments. Probabilistic planners can reason over the missing information and produce plans that take into account the uncertainty about the environment. Unfortunately though, they can rarely scale up to the problems that are of interest in real-world. In this paper, however, we show that for a certain subset of problems we can develop a very efficient probabilistic planner. The proposed planner, called PPCP, is applicable to the problems for which it is clear what values of the missing information would result in the best plan. In other words, there exists a clear preference for the actual values of the missing information. For example, in the problem of robot navigation in partially-known environments it is always preferred to find out that an initially unknown location is traversable rather than not. The planner we propose exploits this property by using a series of deterministic A*-like searches to construct and refine a policy in anytime fashion. On the theoretical side, we show that once converged, the policy is guaranteed to be optimal under certain conditions. On the experimental side, we show the power of PPCP on the problem of robot navigation in partially-known terrains. The planner can scale up to very large environments with thousands of initially unknown locations. We believe that this is several orders of magnitude more unknowns than what the current probabilistic planners developed for the same problem can handle. Also, despite the fact that the problem we experimented on in general does not satisfy the conditions for the solution optimality, PPCP still produces the solutions that are nearly always optimal.

AAMAS Conference 2006 Conference Paper

Real-Time Adaptive A*

• Sven Koenig
• Maxim Likhachev

ICAPS Conference 2005 Conference Paper

A Generalized Framework for Lifelong Planning A* Search

• Maxim Likhachev
• Sven Koenig

We present a graph-based planning and replanning algorithm able to produce bounded suboptimal solutions in an anytime fashion. Our algorithm tunes the quality of its solution based on available search time, at every step reusing previous search efforts. When updated information regarding the underlying graph is received, the algorithm incrementally repairs its previous solution. The result is an approach that combines the benefits of anytime and incremental planners to provide efficient solutions to complex, dynamic search problems. We present theoretical analysis of the algorithm, experimental results on a simulated robot kinematic arm, and two current applications in dynamic path planning for outdoor mobile robots.

AAMAS Conference 2005 Conference Paper

Adaptive A*

• Sven Koenig
• Maxim Likhachev

ICAPS Conference 2005 Conference Paper

Anytime Dynamic A*: An Anytime, Replanning Algorithm

• Maxim Likhachev
• Dave Ferguson 0001
• Geoffrey J. Gordon
• Anthony Stentz
• Sebastian Thrun

We present a graph-based planning and replanning algorithm able to produce bounded suboptimal solutions in an anytime fashion. Our algorithm tunes the quality of its solution based on available search time, at every step reusing previous search efforts. When updated information regarding the underlying graph is received, the algorithm incrementally repairs its previous solution. The result is an approach that combines the benefits of anytime and incremental planners to provide efficient solutions to complex, dynamic search problems. We present theoretical analysis of the algorithm, experimental results on a simulated robot kinematic arm, and two current applications in dynamic path planning for outdoor mobile robots.

ICML Conference 2005 Conference Paper

Bounded real-time dynamic programming: RTDP with monotone upper bounds and performance guarantees

• H. Brendan McMahan
• Maxim Likhachev
• Geoffrey J. Gordon

AIJ Journal 2004 Journal Article

Lifelong Planning A∗

• Sven Koenig
• Maxim Likhachev
• David Furcy

Heuristic search methods promise to find shortest paths for path-planning problems faster than uninformed search methods. Incremental search methods, on the other hand, promise to find shortest paths for series of similar path-planning problems faster than is possible by solving each path-planning problem from scratch. In this article, we develop Lifelong Planning A∗ (LPA∗), an incremental version of A∗ that combines ideas from the artificial intelligence and the algorithms literature. It repeatedly finds shortest paths from a given start vertex to a given goal vertex while the edge costs of a graph change or vertices are added or deleted. Its first search is the same as that of a version of A∗ that breaks ties in favor of vertices with smaller g-values but many of the subsequent searches are potentially faster because it reuses those parts of the previous search tree that are identical to the new one. We present analytical results that demonstrate its similarity to A∗ and experimental results that demonstrate its potential advantage in two different domains if the path-planning problems change only slightly and the changes are close to the goal.

NeurIPS Conference 2004 Conference Paper

Planning for Markov Decision Processes with Sparse Stochasticity

• Maxim Likhachev
• Sebastian Thrun
• Geoffrey Gordon

Planning algorithms designed for deterministic worlds, such as A* search, usually run much faster than algorithms designed for worlds with uncertain action outcomes, such as value iteration. Real-world planning problems often exhibit uncertainty, which forces us to use the slower algorithms to solve them. Many real-world planning problems exhibit sparse uncertainty: there are long sequences of deterministic actions which accomplish tasks like moving sensor platforms into place, inter- spersed with a small number of sensing actions which have uncertain out- comes. In this paper we describe a new planning algorithm, called MCP (short for MDP Compression Planning), which combines A* search with value iteration for solving Stochastic Shortest Path problem in MDPs with sparse stochasticity. We present experiments which show that MCP can run substantially faster than competing planners in domains with sparse uncertainty; these experiments are based on a simulation of a ground robot cooperating with a helicopter to fill in a partial map and move to a goal location. In deterministic planning problems, optimal paths are acyclic: no state is visited more than once. Because of this property, algorithms like A* search can guarantee that they visit each state in the state space no more than once. By visiting the states in an appropriate order, it is possible to ensure that we know the exact value of all of a state's possible successors before we visit that state; so, the first time we visit a state we can compute its correct value. By contrast, if actions have uncertain outcomes, optimal paths may contain cycles: some states will be visited two or more times with positive probability. Because of these cycles, there is no way to order states so that we determine the values of a state's successors before we visit the state itself. Instead, the only way to compute state values is to solve a set of simultaneous equations. In problems with sparse stochasticity, only a small fraction of all states have uncertain outcomes. It is these few states that cause all of the cycles: while a deterministic state s may participate in a cycle, the only way it can do so is if one of its successors has an action with a stochastic outcome (and only if this stochastic action can lead to a predecessor of s). In such problems, we would like to build a smaller MDP which contains only states which are related to stochastic actions. We will call such an MDP a compressed MDP, and we will call its states distinguished states. We could then run fast algorithms like A* search to plan paths between distinguished states, and reserve slower algorithms like value iteration for deciding how to deal with stochastic outcomes. (a) Segbot (b) Robotic helicopter (d) Planning map (e) Execution simulation (c) 3D Map Figure 1: Robot-Helicopter Coordination There are two problems with such a strategy. First, there can be a large number of states which are related to stochastic actions, and so it may be impractical to enumerate all of them and make them all distinguished states; we would prefer instead to distinguish only states which are likely to be encountered while executing some policy which we are considering. Second, there can be a large number of ways to get from one distinguished state to another: edges in the compressed MDP correspond to sequences of actions in the original MDP. If we knew the values of all of the distinguished states exactly, then we could use A* search to generate optimal paths between them, but since we do not we cannot. In this paper, we will describe an algorithm which incrementally builds a compressed MDP using a sequence of deterministic searches. It adds states and edges to the compressed MDP only by encountering them along trajectories; so, it never adds irrelevant states or edges to the compressed MDP. Trajectories are generated by deterministic search, and so undistinguished states are treated only with fast algorithms. Bellman errors in the values for distinguished states show us where to try additional trajectories, and help us build the relevant parts of the compressed MDP as quickly as possible. 1 Robot-Helicopter Coordination Problem The motivation for our research was the problem of coordinating a ground robot and a helicopter. The ground robot needs to plan a path from its current location to a goal, but has only partial knowledge of the surrounding terrain. The helicopter can aid the ground robot by flying to and sensing places in the map. Figure 1(a) shows our ground robot, a converted Segway with a SICK laser rangefinder. Figure 1(b) shows the helicopter, also with a SICK. Figure 1(c) shows a 3D map of the environment in which the robot operates. The 3D map is post-processed to produce a discretized 2D environment (Figure 1(d)). Several places in the map are unknown, either because the robot has not visited them or because their status may have changed (e. g, a car may occupy a driveway). Such places are shown in Figure 1(d) as white squares. The elevation of each white square is proportional to the probability that there is an obstacle there; we assume independence between unknown squares. The robot must take the unknown locations into account when planning for its route. It may plan a path through these locations, but it risks having to turn back if its way is blocked. Alternately, the robot can ask the helicopter to fly to any of these places and sense them. We assign a cost to running the robot, and a somewhat higher cost to running the helicopter. The planning task is to minimize the expected overall cost of running the robot and the helicopter while getting the robot to its destination and the helicopter back to its home base. Figure 1(e) shows a snapshot of the robot and helicopter executing a policy. Designing a good policy for the robot and helicopter is a POMDP planning problem; unfortunately POMDPs are in general difficult to solve (PSPACE-complete [7]). In the POMDP representation, a state is the position of the robot, the current location of the helicopter (a point on a line segment from one of the unknown places to another unknown place or the home base), and the true status of each unknown location. The positions of the robot and the helicopter are observable, so that the only hidden variables are whether each unknown place is occupied. The number of states is (# of robot locations)(# of helicopter locations)2# of unknown places. So, the number of states is exponential in the number of unknown places and therefore quickly becomes very large. We approach the problem by planning in the belief state space, that is, the space of probability distributions over world states. This problem is a continuous-state MDP; in this belief MDP, our state consists of the ground robot's location, the helicopter's location, and a probability of occupancy for each unknown location. We will discretize the continuous probability variables by breaking the interval [0, 1] into several chunks; so, the number of belief states is exponential in the number of unknown places, and classical algorithms such as value iteration are infeasible even on small problems. If sensors are perfect, this domain is acyclic: after we sense a square we know its true state forever after. On the other hand, imperfect sensors can lead to cycles: new sensor data can contradict older sensor data and lead to increased uncertainty. With or without sensor noise, our belief state MDP differs from general MDPs because its stochastic transitions are sparse: large portions of the policy (while the robot and helicopter are traveling be- tween unknown locations) are deterministic. The algorithm we propose in this paper takes advantage of this property of the problem, as we explain in the next section. 2 The Algorithm Our algorithm can be broken into two levels. At a high level, it constructs a compressed MDP, denoted M c, which contains only the start, the goal, and some states which are out- comes of stochastic actions. At a lower level, it repeatedly runs deterministic searches to find new information to put into M c. This information includes newly-discovered stochas- tic actions and their outcomes; better deterministic paths from one place to another; and more accurate value estimates similar to Bellman backups. The deterministic searches can use an admissible heuristic h to focus their effort, so we can often avoid putting many irrelevant actions into M c. Because M c will often be much smaller than M, we can afford to run stochastic plan- ning algorithms like value iteration on it. On the other hand, the information we get by planning in M c will improve the heuristic values that we use in our deterministic searches; so, the deterministic searches will tend to visit only relevant portions of the state space. 2. 1 Constructing and Solving a Compressed MDP Each action in the compressed MDP represents several consecutive actions in M: if we see a sequence of states and actions s1, a1, s2, a2, .. ., sk, ak where a1 through ak-1 are deterministic but ak is stochastic, then we can represent it in M c with a single action a, available at s1, whose outcome distribution is P (s | sk, ak) and whose cost is k-1 c(s1, a, s ) = c(si, ai, si+1) + c(sk, ak, s ) i=1 (See Figure 2(a) for an example of such a compressed action. ) In addition, if we see a se- quence of deterministic actions ending in sgoal, say s1, a1, s2, a2, .. ., sk, ak, sk+1 = sgoal, we can define a compressed action which goes from s1 to sgoal at cost k c(s i=1 i, ai, si+1). We can label each compressed action that starts at s with (s, s, a) (where a = null if s = sgoal). Among all compressed actions starting at s and ending at (s, a) there is (at least) one with lowest expected cost; we will call such an action an optimal compression of (s, s, a). Write Astoch for the set of all pairs (s, a) such that action a when taken from state s has more than one possible outcome, and include as well (sgoal, null). Write Sstoch for the states which are possible outcomes of the actions in Astoch, and include sstart as well. If we include in our compressed MDP an optimal compression of (s, s, a) for every s Sstoch and every (s, a) Astoch, the result is what we call the full compressed MDP; an example is in Figure 2(b). If we solve the full compressed MDP, the value of each state will be the same as the value of the corresponding state in M. However, we do not need to do that much work: (a) action compression (b) full MDP compression (c) incremental MDP compression Figure 2: MDP compression Main() 01 initialize M c with sstart and sgoal and set their v-values to 0; 02 while (s M c s. t. RHS(s) - v(s) > and s belongs to the current greedy policy) 03 select spivot to be any such state s; 04 [v; vlim] = Search(spivot); 05 v(spivot) = v; 06 set the cost c(spivot, a, sgoal) of the limit action a from spivot to vlim; 07 optionally run some algorithm satisfying req. A for a bounded amount of time to improve the value function in M c; Figure 3: MCP main loop many states and actions in the full compressed MDP are irrelevant since they do not appear in the optimal policy from sstart to sgoal. So, the goal of the MCP algorithm will be to construct only the relevant part of the compressed MDP by building M c incrementally. Figure 2(c) shows the incremental construction of a compressed MDP which contains all of the stochastic states and actions along an optimal policy in M. The pseudocode for MCP is given in Figure 3. It begins by initializing M c to contain only sstart and sgoal, and it sets v(sstart) = v(sgoal) = 0. It maintains the invariant that 0 v(s) v(s) for all s. On each iteration, MCP looks at the Bellman error of each of the states in M c. The Bellman error is v(s) - RHS(s), where RHS(s) = min RHS(s, a) RHS(s, a) = Es succ(s, a)(c(s, a, s ) + v(s )) aA(s) By convention the min of an empty set is, so an s which does not have any compressed actions yet is considered to have infinite RHS. MCP selects a state with negative Bellman error, spivot, and starts a search at that state. (We note that there exist many possible ways to select spivot; for example, we can choose the state with the largest negative Bellman error, or the largest error when weighted by state visitation probabilities in the best policy in M c. ) The goal of this search is to find a new compressed action a such that its RHS-value can provide a new lower bound on v(spivot). This action can either decrease the current RHS(spivot) (if a seems to be a better action in terms of the current v-values of action outcomes) or prove that the current RHS(spivot) is valid. Since v(s ) v(s ), one way to guarantee that RHS(spivot, a) v(spivot) is to compute an optimal compression of (spivot, s, a) for all s, a, then choose the one with the smallest RHS. A more sophisticated strategy is to use an A search with appropriate safeguards to make sure we never overestimate the value of a stochastic action. MCP, however, uses a modified A search which we will describe in the next section. As the search finds new compressed actions, it adds them and their outcomes to M c. It is allowed to initialize newly-added states to any admissible values. When the search returns, MCP sets v(spivot) to the returned value. This value is at least as large as RHS(spivot). Consequently, Bellman error for spivot becomes non-negative. In addition to the compressed action and the updated value, the search algorithm returns a "limit value" vlim(spivot). These limit values allow MCP to run a standard MDP planning algorithm on M c to improve its v(s) estimates. MCP can use any planning algorithm which guarantees that, for any s, it will not lower v(s) and will not increase v(s) beyond the smaller of vlim(s) and RHS(s) (Requirement A). For example, we could insert a fake "limit action" into M c which goes directly from spivot to sgoal at cost vlim(spivot) (as we do on line 06 in Figure 3), then run value iteration for a fixed amount of time, selecting for each backup a state with negative Bellman error. After updating M c from the result of the search and any optional planning, MCP begins again by looking for another state with a negative Bellman error. It repeats this process until there are no negative Bellman errors larger than. For small enough, this property guarantees that we will be able to find a good policy (see section 2. 3). 2. 2 Searching the MDP Efficiently The top level algorithm (Figure 3) repeatedly invokes a search method for finding trajec- tories from spivot to sgoal. In order for the overall algorithm to work correctly, there are several properties that the search must satisfy. First, the estimate v that search returns for the expected cost of spivot should always be admissible. That is, 0 v v(spivot) (Property 1). Second, the estimate v should be no less than the one-step lookahead value of spivot in M c. That is, v RHS(spivot) (Property 2). This property ensures that search either increases the value of spivot or finds additional (or improved) compressed actions. The third and final property is for the vlim value, and it is only important if MCP uses its optional planning step (line 07). The property is that v vlim v(spivot) (Property 3). Here v(spivot) denotes the minimum expected cost of starting at spivot, picking a com- pressed action not in M c, and acting optimally from then on. (Note that v can be larger than v if the optimal compressed action is already part of M c. ) Property 3 uses v rather than v since the latter is not known while it is possible to compute a lower bound on the former efficiently (see below). One could adapt A* search to satisfy at least Properties 1 and 2 by assuming that we can control the outcome of stochastic actions. However, this sort of search is highly optimistic and can bias the search towards improbable trajectories. Also, it can only use heuristics which are even more optimistic than it is: that is, h must be admissible with respect to the optimistic assumption of controlled outcomes. We therefore present a version of A*, called MCP-search (Figure 4), that is more effi- cient for our purposes. MCP-search finds the correct expected value for the first stochas- tic action it encounters on any given trajectory, and is therefore far less optimistic. And, MCP-search only requires heuristic values to be admissible with respect to v values, h(s) v(s). Finally, MCP-search speeds up repetitive searches by improving heuris- tic values based on previous searches. A* maintains a priority queue, OPEN, of states which it plans to expand. The OPEN queue is sorted by f (s) = g(s)+h(s), so that A* always expands next a state which appears to be on the shortest path from start to goal. During each expansion a state s is removed from OPEN and all the g-values of s's successors are updated; if g(s ) is decreased for some state s, A* inserts s into OPEN. A* terminates as soon as the goal state is expanded. We use the variant of A* with pathmax [5] to use efficiently heuristics that do not satisfy the triangle inequality. MCP is similar to A, but the OPEN list can also contain state-action pairs {s, a} where a is a stochastic action (line 31). Plain states are represented in OPEN as {s, null}. Just ImproveHeuristic(s) 01 if s M c then h(s) = max(h(s), v(s)); 02 improve heuristic h(s) further if possible using f best and g(s) from previous iterations; procedure fvalue({s, a}) 03 if s = null return; 04 else if a = null return g(s) + h(s); 05 else return g(s) + max(h(s), E {c(s, a, s ) + h(s )}) s Succ(s, a); CheckInitialize(s) 06 if s was accessed last in some previous search iteration 07 ImproveHeuristic(s); 08 if s was not yet initialized in the current search iteration 09 g(s) =; InsertUpdateCompAction(spivot, s, a) 10 reconstruct the path from spivot to s; 11 insert compressed action (spivot, s, a) into A(spivot) (or update the cost if a cheaper path was found) 12 for each outcome u of a that was not in M c previously 13 set v(u) to h(u) or any other value less than or equal to v(u); 14 set the cost c(u, a, sgoal) of the limit action a from u to v(u); procedure Search(spivot) 15 CheckInitialize(sgoal), CheckInitialize(spivot); 16 g(spivot) = 0; 17 OPEN = {{spivot, null}}; 18 {sbest, abest} = {null, null}, f best =; 19 while(g(sgoal) > min{s, a}OPEN(fvalue({s, a})) AND f best + > min{s, a}OPEN(fvalue({s, a}))) 20 remove {s, a} with the smallest fvalue({s, a}) from OPEN breaking ties towards the pairs with a = null; 21 if a = null //expand state s 22 for each s Succ(s) 23 CheckInitialize(s ); 24 for each deterministic a A(s) 25 s = Succ(s, a ); 26 h(s ) = max(h(s ), h(s) - c(s, a, s )); 27 if g(s ) > g(s) + c(s, a, s ) 28 g(s ) = g(s) + c(s, a, s ); 29 insert/update {s, null} into OPEN with fvalue({s, null}); 30 for each stochastic a A(s) 31 insert/update {s, a } into OPEN with fvalue({s, a }); 32 else //encode stochastic action a from state s as a compressed action from spivot 33 InsertUpdateCompAction(spivot, s, a); 34 if f best > fvalue({s, a}) then {sbest, abest} = {s, a}, f best = fvalue({s, a}); 35 if (g(sgoal) min{s, a}OPEN(fvalue({s, a})) AND OPEN = ) 36 reconstruct the path from spivot to sgoal; 37 update/insert into A(spivot) a deterministic action a leading to sgoal; 38 if f best g(sgoal) then {sbest, abest} = {sgoal, null}, f best = g(sgoal); 39 return [f best; min{s, a}OPEN(fvalue({s, a}))]; Figure 4: MCP-search Algorithm like A*, MCP-search expands elements in the order of increasing f -values, but it breaks ties towards elements encoding plain states (line 20). The f -value of {s, a} is defined as g(s) + max[h(s), Es Succ(s, a)(c(s, a, s ) + h(s ))] (line 05). This f -value is a lower bound on the cost of a policy that goes from sstart to sgoal by first executing a series of deterministic actions until action a is executed from state s. This bound is as tight as possible given our heuristic values. State expansion (lines 21-31) is very similar to A. When the search removes from OPEN a state-action pair {s, a} with a = null, it adds a compressed action to M c (line 33). It also adds a compressed action if there is an optimal deterministic path to sgoal (line 37). f best tracks the minimum f -value of all the compressed actions found. As a result, f best v(spivot) and is used as a new estimate for v(spivot). The limit value vlim(spivot) is obtained by continuing the search until the minimum f -value of elements in OPEN approaches f best + for some 0 (line 19). This minimum f -value then provides a lower bound on v(spivot). To speed up repetitive searches, MCP-search improves the heuristic of every state that it encounters for the first time in the current search iteration (lines 01 and 02). Line 01 uses the fact that v(s) from M c is a lower bound on v(s). Line 02 uses the fact that f best - g(s) is a lower bound on v(s) at the end of each previous call to Search; for more details see [4]. 2. 3 Theoretical Properties of the Algorithm We now present several theorems about our algorithm. The proofs of these and other theo- rems can be found in [4]. The first theorem states the main properties of MCP-search. Theorem 1 The search function terminates and the following holds for the values it re- turns: (a) if sbest = null then v(spivot) f best E{c(spivot, abest, s ) + v(s )} (b) if sbest = null then v(spivot) = f best = (c) f best min{s, a}OPEN(fvalue({s, a})) v(spivot). If neither sgoal nor any state-action pairs were expanded, then sbest = null and (b) says that there is no policy from spivot that has a finite expected cost. Using the above theorem it is easy to show that MCP-search satisfies Properties 1, 2 and 3, considering that f best is returned as variable v and min{s, a}OPEN(fvalue({s, a})) is returned as variable vlim in the main loop of the MCP algorithm (Figure 3). Property 1 follows directly from (a) and (b) and the fact that costs are strictly positive and v-values are non-negative. Property 2 also follows trivially from (a) and (b). Property 3 follows from (c). Given these properties the next theorem states the correctness of the outer MCP algorithm (in the theorem cgreedy denotes a greedy policy that always chooses an action that looks best based on its cost and the v-values of its immediate successors). Theorem 2 Given a deterministic search algorithm which satisfies Properties 13, the MCP algorithm will terminate. Upon termination, for every state s M c c we greedy have RHS(s) - v(s) v(s). Given the above theorem one can show that for 0 < cmin (where cmin is the smallest expected action cost in our MDP) the expected cost of executing c from greedy sstart is at most cmin v(s c start). Picking cmin is not guaranteed to result in a proper min - policy, even though Theorem 2 continues to hold. 3 Experimental Study We have evaluated the MCP algorithm on the robot-helicopter coordination problem de- scribed in section 1. To obtain an admissible heuristic, we first compute a value function for every possible configuration of obstacles. Then we weight the value functions by the probabilities of their obstacle configurations, sum them, and add the cost of moving the helicopter back to its base if it is not already there. This procedure results in optimistic cost estimates because it pretends that the robot will find out the obstacle locations immediately instead of having to wait to observe them. The results of our experiments are shown in Figure 5. We have compared MCP against three algorithms: RTDP [1], LAO* [2] and value iteration on reachable states (VI). RTDP can cope with large size MDPs by focussing its planning efforts along simulated execu- tion trajectories. LAO* uses heuristics to prune away irrelevant states, then repeatedly performs dynamic programming on the states in its current partial policy. We have im- plemented LAO* so that it reduces to AO* [6] when environments are acyclic (e. g. , the robot-helicopter problem with perfect sensing). VI was only able to run on the problems with perfect sensing since the number of reachable states was too large for the others. The results support the claim that MCP can solve large problems with sparse stochas- ticity. For the problem with perfect sensing, on average MCP was able to plan 9. 5 times faster than LAO*, 7. 5 times faster than RTDP, and 8. 5 times faster than VI. On average for these problems, MCP computed values for 58633 states while M c grew to 396 states, and MCP encountered 3740 stochastic transitions (to give a sense of the degree of stochastic- ity). The main cost of MCP was in its deterministic search subroutine; this fact suggests that we might benefit from anytime search techniques such as ARA* [3]. The results for the problems with imperfect sensing show that, as the number and den- sity of uncertain outcomes increases, the advantage of MCP decreases. For these problems MCP was able to solve environments 10. 2 times faster than LAO* but only 2. 2 times faster than RTDP. On average MCP computed values for 127, 442 states, while the size of M c was 3, 713 states, and 24, 052 stochastic transitions were encountered. Figure 5: Experimental results. The top row: the robot-helicopter coordination problem with perfect sensors. The bottom row: the robot-helicopter coordination problem with sensor noise. Left column: running times (in secs) for each algorithm grouped by environments. Middle column: the number of backups for each algorithm grouped by environments. Right column: an estimate of the expected cost of an optimal policy (v(sstart)) vs. running time (in secs) for experiment (k) in the top row and experiment (e) in the bottom row. Algorithms in the bar plots (left to right): MCP, LAO*, RTDP and VI (VI is only shown for problems with perfect sensing). The characteristics of the environments are given in the second and third rows under each of the bar plot. The second row indicates how many cells the 2D plane is discretized into, and the third row indicates the number of initially unknown cells in the environment. 4 Discussion The MCP algorithm incrementally builds a compressed MDP using a sequence of deter- ministic searches. Our experimental results suggest that MCP is advantageous for problems with sparse stochasticity. In particular, MCP has allowed us to scale to larger environments than were otherwise possible for the robot-helicopter coordination problem. Acknowledgements This research was supported by DARPA's MARS program. All conclusions are our own.

NeurIPS Conference 2003 Conference Paper

ARA*: Anytime A* with Provable Bounds on Sub-Optimality

• Maxim Likhachev
• Geoffrey Gordon
• Sebastian Thrun

In real world planning problems, time for deliberation is often limited. Anytime planners are well suited for these problems: they find a feasi- ble solution quickly and then continually work on improving it until time runs out. In this paper we propose an anytime heuristic search, ARA*, which tunes its performance bound based on available search time. It starts by finding a suboptimal solution quickly using a loose bound, then tightens the bound progressively as time allows. Given enough time it finds a provably optimal solution. While improving its bound, ARA* reuses previous search efforts and, as a result, is significantly more effi- cient than other anytime search methods. In addition to our theoretical analysis, we demonstrate the practical utility of ARA* with experiments on a simulated robot kinematic arm and a dynamic path planning prob- lem for an outdoor rover.

ICRA Conference 2002 Conference Paper

Improved Fast Replanning for Robot Navigation in Unknown Terrain

• Sven Koenig
• Maxim Likhachev

Mobile robots often operate in domains that are only incompletely known, for example, when they have to move from given start coordinates to given goal coordinates in unknown terrain. In this case, they need to be able to replan quickly as their knowledge of the terrain changes. Stentz' Focussed Dynamic A* is a heuristic search method that repeatedly determines a shortest path from the current robot coordinates to the goal coordinates while the robot moves along the path. It is able to replan one to two orders of magnitudes faster than planning from scratch since it modifies previous search results locally. Consequently, it has been extensively used in mobile robotics. In this paper, we introduce an alternative to Focussed Dynamic A* that implements the same navigation strategy but is algorithmically different. Focussed Dynamic A* Lite is simpler, easier to understand, easier to analyze and easier to extend than Focussed Dynamic A*, yet is more efficient. We believe that our results will make D*-like replanning algorithms even more popular and enable robotics researchers to adapt them to additional applications.

IROS Conference 2002 Conference Paper

Incremental replanning for mapping

• Maxim Likhachev
• Sven Koenig

Incremental heuristic search methods can often replan paths much faster than incremental or heuristic search methods individually, yet are simple to use. So far, they have only been used in mobile robotics to move a robot to given goal coordinates in unknown terrain. As far as we know, incremental heuristic search methods have not yet been applied to the problem of mapping unknown terrain. In this paper we therefore describe how to apply our incremental heuristic search method D* Lite, that combines ideas from Lifelong Planning A* and Focussed D*, to mapping unknown terrain, which is rather nontrivial. We then compare its runtime against that of incremental search and heuristic search alone, demonstrating the computational benefits of their combination. By demonstrating the versatility and computational benefits of incremental heuristic search, we hope that this underexploited technique will be used more often in mobile robotics.

ICRA Conference 2002 Conference Paper

Learning Behavioral Parameterization using Spatio-Temporal Case-Based Reasoning

• Maxim Likhachev
• Michael Kaess
• Ronald C. Arkin

This paper presents an approach to learning an optimal behavioral parameterization in the framework of a case-based reasoning methodology for autonomous navigation tasks. It is based on our previous work on a behavior-based robotic system that also employed spatio-temporal case-based reasoning in the selection of behavioral parameters but was not capable of learning new parameterizations. The present method extends the case-based reasoning module by making it capable of learning new and optimizing the existing cases where each case is a set of behavioral parameters. The learning process can either be a separate training process or be part of the mission execution. In either case, the robot learns an optimal parameterization of its behavior for different environments it encounters. The goal of this research is not only to automatically optimize the performance of the robot but also to avoid the manual configuration of behavioral parameters and the initial configuration of a case library, both of which require the user to possess good knowledge of robot behavior and the performance of numerous experiments. The presented method was integrated within a hybrid robot architecture and evaluated in extensive computer simulations, showing a significant increase in the performance over a nonadaptive system and a performance comparable to a non-learning CBR system that uses a hand-coded case library.

ICRA Conference 2002 Conference Paper

Selection of Behavioral Parameters: Integration of Discontinuous Switching via Case-Based Reasoning with Continuous Adaptation via Learning Momentum

• J. Brian Lee
• Maxim Likhachev
• Ronald C. Arkin

This paper studies the effects of the integration of two learning algorithms, case-base reasoning (CBR) and learning momentum (LM), for the selection of behavioral parameters in real-time for robotic navigational tasks. Use of CBR methodology in the selection of behavioral parameters has already shown significant improvement in robot performance as measured by mission completion time and success rate. It has also made unnecessary the manual configuration of behavioral parameters from a user. However, the choice of the library of CBR cases does affect the robot performance, and choosing the right library sometimes is a difficult task especially when working with a real robot. In contrast, learning momentum does not depend on any prior information such as cases and searches for the "right" parameters in real-time. This results in high mission success rates and requires no manual configuration of parameters, but it shows no improvement in mission completion time. This work combines the two approaches so that CBR discontinuously switches behavioral parameters based on given cases whereas LM uses these parameters as a starting point for the real-time search for the "right" parameters. The integrated system was extensively evaluated on both simulated and physical robots. The tests showed that on simulated robots the integrated system performed as well as the CBR only system and outperformed the LM only system, whereas on real robots it significantly outperformed both CBR only and LM only systems.

NeurIPS Conference 2002 Conference Paper

Speeding up the Parti-Game Algorithm

• Maxim Likhachev
• Sven Koenig

In this paper, we introduce an efficient replanning algorithm for nonde- terministic domains, namely what we believe to be the first incremental heuristic minimax search algorithm. We apply it to the dynamic dis- cretization of continuous domains, resulting in an efficient implemen- tation of the parti-game reinforcement-learning algorithm for control in high-dimensional domains.

ICRA Conference 2001 Conference Paper

Spatio-Temporal Case-Based Reasoning for Behavioral Selection

• Maxim Likhachev
• Ronald C. Arkin

Presents the application of a case-based reasoning approach to the selection and modification of behavioral assemblage parameters. The goal of this research is to achieve an optimal parameterization of robotic behaviors in run-time. This increases robot performance and makes a manual configuration of parameters unnecessary. The case-based reasoning module selects a set of parameters for an active behavioral assemblage in real-time. This set of parameters fits the environment better than hand-coded ones, and its performance is monitored providing feedback for a possible reselection of the parameters. The paper places a significant emphasis on the technical details of the case-based reasoning module and how it is integrated within a schema-based reactive navigation system. The paper also presents the results and evaluation of the system in both in simulation and real world robotic experiments.