  Robots often need to solve path planning problems where essential and
discrete aspects of the environment are partially observable. This introduces a
multi-modality, where the robot must be able to observe and infer the state of
its environment. To tackle this problem, we introduce the Path-Tree
Optimization (PTO) algorithm which plans a path-tree in belief-space. A
path-tree is a tree-like motion with branching points where the robot receives
an observation leading to a belief-state update. The robot takes different
branches depending on the observation received. The algorithm has three main
steps. First, a rapidly-exploring random graph (RRG) on the state space is
grown. Second, the RRG is expanded to a belief-space graph by querying the
observation model. In a third step, dynamic programming is performed on the
belief-space graph to extract a path-tree. The resulting path-tree combines
exploration with exploitation i.e. it balances the need for gaining knowledge
about the environment with the need for reaching the goal. We demonstrate the
algorithm capabilities on navigation and mobile manipulation tasks, and show
its advantage over a baseline using a task and motion planning approach (TAMP)
both in terms of optimality and runtime.
