SeGMan: Sequential and Guided Manipulation Planner for Robust Planning in 2D Constrained Environments

TL;DR

SeGMan addresses robust planning for sequential manipulation in cluttered 2D environments by weaving together sampling-based planning, optimization-based trajectory optimization, and guided forward search. It introduces an adaptive subgoal selection mechanism and LOG-based heuristics to steer obstacle relocation and subgoal generation, deployed within a two-phase architecture that handles pick and place as well as obstacle removal. The approach decomposes tasks into subproblems, solves subproblems with KOMO and Bi-RRT, and uses a forest of obstacle subsets to identify minimal relocations, achieving strong robustness and efficiency across 15 maze-like scenarios. The results show SeGMan often outperforms state-of-the-art baselines in obstacle-rich settings while maintaining competitive solution quality, indicating practical impact for complex manipulation in constrained environments.

Abstract

In this paper, we present SeGMan, a hybrid motion planning framework that integrates sampling-based and optimization-based techniques with a guided forward search to address complex, constrained sequential manipulation challenges, such as pick-and-place puzzles. SeGMan incorporates an adaptive subgoal selection method that adjusts the granularity of subgoals, enhancing overall efficiency. Furthermore, proposed generalizable heuristics guide the forward search in a more targeted manner. Extensive evaluations in maze-like tasks populated with numerous objects and obstacles demonstrate that SeGMan is capable of generating not only consistent and computationally efficient manipulation plans but also outperform state-of-the-art approaches.

Figures (6)

• Figure 1: The example illustrates a pick-and-place task where the agent (yellow) must move the goal object (blue) to the goal (red). Before doing so, it selectively removes the obstacles (white) to ensure it can access the goal object.
• Figure 2: The forest of $o_{set}$ trees.
• Figure 3: System flowchart of SeGMan's two-phase architecture: (I) hybrid manipulation of goal object to goal position with adaptive subgoal selection, (II) forward search of determining obstacle objects and their removal. If no pick or place plan can be realized for the goal object, the obstacle removal phase clears the path. Once the paths are cleared, the framework continues to pick-place the goal object from where it left off.
• Figure 4: Left: The clustered object sets (arrow direction is not important). Right: Two different clusters (red and blue) with each path are generated for $o_{set} \in C_i$. The more transparent the color of the path is, the less the $o_{set}$ is prioritized .
• Figure 5: Left: The Four Block scene, obj1 is relocated (blue arrow). Right: Local occupancy grid (LOG) for obj4. The colors red and green, respectively, represent objects and free spaces. The color blue represents $o_{set}$ trajectory, the initial positions of each object in $o_{\text{set}}$, and the initial position of the agent. When obj1 moves, blue and green areas expand; thus, the configuration score increases.