We describe a task and motion planning architecture for highly dynamic systems that combines a domain-independent sampling-based deliberative planning algorithm with a global reactive planner. We leverage the recent development of a reactive, vector field planner that provides guarantees of reachability to large regions of the environment even in the face of unknown or unforeseen obstacles. The reachability guarantees can be formalized using contracts that allow a deliberative planner to reason purely in terms of those contracts and synthesize a plan by choosing a sequence of reactive behaviors and their target configurations, without evaluating specific motion plans between targets. This reduces both the search depth at which plans will be found, and the number of samples required to ensure a plan exists, while crucially preserving correctness guarantees. The result is reduced computational cost of synthesizing plans, and increased robustness of generated plans to actuator noise, model misspecification, or unknown obstacles. Simulation studies show that our hierarchical planning and execution architecture can solve complex navigation and rearrangement tasks, even when faced with narrow passageways or incomplete world information.

We present a new representation for task and motion planning that uses constraints to capture both continuous and discrete phenomena in a unified framework. We show that we can decide if a feasible plan exists for a given problem instance using only polynomial space if the constraints are semialgebraic and all actions have uniform stratified accessibility, a technical condition closely related to both controllability and to the existence of a symbolic representation of a planning domain. We show that there cannot exist an algorithm that solves the more general problem of deciding if a plan exists for an instance with arbitrary semialgebraic constraints. Finally, we show that our formalism is universal, in the sense that every deterministic robotic planning problem can be well-approximated within our formalism. Together, these results imply task and motion planning is PSPACE-complete.

Path planning classically focuses on avoiding environmental contact. However, some assembly tasks permit contact through compliance, and such contact may allow for more efficient and reliable solutions under action uncertainty. But, optimal manipulation plans that leverage environmental contact are difficult to compute. Environmental contact produces complex kinematics that create difficulties for planning. This complexity is usually addressed by discretization over state and action space, but discretization quickly becomes computationally intractable. To overcome the challenge, we use the insight that only actions on configurations near the contact manifold are likely to involve complex kinematics, while segments of the plan through free space do not. Leveraging this structure can greatly reduce the number of states considered and scales much better with problem complexity. We develop an algorithm based on this idea and show that it performs comparably to full MDP solutions at a fraction of the computational cost.

We present a novel method of measurement covariance estimation that models measurement uncertainty as a function of the measurement itself. Existing work in predictive sensor modeling outperforms conventional fixed models, but requires domain knowledge of the sensors that heavily influences the accuracy and the computational cost of the models. In this work, we introduce Deep Inference for Covariance Estimation (DICE), which utilizes a deep neural network to predict the covariance of a sensor measurement from raw sensor data. We show that given pairs of raw sensor measurement and ground-truth measurement error, we can learn a representation of the measurement model via supervised regression on the prediction performance of the model, eliminating the need for hand-coded features and parametric forms. Our approach is sensor-agnostic, and we demonstrate improved covariance prediction on both simulated and real data.

This paper considers the problem of completing assemblies of passive objects in nonconvex environments, cluttered with convex obstacles of unknown position, shape and size that satisfy a specific separation assumption. A differential drive robot equipped with a gripper and a LIDAR sensor, capable of perceiving its environment only locally, is used to position the passive objects in a desired configuration. The method combines the virtues of a deliberative planner generating high-level, symbolic commands, with the formal guarantees of convergence and obstacle avoidance of a reactive planner that requires little onboard computation and is used online. The validity of the proposed method is verified both with formal proofs and numerical simulations.

We define an admissibility condition for abstractions expressed using angelic semantics and show that these conditions allow us to accelerate planning while preserving the ability to find the optimal motion plan. We then derive admissible abstractions for two motion planning domains with continuous state. We extract upper and lower bounds on the cost of concrete motion plans using local metric and topological properties of the problem domain. These bounds guide the search for a plan while maintaining performance guarantees. We show that abstraction can dramatically reduce the complexity of search relative to a direct motion planner. Using our abstractions, we find near-optimal motion plans in planning problems involving 10^13 states without using a separate task planner.

We demonstrate the physical rearrangement of wheeled stools in a moderately cluttered indoor environment by a quadrupedal robot that autonomously achieves a user's desired configuration. The robot's behaviors are planned and executed by a three layer hierarchical architecture consisting of: an offline symbolic task and motion planner; a reactive layer that tracks the reference output of the deliberative layer and avoids unanticipated obstacles sensed online; and a gait layer that realizes the abstract unicycle commands from the reactive module through appropriately coordinated joint level torque feedback loops. This work also extends prior formal results about the reactive layer to a broad class of nonconvex obstacles. Our design is verified both by formal proofs as well as empirical demonstration of various assembly tasks.

Many algorithms in computer vision and robotics make strong assumptions about uncertainty, and rely on the validity of these assumptions to produce accurate and consistent state estimates. In practice, dynamic environments may degrade sensor performance in predictable ways that cannot be captured with static uncertainty parameters. In this paper, we employ fast nonparametric Bayesian inference techniques to more accurately model sensor uncertainty. By setting a prior on observation uncertainty, we derive a predictive robust estimator, and show how our model can be learned from sample images, both with and without knowledge of the motion used to generate the data. We validate our approach through Monte Carlo simulations, and report significant improvements in localization accuracy relative to a fixed noise model in several settings, including on synthetic data, the KITTI dataset, and our own experimental platform.

We present CELLO (Covariance Estimation and Learning through Likelihood Optimization), an algorithm for predicting the covariances of measurements based on any available informative features. This algorithm is intended to improve the accuracy and reliability of on-line state estimation by providing a principled way to extend the conventional fixed-covariance Gaussian measurement model. We show that in experiments, CELLO learns to predict measurement covariances that agree with empirical covariances obtained by manually annotating sensor regimes. We also show that using the learned covariances during filtering provides substantial quantitative improvement to the overall state estimate.

We present an algorithm for providing a dynamic model of sensor measurements. Rather than depending on a model of the vehicle state and environment to capture the distribution of possible sensor measurements, we provide an approximation that allows the sensor model to depend on the measurement itself. Building on previous work, we show how the sensor model predictor can be learned from data without access to ground truth labels of the vehicle state or true underlying distribution, and we show our approach to be a generalization of non-parametric kernel regressors. Our algorithm is demonstrated in simulation and on real world data for both laser-based scan matching odometry and RGB-D camera odometry in an unknown map. The performance of our algorithm is shown to quantitatively improve estimation, both in terms of consistency and absolute accuracy, relative to other algorithms and to fixed covariance models.