SyDeBO: Symbolic-Decision-Embedded Bilevel Optimization for Long-Horizon Manipulation in Dynamic Environments

This study proposes a Task and Motion Planning (TAMP) method with symbolic decisions embedded in a bilevel optimization. This TAMP method exploits the discrete structure of sequential manipulation for long-horizon and versatile tasks in dynamically changing environments. At the symbolic planning level, we propose a scalable decision-making method for long-horizon manipulation tasks using the Planning Domain Definition Language (PDDL) with causal graph decomposition. At the motion planning level, we devise a trajectory optimization (TO) approach based on the Alternating Direction Method of Multipliers (ADMM), suitable for solving constrained, large-scale nonlinear optimization in a distributed manner. Distinct from conventional geometric motion planners, our approach generates highly dynamic manipulation motions by incorporating the full robot and object dynamics. Furthermore, in lieu of a hierarchical planning approach, we solve a holistically integrated bilevel optimization problem involving costs from both the low-level TO and the high-level search. Simulation and experimental results demonstrate dynamic manipulation for long-horizon object sorting tasks in clutter and on a moving conveyor belt.