LAGO Policy: Latency-Aware Asynchronous Diffusion Policies with Goal-Directed Collision-Free Planning for Smooth Manipulation

Guowei Shi1,2,4,*, Xupeng Xie1,2,4,*, Yiming Luo3, Jian Guo2, Jun Ma1, Boyu Zhou4,†
1The Hong Kong University of Science and Technology (Guangzhou), 2International Digital Economy Academy (IDEA), 3The University of Hong Kong, 4Southern University of Science and Technology
*Equal contribution.  †Corresponding author.
Paper arXiv Code
LAGO Policy Overview
LAGO Policy. Latency-aware classifier-free guidance improves inter-chunk consistency under asynchronous inference, goal prediction enables goal-directed collision-free planning, and spatial–temporal trajectory optimization refines the executed motion for smooth execution.

Video

Abstract

Diffusion-based visuomotor policies deployed with asynchronous inference often exhibit inter-chunk discontinuities and lack explicit mechanisms for obstacle-aware execution, leading to jerky motions and collisions that hinder reliable manipulation in real-world scenes. To address these issues, we propose LAGO Policy, a unified asynchronous action-generation framework that integrates trajectory optimization with diffusion policy for smooth and safe execution. LAGO Policy improves inter-chunk consistency via latency-aware classifier-free guidance conditioning on future actions. It further enables goal-directed collision-free trajectory planning by predicting a task-relevant interaction goal from demonstrations. Finally, spatial–temporal trajectory optimization refines the actions to be executed for low-jerk and feasible motion. Extensive real-world experiments demonstrate that LAGO Policy achieves smooth collision-free execution with high task success across challenging manipulation tasks.

Contributions

  1. A latency-aware CFG training scheme that decouples observation and future-action conditioning with randomized delay for smooth asynchronous execution.
  2. A goal-directed safe generation paradigm coupling demonstration-derived goal prediction with trajectory optimization for safe, smooth motion.
  3. Comprehensive real-world validation across eight challenging manipulation tasks.

Method

LAGO Policy Pipeline
Overview of LAGO Policy. LAGO Policy unifies temporally consistent action generation with goal-directed collision-free motion generation for smooth robotic manipulation.

Each asynchronous cycle generates an action chunk and predicts a task-relevant goal; goal-conditioned trajectory generation is triggered when the path to the goal is obstructed or far, and all executed motions are refined by spatial–temporal optimization.

Latency-Aware Classifier-Free Guidance

We inject the future-action condition through CFG and randomize its temporal delay during training, making inter-chunk execution robust to latency-induced shifts at deployment.

Latency-Aware CFG Motivation
Motivation for latency-aware CFG. (a) Aligned case. (b) Training with aligned future-action conditions. (c) Shifted conditions at deployment: inference latency induces discontinuities at chunk boundaries.

Goal-Prediction Head

A goal-prediction head on the denoising U-Net bottleneck predicts a task-relevant interaction goal from demonstrations, guiding collision-free planning in cluttered scenes.

Collision-Free Trajectory Generation

A line-of-sight check toward the predicted goal triggers a spline-based optimizer (inspired by EGO-Planner) that produces a smooth collision-free end-effector trajectory without requiring a collision-free initialization.

Collision-Free Trajectory Generation
The initial trajectory collides with an unseen obstacle. A* provides a collision-free connector, guiding optimization to a smooth collision-free trajectory.

Spatial-Temporal Trajectory Optimization

Executed actions are treated as keypoints and refined with MINCO-based spatial–temporal optimization for low-jerk, feasible motion.

Experiments

We design experiments to answer the following questions:

  • Q1: Does latency-aware classifier-free guidance maintain inter-chunk consistency under temporally shifted future-action conditions?
  • Q2: Does LAGO Policy produce smoother execution trajectories in real-world manipulation?
  • Q3: Does goal-directed collision-free planning improve task completion in the presence of unseen obstacles?

Experiment Setup

