Stack-Aware Rearrangement of Objects in Confined Environments
We propose a stack-aware task and motion planning (TAMP) framework that rearranges heterogeneous objects within confined and cluttered environments. These settings involve vertical stacking and variations in object size and shape, which increase planning complexity. However, existing TAMP approaches typically assume uniform object geometries or rely on 2D abstractions that ignore detailed 3D stacking dependencies, leading to inefficient plans and frequent failures in tightly packed environments with densely stacked objects. To overcome these challenges, we introduce a rank-based task planner that assigns each potential relocation a lexicographically ordered score based on four factors: object rank, the number of objects stacked above it, buffer space clearance, and the number of blocking objects. The planner then selects the relocation with the lowest combined score, thereby reducing symbolic distance to the goal. During execution, the planner repairs the plan reactively by preserving valid subplans and applying local adjustments when collisions occur. Experimental results from both simulations and real robot experiments demonstrate higher success rates, reduced planning times, and improved object efficiency under tight spatial and stacking constraints.