Robots often have to operate in discrete partially observable worlds, where the state of the world is only observable at runtime. To react to different world states, robots need contingencies. To find contingencies, prior work developed the path tree optimization (PTO) method, which computes motion contingencies by constructing a tree of motion paths in belief space. In this paper, we extend upon PTO by enabling camera-based belief space planning through an extension of the open motion planning library (OMPL). By leveraging this extension, we develop an improved camera-based state sampler and an efficient open-source implementation of PTO. This version of PTO supports a virtual camera, non-euclidean state spaces, and different state samplers. We evaluate this improved version of PTO on four realistic scenarios with a virtual camera in up to 10-dimensional state spaces. In our evaluations, we compare PTO both with a default and with the new camera-based state sampler. The results indicate that the camera-based state sampler improves success rates in 3 out of 4 scenarios while having a significant lower memory footprint. Our work thus makes an important step in advancing belief-space planning and provides researchers with an open source tool to use, modify, and benchmark belief-space planning methods.

This paper presents a new approach to Model Predictive Control for environments where essential, discrete variables are partially observed. Under this assumption, the belief state is a probability distribution over a finite number of states. We optimize a control-tree where each branch assumes a given state-hypothesis. The control-tree optimization uses the probabilistic belief state information. This leads to policies more optimized with respect to likely states than unlikely ones, while still guaranteeing robust constraint satisfaction at all times. We apply the method to both linear and non-linear MPC with constraints. The optimization of the control-tree is decomposed into optimization subproblems that are solved in parallel leading to good scalability for high number of state-hypotheses. We demonstrate the real-time feasibility of the algorithm on two examples and show the benefits compared to a classical MPC scheme optimizing w.r.t. one single hypothesis.

We propose a novel approach to Combined Task and Motion Planning (TAMP) under partial observability. Previous optimization-based TAMP methods [1][2] compute optimal plans and paths assuming full observability. However, partial observability requires the solution to be a policy that reacts to the observations that the agent receives. We consider a formulation where observations introduce additional branching in the symbolic decision tree. The solution is now given by a reactive policy on the symbolic level together with a path tree that describes the branchings of optimal motion depending on the observations. Our method works in two stages: First, the symbolic policy is optimized using approximate path costs estimated from independent optimizations of trajectory pieces. Second, we fix the best symbolic policy and optimize a joint trajectory tree. We test our approach on object manipulation and autonomous driving examples. We also compare the algorithm's performance to a state-of-the-art TAMP planner in fully observable cases.