Visibility-Aware Task and Motion Planning with Task-Centric Perception

Massachusetts Institute of Technology

VA-TAMP extends fully observed TAMP solvers to partially observed domains with an object-based memory and task-centric active perception.

Abstract

Long-horizon manipulation under uncertainty remains a challenging problem in robotics. We present VA-TAMP, an algorithm for Visibility-Aware Task and Motion Planning. VA-TAMP extends existing deterministic task and motion planning solvers to partially observable object rearrangement tasks, while still being able to take advantage of the efficiency and compositional generalization over long horizons of the base solver. This extension interleaves sensing actions into a plan skeleton that assumed full observability, and uses a novel persistent object-based memory to fuse observations into object mesh hypotheses and an observed occupancy map. We evaluate VA-TAMP on real-world tabletop rearrangement tasks which feature heavy occlusion and partial visibility and demonstrate that it outperforms baselines that do not perform active perception or have persistent memory.

Motivation

Task and Motion Planning (TAMP) solvers efficiently compose manipulation skills over long horizons, but they assume the scene is known. In cluttered tabletop tasks like the one below, where the robot must place the cleaning supplies on the red tray, the initial observation is noisy and incomplete: the tray extends out of frame and the objects to be moved are partially hidden behind boxes. The robot never sees the full shape of any object, yet it must plan from this scene estimate.

Initial wrist view.

(a) Initial wrist view. Goal: {On(spray-bottle, red-tray), On(green-eraser, red-tray)}

Initial scene estimate.

(b) Initial scene estimate: a convex hull per detected object.

Planned wrist view.

(c) A planned wrist view reveals the full extent of the eraser.

Completed shapes.

(d) Task-relevant objects refined into completed shapes.

Simply re-estimating and replanning after every observation does not fix this: no single view shows the far edge of the tray, and a strictly goal-directed plan never looks behind the boxes. Belief-space planning handles uncertainty in principle, but reasoning over distributions of object geometry is intractable. VA-TAMP instead keeps an off-the-shelf TAMP solver and adds constraints requiring the robot to verify its object models before interacting with them. These constraints induce information-gathering actions, and the resulting observations refine the models.

Method Overview

VA-TAMP extends fully observed TAMP to partially observed domains by coupling object-based memory with scene-model verification. The memory supplies object meshes and an occupancy grid, and the planner constructs a goal-reaching plan that also validates the meshes. As observations arrive, the remaining plan guides refinement of task-relevant objects.

Overview of the VA-TAMP policy.

Overview of VA-TAMP. RGB-D observations update an object-based memory (OBM), which maintains observed occupancy \(V_t\) and object mesh hypotheses \(\mathcal{M}_t\). Given a goal \(G\), a fully observed TAMP solver computes a plan \(p\) over \(\mathcal{M}_t\). Justify uses \(\mathcal{M}_t\) and \(V_t\) to derive a set of visibility preconditions \(\Phi\) and insert look actions to produce a justified plan \(\hat p\). These preconditions are checked against \(V_t\) to select the next action \(a_t\) or trigger replanning. The plan also guides OBM refinement, prioritizing mesh hypotheses that are relevant to the plan and inconsistent with accumulated observations.

Given a set of object meshes, we can leverage existing classical TAMP solvers to generate a plan skeleton: an optimistic plan that assumes the current object meshes are accurate and is entirely goal-oriented. To guard against this optimism, we require that every plan be justified. This means that any time the plan requires the robot to move through space, it must have observed that the space is collision free. Any time the plan requires the robot to grasp an object at a given pose, it must have observed that the object surface exists in between the fingers at that pose. To justify a plan, the planner has to find motions that move the camera to viewpoints where the desired regions are visible. The initial hull of the green eraser (b) does not justify a grasp; the planner inserts the wrist view in (c), which reveals the eraser's full extent and lets the grasp proceed.

