CSubBT: A modular execution framework with self-adjusting capability for mobile manipulation system

Embodied intelligence is advancing the capability of intelligent agents to transition from controlled environments, such as factories, to unstructured real-world settings by integrating perception, planning, and physical interaction. Task and Motion Planning (TAMP) can guide agents in completing complex tasks in these unstructured environments. However, the execution of plans by agents is not merely the implementation of pre-defined instructions. The planned actions often fail due to discrepancies between the perceptual information used in planning and the actual conditions encountered. Existing robust execution systems fall short of providing a universal solution at the execution level, making them unsuitable as actuators for upstream task planners. In this paper, we propose the Conditional Subtree (CSubBT), a modular, self-adjusting execution framework for mobile manipulation systems based on Behavior Trees (BTs). CSubBT decomposes planned actions into sub-actions and leverages BTs to control their execution, addressing potential anomalies without the need for intervention from high-level planners. CSubBT treats common anomalies as constraint non-satisfaction problems and continuously guides the robot in performing tasks by sampling new action parameters in the constraint space when anomalies are detected. We validate the robustness of our framework through extensive manipulation experiments conducted in both simulated and real-world environments across different platforms.