Kinodynamic Task and Motion Planning using VLM-guided and Interleaved Sampling

Task and Motion Planning (TAMP) integrates high-level task planning with low-level motion feasibility, but existing methods are costly in long-horizon problems due to excessive motion sampling. While LLMs provide commonsense priors, they lack 3D spatial reasoning and cannot ensure geometric or dynamic feasibility. We propose a kinodynamic TAMP planner based on a hybrid state tree that uniformly represents symbolic and numeric states during planning, enabling task and motion decisions to be jointly decided. Kinodynamic constraints embedded in the TAMP problem are verified by an off-the-shelf motion planner and physics simulator, and a VLM guides exploring a TAMP solution and backtracks the search based on visual rendering of the states. Experiments on the simulated domains and in the real world show 32.14% - 1166.67% increased average success rates compared to traditional and LLM-based TAMP planners and reduced planning time on complex problems, with ablations further highlighting the benefits of VLM backtracking. More details are available at https://graphics.ewha.ac.kr/kinodynamicTAMP/.

💡 Research Summary

**
This paper introduces a novel kinodynamic task and motion planning (TAMP) framework that tightly integrates high‑level symbolic reasoning with low‑level continuous motion validation, guided by a visual language model (VLM). Traditional TAMP approaches fall into two categories: sequencing‑first, which first generates a symbolic plan and then attempts to satisfy geometric constraints, and satisfaction‑first, which samples motions before symbolic search. Both suffer from inefficiencies in long‑horizon problems—repeated constraint solving or combinatorial explosion of samples. Recent attempts to incorporate large language models (LLMs) provide commonsense knowledge but lack 3‑D spatial understanding and cannot guarantee geometric or dynamic feasibility.

The authors address these issues by constructing a Hybrid State Tree whose nodes are tuples (s, x) combining a symbolic state s and a continuous world state x (object poses, robot joint configurations, trajectories). This tree structure naturally accommodates multiple continuous instantiations of the same symbolic state, avoiding redundant sampling while allowing simultaneous decision making for both levels.

The planning pipeline proceeds as follows: (1) A top‑k symbolic planner (based on the K* algorithm and Fast‑Downward) generates a diverse set of task skeletons and organizes them into a discrete state graph G. (2) For a current hybrid node hₜ = (sₜ, xₜ), the outgoing edges of sₜ in G define the admissible symbolic actions. (3) Each symbolic action is instantiated by sampling continuous parameters: grasp or placement pose p ∈ SE(3), current and target robot configurations q, q′, and a trajectory ξ mapping q to q′. (4) The sampled motion is first checked by an off‑the‑shelf motion planner for collision and joint limits, then validated by a physics simulator (e.g., PyBullet) for kinodynamic constraints such as inertia, forces, grasp stability, and object stability. If both checks succeed, a new hybrid node hₜ₊₁ is added to the tree.

When a node cannot be expanded, the system attempts up to K random resamplings. If still unsuccessful, the VLM‑guided backtracking mechanism is invoked. The current world state is rendered into an image and fed to a pre‑trained VLM together with a textual prompt describing the failure. The VLM reasons over the visual context and suggests a backtrack node hᵣ from which expansion should resume, effectively providing a visual‑informed heuristic for recovery. This contrasts with prior work that relies solely on textual re‑prompting.

Experiments were conducted in three domains: (i) a Blocksworld simulation, (ii) a Kitchen simulation with multiple objects, dynamic constraints, and clutter, and (iii) a real‑world robot arm setup. Baselines included a traditional domain‑independent TAMP planner and an LLM‑based TAMP planner. Results show substantial improvements: in Blocksworld the average success rate increased by 32.14%–105.56%; in Kitchen the increase ranged from 280% to 1166.67%, with the proposed method succeeding where baselines failed. Planning time on complex problems decreased by 40%–70% compared to baselines. An ablation study demonstrated that removing VLM guidance reduced success rates by 15%–30%, confirming the critical role of visual feedback for backtracking.

Key contributions are: (1) the hybrid state tree that unifies symbolic and continuous planning, (2) integration of top‑k symbolic planning with motion planning and physics simulation to enforce kinodynamic feasibility, (3) novel use of a VLM for visual‑based backtracking, and (4 extensive validation in simulation and on a physical robot. Limitations include the computational overhead of VLM inference and dependence on the fidelity of the physics simulator. Future work may explore lightweight VLM architectures, probabilistic backtrack policies, and domain‑transfer techniques to bridge simulation‑to‑real gaps.