VA-TAMP takes a model-predictive control style approach to constructing a policy. We combine a state estimator which computes models of the scene conditioned on observations with an action-selection module which selects an action conditioned on the goal and current scene models. On execution of the action, the policy receives a new observation, which it uses to update its model and continue execution or replan. The action selection starts by solving a fully observed TAMP problem assuming \(\mathcal{M}\) is the true state to get a plan backbone \(p\). The Justify procedure derives observation-conditioned preconditions for manipulation actions and then interleaves sensing actions into the plan to verify the preconditions to get the justified plan \(\hat{p}\). If the preconditions hold in the true scene, we execute the actions in the plan and receive new observations, which are used to update the OBM. If the preconditions do not hold, the OBM is queried for a fresh scene model to plan against.

Contributions

  • VA-TAMP: An algorithm for Visibility-Aware Task and Motion Planning. VA-TAMP augments classical TAMP solvers with a persistent memory system and task-driven information gathering behavior. We show that this method outperforms systems which don't perform active perception or do not associate object detections across observations.
  • A task-centric, resource-rational OBM. Accurately modeling a scene is a computationally expensive task, with state-of-the-art completion models taking 4-5 seconds per object in a single frame. Rather than completing every object in every frame, the OBM prioritizes reconstruction on objects that are task-relevant and do not yet have a good model.
  • Efficient information gathering in partially observable problems by leveraging a fully observed TAMP solver. We use the result of an existing TAMP solver to define which regions of space the robot interacts with, either as motion in assumed free space or contact with assumed object surfaces. We make the observation of these regions of space a precondition to acting, forcing the robot to collect more observations and update its models as needed.

Task-Centric Object-Based Memory

In order to build and refine our scene representation as we observe more of the scene and objects move, we define an Object-Based Memory module (OBM). The OBM collects a history of observations and maintains two complementary representations of the scene. The first representation is an occupancy grid, which accumulates depth data and tracks the regions of space that have not yet been observed. The second representation is a set of objects that are believed to exist in the scene after being detected in individual observations. The OBM associates these detections on a per-object basis and leverages a generative image-to-3D reconstruction model to hypothesize a complete 3D mesh geometry from partial RGBD observations.

Formally, the OBM takes as input an observation history \(o_{1:t}\) and the robot's current plan \(\hat{p}\). It returns a collection of posed surface meshes \(\mathcal{M}_t\) and an occupancy map \(V_t\), where each voxel is labeled empty, full, or unknown.

Associating detections into slots

Our OBM maintains a set of object slots, each representing a hypothesized object through its observation history and current 3D geometry estimate. For each observation, we query a VLM for bounding boxes of objects on the table, then use a segmentation model to refine each box into an object mask. These detections are then either associated with an existing object slot, by matching DINO and geometric similarity, or initialize a new object slot. Image-to-3D generative models provide a data driven prior for object completion.

OBM updating during robot execution

The voxel grid \(V\) is instantiated from the first observation and updated in real time as new observations arrive. Querying the generative shape model is slow relative to the observation frequency, so the OBM makes at most one query per perception step, running in parallel with robot execution. Estimating \(\mathcal{M}_t\) is therefore a sequential decision problem: at each step, the OBM chooses which object to refine and whether the returned mesh should replace its current hypothesis.

Choosing what to refine

The OBM is resource-rational and task-centric: it focuses its computation time on objects that the planner has decided are relevant to the task. Because the true geometries in the scene are unknown, we define a surrogate cost \(\mathcal{L}\) which measures how inconsistent a candidate posed mesh is with the object detections and observed points. The OBM then greedily picks the slot whose current estimate is the most inconsistent with its evidence, weighted by task relevance,

\( i^{*} = \arg\max_{i}\; w(M_t^i, \hat{p}_t)\, \mathcal{L}(M_t^i, \mathcal{P}_t^i, \mathcal{D}_t^i), \qquad w(M_t^i, \hat{p}_t) = \exp\!\Big(-\beta \min_{x \in M_t^i,\; y \in \tau_{ee}(\hat{p}_t)} \|x - y\|\Big), \)

where \(\tau_{ee}(\hat{p}_t)\) is the end-effector path swept by the current plan. The selected slot is passed to the image-to-3D model, and the returned mesh replaces the current hypothesis only if it is more consistent with the accumulated observations. In the motivating example, the eraser and tray are refined into complete meshes while the cardboard boxes are left as the coarse hulls from the first frame.

