Logic-geometric programming: an optimization-based approach to combined task and motion planning

TLDR

Sequential robot manipulation problems are often framed as combined task and motion planning, where the goal is expressed as a cost over the final geometric state rather than a symbolic goal. The authors aim to solve these problems by formulating them as a first‑order logic extension of a nonlinear constrained mathematical program that couples symbolic action sequences with continuous trajectory constraints. They address the resulting optimization challenge with a three‑level approximation hierarchy: an effective end‑state kinematics level, an interaction‑keyframe level, and a full‑trajectory level. The coarsest level yields fast optimization that guides symbolic search, and the method is validated on a task that maximizes the height of a physically stable construction from boards, cylinders, and blocks.

Abstract

We consider problems of sequential robot manipulation (aka. combined task and motion planning) where the objective is primarily given in terms of a cost function over the final geometric state, rather than a symbolic goal description. In this case we should leverage optimization methods to inform search over potential action sequences. We propose to formulate the problem holistically as a 1st- order logic extension of a mathematical program: a non-linear constrained program over the full world trajectory where the symbolic state-action sequence defines the (in-)equality constraints. We tackle the challenge of solving such programs by proposing three levels of approximation: The coarsest level introduces the concept of the effective end state kinematics, parametrically describing all possible end state configurations conditional to a given symbolic action sequence. Optimization on this level is fast and can inform symbolic search. The other two levels optimize over interaction keyframes and eventually over the full world trajectory across interactions. We demonstrate the approach on a problem of maximizing the height of a physically stable construction from an assortment of boards, cylinders and blocks.