We evaluate on two platforms — a 6-DoF ARX5 and a 7-DoF Franka arm — each with two external Intel RealSense RGB-D cameras and a wrist-mounted 180° fisheye camera, across eight real-world manipulation tasks. Performance is measured by success rate (SR), average time of successful rollouts (ATR), inter-chunk consistency (CON), and integrated squared jerk (ISJ).

Real-world task setups
Real-world task setups. Tape Hanging and Screw Sorting are evaluated on the Franka arm, while the remaining tasks are evaluated on ARX5.

Real-World Evaluation Results

Method Pick & Place Pen Insertion Pouring Cup Transfer
SR↑ATR↓CON↓ISJ↓ SR↑ATR↓CON↓ISJ↓ SR↑ATR↓CON↓ISJ↓ SR↑ATR↓CON↓ISJ↓
DP 0.8516.910.03427.29 0.708.760.009232.71 0.2520.240.03322.44 0.1010.270.04126.34
Ours 1.015.410.01924.37 0.907.940.01121.45 0.6519.410.01215.74 0.4010.620.008310.11
Method Towel Folding Box Organizing Tape Hanging Screw Sorting
SR↑ATR↓CON↓ISJ↓ SR↑ATR↓CON↓ISJ↓ SR↑ATR↓CON↓ISJ↓ SR↑ATR↓CON↓ISJ↓
DP 0.3518.240.05833.11 0.6028.130.07296.10 0.8032.820.0066115.51 0.6053.500.0062116.67
Ours 0.7014.270.01715.25 0.8527.310.02629.07 0.9036.800.005921.16 0.9552.970.003718.77

Table 1: Real-World Evaluation. LAGO Policy achieves lower CON and ISJ on nearly all tasks, indicating improved inter-chunk continuity and smoother execution trajectories, while also improving success rates.

Robustness to Future-Action Condition Shifts (Q1)

CFG Shift Ablation
SR and CON under simulated future-action shifts. As the shift increases, Ours-LA-CFG maintains a higher success rate and exhibits less inter-chunk discontinuity.

Across increasing future-action shifts, Ours-LA-CFG (CFG injection with delay randomization) maintains higher SR and smaller CON change than SAIL and Ours-CFG, confirming improved robustness to temporal shifts.

Smooth Execution in Real-World Manipulation (Q2)

LAGO Policy achieves lower CON on seven tasks and lower ISJ on all eight, with steadier gripper commands that improve contact-rich tasks such as Towel Folding and Pouring.

Goal-Directed Obstacle Avoidance (Q3)

Obstacle Avoidance
Obstacles absent from the training demonstrations are added to four tasks (highlighted in yellow). Our Goal-Directed method plans smooth collision-free motion from the start to the predicted goal.
Method Box Cup Pouring Pen
No-Avoid 0.400.00.00.60
Local 0.600.200.400.80
Goal-Directed 1.00.800.801.0

Table 2: Success rate on four real-world tasks with unseen obstacles.

By planning a collision-free trajectory to the predicted goal before handing control back for the final interaction, Goal-Directed avoids reactive short-horizon failures and achieves the highest success under unseen obstacles.

Goal-Directed Motion Planning
Effect of goal-directed motion planning. (a) Without goal-directed planning, the motion remains piecewise and inefficient. (b) With goal-directed planning, the robot reaches the goal along a smooth trajectory, reducing task completion time.

Conclusion

LAGO Policy combines latency-aware CFG with goal-directed collision-free planning to deliver smooth, safe, and successful diffusion-based manipulation, validated on eight real-world tasks.

BibTeX

@article{shi2026lago,
  title   = {LAGO Policy: Latency-Aware Asynchronous Diffusion Policies with Goal-Directed Collision-Free Planning for Smooth Manipulation},
  author  = {Shi, Guowei and Xie, Xupeng and Luo, Yiming and Guo, Jian and Ma, Jun and Zhou, Boyu},
  journal = {arXiv preprint arXiv:2606.17982},
  year    = {2026},
  url     = {https://lago-policy.github.io/}
}