Task and Motion Planning (TAMP) has progressed significantly in solving intricate manipulation tasks in recent years, but the robust execution of these plans remains less touched. Particularly, generalizing to diverse geometric scenarios is still challenging during execution. In this work, we propose a reactive TAMP method to deal with disturbances and geometric ambiguities by combining an active inference planner (AIP) for online action selection with a proposed multi-modal model predictive path integral controller (M3P2I) for low-level control. The AIP generates online alternative plans, each of which is translated into a cost function to be sampled for the proposed method. The proposed M3P2I then uses a parallelizable physics simulator for throwing different rollouts, leading to a coherent optimal solution by averaging the weighted samples based on their costs. Our method empowers real-time adaptation of action sequences to rectify failed plans, while also computing low-level motions to address dynamic obstacles or disturbances that could potentially invalidate the existing plan.

Theoretical findings are validated in simulation and in the real world. We show that the framework exhibits reactiveness in different scenarios, including battery charging, push-pull among obstacles, and pick-place with disturbances. We show that our framework outperforms an off-the-shelf RL method in the reactive pick-place task in terms of position error and orientation error. We also show that M3P2I is generalizable in combining different constraints, such as generating hybrid motions of push and pull for the mobile robot, and grasping objects with different grasping poses. The real-world experiments show that the system exhibits reactiveness and robustness against human disturbances in a variety of manipulation tasks.

The supporting videos can be found at https://sites.google.com/view/m3p2i-aip.