Plan Justification

The input to plan justification is a plan \(p = (a_1, \ldots, a_n)\) from a TAMP solver that treated the OBM's mesh hypotheses \(\mathcal{M}_t\) as ground truth. The output is a justified plan \(\hat{p}\), which interleaves Look actions with the original manipulation actions in \(p\), and a set of visibility preconditions \(\Phi\). These preconditions are tested before each action executes, and hold when the constraints on the hypothesized meshes \(\mathcal{M}_t\) are supported by the observed occupancy grid \(V_t\). A justified plan should be valid: before each manipulation action, the preceding Look actions should have labeled every voxel its precondition depends on. It should also be efficient, avoiding observations that increase the original plan's cost without validating or helping refine a task-relevant object.

Looks are inserted by a fixed rule. For each pick and place pair in \(p\), a look for the pick's precondition and another for the place's precondition are both inserted immediately before the pick. The place's look is not deferred until after the pick because, once the robot grasps something, the hand is no longer free and the wrist camera cannot collect new observations. The policy then executes \(\hat{p}\) in order, checking each precondition against the current \(V_t\) before its action runs and returning to the OBM for a fresh scene estimate when a check fails: if \(\phi\) is not satisfied, replan.

Observe the parts of the environment the plan interacts with, replan if the observations contradict the estimated meshes. A fully observed TAMP solution is justified by inserting look actions at planned look poses; when the observed spray can occupies space assumed empty, the robot replans with updated meshes.

Deriving visibility preconditions

A visibility precondition before and after the look.

Every visibility precondition is a tuple of point sets \(\phi_a = (F_a, E_a)\), where every point in \(F_a\) must be labeled full and every point in \(E_a\) must be labeled empty.

The figure shows a precondition for pick(eraser, g, q). (a) The precondition computed from an estimated \(\mathcal{M}_t\). The finger contacts \(F_a\) (pink) must read full, and the swept volume of the gripper along the grasp trajectory \(E_a\) must read empty. The blue cells are known to be empty, the orange cells are unknown. (b) After the look, the required cells are observed and \(\phi_a\) holds.

  • pick: the antipodal constraint on the grasp depends on the surface of the object at the two finger contacts, so \(F_a\) contains the points on the object where the finger pads close. The collision-free constraint is on the volume swept by the arm while approaching the grasp, so \(E_a\) contains samples of this volume.
  • place: the support constraint starts by defining the “footprint” of the object as the projection of its volume onto a plane at its lowest point. \(F_a\) contains a column of points below each cell of the footprint, satisfied when any point in the column is labeled full. The collision-free constraint depends on the footprint at the placement, the volume swept by the held object along its carry, and the volume swept by the arm, so \(E_a\) contains samples of all three.

The visibility precondition holds when a fixed fraction of the constrained points have been observed and have the required label. Preconditions are derived once from the plan with respect to \(\mathcal{M}_t\) and re-derived whenever \(\mathcal{M}_t\) changes.

The look operator

Satisfying a visibility precondition requires new observations. The Look action takes a precondition and a sequence of camera poses. A single pose is insufficient when the points of a precondition do not fit in one field of view, or are occluded from one direction and visible from another. The sampler for the camera poses is a greedy set cover over the subset of \(F_a \cup E_a\) that is currently labeled unknown in \(V_t\). Each round selects an unknown point and samples candidate camera poses on the surface of a sphere around that point, oriented to look at the point. Samples are discarded if they are kinematically infeasible, are predicted to be occluded by other objects in the scene, or move the robot through points not labeled empty. Among the surviving samples, we keep the one whose field of view contains the most points in the precondition that are still unknown. If no cover can be found, the precondition is marked unobservable; its precondition then fails at execution time and the policy replans. Because the underlying TAMP solver assumes full observability, the Look actions assume that there is some viewpoint from which the region can be viewed, rather than planning actions to increase the visibility. In order to construct a solvable problem for the fully observed TAMP planner, we also assume that the initial observation contains every object referenced in the goal statement.

