  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.