Experiments

Partially observable object rearrangement is a ubiquitous but surprisingly under-studied problem setting and, as such, there are no directly comparable existing methods. We explore the effectiveness of VA-TAMP in three scenarios of increasing complexity that were specifically designed to stress different aspects of the problem. Our ablations and tasks are designed to explore the following questions:

Q1: Do planned views derived from the current scene estimate generate useful targets for active perception?

Q2: Does a task-centric approach to model refinement improve downstream planning performance?

We answer Q1 by defining the visibility-unaware ablation VUTAMP, which does not actively plan observation actions or derive visibility preconditions for replanning. We answer Q2 by defining the VUTAMP-UNIFORM ablation, which follows the same planning and replanning loop as VUTAMP, but the update strategy for the OBM is task-agnostic. Finally, we compare against TiPToP, an open-loop model-based planning system that forms initial conservative estimates of the scene and executes the generated plan open-loop, without updating on new observations.

We perform evaluations on the Franka FR3 with a ZED-M wrist camera. The perception pipeline uses Gemini-ER-2 as the VLM, SAM2 as the segmentation model, and RecGen as the image-to-3D reconstruction model. The fully observed TAMP solver we use is cuTAMP, a GPU-parallelized TAMP solver.

Tasks

Cube on Bread initial view.

Cubes on Bread. The robot must pick up two stacked blocks and place both of them on the slice of bread. Moving the top block reveals the extent of the bottom block.

Teacups on Plate initial view.

Teacup on Plate. The robot has to move the blue cup from the table to the flower-shaped plate. To do this, it must first take the occluding block off the cup.

Supplies on Tray initial view.

Supplies on Tray. Two cleaning supplies are occluded by the boxes in the scene, which the robot has to look behind. The tray also has collision obstacles that are not visible from the initial view.

The initial wrist-camera view of each task.

Results

Method Cubes on Bread Teacup on Plate Supplies on Tray
TiPToP2 / 103 / 100 / 10
VUTAMP-UNIFORM8 / 103 / 102 / 10
VUTAMP8 / 109 / 102 / 10
VA-TAMP9 / 1010 / 1010 / 10

Success rate (successful trials / attempted trials) for each method on each scene.

In the Cubes on Bread task, TiPToP's failures could be divided largely into grasp failures due to trying to place the robot's finger inside the body of the bottom cube, and placement failures due to trying to place the bottom cube onto a region of the toast that the top cube had already occupied. Because VUTAMP-UNIFORM was able to update its memories, it could outperform TiPToP.

In the Cubes on Bread task, there were only three objects in the scene and they were all task-relevant. The Teacup on Plate task introduces another layer of complexity by cluttering the scene with many task-irrelevant objects. By weighing the objects higher due to their proximity to the robot in its nominal plan, the objects are refined earlier by the OBM, leading to VUTAMP outperforming VUTAMP-UNIFORM.

In the Supplies on Tray task, the two main failure modes were grabbing an object about its longer axis (which seemed collision-free due to the cardboard box occlusion), or trying to place the objects on the red tray at an orientation that caused collision with the obstacles already there. In both cases, the robot had to plan observations which did not directly interact with the objects in the scene, but did serve to validate the robot's scene models. VA-TAMP is the only method that does this and thus has the strongest performance on this task.

Planned vs. executed: VA-TAMP on Supplies on Tray

Planned motion over the OBM's current mesh hypotheses, including the inserted looks.

The same run executed on the Franka FR3.

BibTeX

@article{chintalapudi2026vatamp,
  title   = {Visibility-Aware Task and Motion Planning with Task-Centric Perception},
  author  = {Chintalapudi, Sahit and Wang, Xiaoyan and Zoroufi, Aryan and Agarwal, Aditya and Kanwisher, Nancy and Lozano-P{\'e}rez, Tom{\'a}s and Kaelbling, Leslie Pack},
  journal = {arXiv preprint arXiv:2611.16213},
  year    = {2026}
}