Warm-Starting Trajectory Solvers from Partial, Noisy Point Clouds

A robot arm that plans with a numerical optimizer can in principle guarantee that its motion avoids collisions. It needs two things to do that quickly. The first is a good starting guess, since otherwise the solver grinds for seconds. The second is an obstacle model it can trust. A depth camera supplies neither. It sees only the front surfaces of objects, it drops pixels at edges and on shiny material, and its noise looks nothing like the clean depth a simulator renders.

We propose to split these two needs deliberately. A learned generator proposes starting trajectories, and it may freely "imagine" the hidden backs of objects, because a wrong guess only costs solver time. The solver's hard collision constraints are built only from what was actually observed, plus a conservative model of what could not be seen. Concretely, we train a conditional diffusion model (a generator that learns to turn random noise into samples by gradually denoising it) on noisy, partial simulated point clouds. Its targets are trajectories that were solved against the complete ground-truth geometry, which the simulator knows even though the camera does not.

The result is a warm-starter that learns shape completion implicitly, only to the extent that completion changes where good trajectories go. It never asks the solver to trust hallucinated surfaces. The two-sentence version is this: put the shape prior in the initial guess, not in the constraints. Then test whether that beats both cold starts and the obvious pipeline of "complete the point cloud, then plan."

Core Claim

Trajectories solved against privileged complete geometry, used as supervision for a generator that sees only degraded partial observations, give warm starts with three properties. They cut interior-point solve time sharply relative to cold starts. They route around hidden object volumes more reliably than warm-starters trained on observed geometry alone. They reach this without the failure mode of explicit completion networks, whose phantom points and holes corrupt the collision constraints themselves.

The measurable consequence is that, against a completion-first pipeline of equal compute, our system should match or beat solve time and success. It should also have a lower rate of hidden collisions, meaning trajectories that are feasible with respect to the robot's constraints but collide with the true, unobserved geometry.

Where This Sits

Generative warm-starting is well established when the problem is described by clean vectors. DiffuSolve samples multimodal trajectory seeds with a conditional diffusion model and reports 2x to 11x efficiency over uniform sampling. Constraint-Aware Diffusion Models for Trajectory Optimization adds an analytic constraint-violation loss that raises the feasible fraction of raw samples from 8.5% to 58.3%, at 4 to 7 times the training cost. Learning-based Warm-Starting for Fast Sequential Convex Programming cuts iterations by 57% on hard instances with a deterministic network.

All three assume exact analytic obstacles. They supply our method: diffusion seeding followed by solver refinement. They do not address perception. Their constraint loss cannot even be evaluated on a hollow point cloud.

The closest work is Structured deep generative models for sampling on constraint manifolds. It conditions a GAN on rendered depth maps and masks, then projects the seeds with a local solver, reaching 78% pick-and-place success versus 46% for a non-factorized model, with 10 samples in 8 ms. It differs from our proposal in three ways. Its depth is noise-free. Its supervision comes from the same geometry the network sees. It never confronts the question of which geometry the solver's constraints should use.

Grasping Trajectory Optimization with Point Clouds supplies our solver setting and motivates the problem. It optimizes grasp trajectories in Ipopt with collision distances computed directly from the observed point cloud, reaching 65% on the real SceneReplica benchmark. However, it cold-starts at 15.4 seconds per grasp and has no protection against collisions behind objects.

Two perception papers constrain the design:

  • PCN shows that global shape priors can fill in missing hemispheres, which local upsampling like PUGeo-Net cannot. However, PCN is judged only by point-distance metrics, and generative completion is known to produce plausible but wrong occupancy. That is exactly why we refuse to put it in the constraints.
  • DexPoint and Deep Drone Acrobatics show that geometric inputs transfer from simulation far better than raw images; in the drone case, image-based models reach 0% on novel backgrounds. This supports conditioning on point clouds rather than RGB. DexPoint only handles occlusion by the robot's own hand, via forward kinematics. The external occlusions we target are left open.

Research Program

Data generation. In PyBullet we generate tabletop and shelf scenes of YCB and ShapeNet objects. For each scene we render a single-view depth image through a structured degradation model:

  • edge fraying,
  • dropout on specular and dark surfaces,
  • depth-dependent noise,
  • occasional multipath-style ghost returns.

We back-project the rendered depth to a partial point cloud. Separately, we solve the grasp-and-reach trajectory problem of Yu et al. in Ipopt against a signed distance field built from the complete meshes. We keep multiple local optima per scene, using multi-start, so that the targets are multimodal. The target dataset size is on the order of the 114k to 179k trajectories used by DiffuSolve. Budget permitting, we will ablate downward toward the 4,000 pairs per scenario of the structured-generative approach.

Generator. A PointNet++-style encoder conditions a diffusion model over joint-space trajectories. We run it with a small number of denoising steps (around 10) to keep latency in the tens of milliseconds. A conditional variational autoencoder, in the spirit of 6-DOF GraspNet, serves as a faster alternative.

Solver constraints at test time. The solver never sees complete geometry. We compare two constraint sets:

  • the observed signed distance field, which reproduces Yu et al.;
  • the observed field plus a shadow volume: occluded space behind observed surfaces up to a fixed depth, treated as occupied.

The shadow volume is conservative and can block valid paths. One question we measure is whether good seeds let the solver succeed even under that conservatism.

Analysis. We characterize when privileged supervision helps, as a function of occluded fraction and noise level. We also look at failure cases where the seed's implicit completion is wrong and check whether the solver recovers or is misled.

Baselines

The naive reference is Ipopt cold-started with the heuristic initialization of Grasping Trajectory Optimization with Point Clouds, on the same scenes, cost, and constraint sets. It reports 65% real SceneReplica success and 15.4 s per grasp, against 57% for an OMPL baseline and 3.2 s for OMG-Planner. We will reproduce it in simulation before comparing. Beating it means a large reduction in wall-clock time, including network inference, at equal or better success and hidden-collision rate.

The strongest existing learned competitor is a structured-generative-style warm-starter after Ortiz-Haro et al.. We retrain it on our scenes with its original recipe: noise-free depth, and supervision solved against observed geometry. For fairness it gets the same trajectory count, encoder capacity, and hyperparameter search budget as our model. Its reported numbers (78% pick-and-place; 2.07 to 2.77 solver attempts) come from a different, noise-free task, so they are context rather than a target. No reported result was found on Lacuna for any warm-starter evaluated on noisy partial point clouds. Beating it means lower hidden-collision rate and fewer solver attempts under noise, with no loss in clean conditions.

The obvious composite is complete-then-plan: PCN, trained on the same noisy partial clouds, produces a completed cloud. That cloud feeds both the warm-starter and the solver's distance field. A variant uses completion only for the solver and cold-starts. We give the completion network the same compute as our generator's encoder. Its reported quality is only qualitative relative to volumetric baselines on Chamfer and Earth Mover's distance, so no directly comparable number exists on Lacuna.

A bounding-box baseline replaces unseen geometry with oriented boxes fitted to each observed segment.

Three ablations isolate the key ingredient:

  • training on observed-geometry solutions instead of complete-geometry solutions (tests privileged supervision);
  • training on clean instead of degraded depth (tests the noise model);
  • a single-output regression network in the style of Learning-based Warm-Starting for Fast SCP (tests multimodality).

Why not just…?

Why not just complete the point cloud with PCN and plan on the result? This is the real threat, and it is a primary baseline rather than a straw man. Our argument against it is specific. Any completion error becomes a hard constraint. Phantom points block feasible paths, and holes admit colliding ones. Point-distance metrics do not track either failure, a gap the research file notes for PCN and 3D MeanFlow. If complete-then-plan matches us on hidden-collision rate and solve time at equal compute, the claim fails. We would then report that explicit completion suffices.

Why not just inflate obstacles with bounding boxes or convex hulls? Yu et al. note that bounding primitives over-approximate free space and reject valid motions in tight spaces. We expect this to show up as lower success in shelf and clutter scenes, and we measure it directly. Boxes may be adequate in open tabletop scenes. If so, we will say that the method's value is confined to cluttered, tight-clearance settings.

Why not just add depth-noise domain randomization to an existing warm-starter? Noise randomization is part of our recipe, but by itself it does not tell the generator what lies behind objects. The clean-versus-noisy ablation and the observed-versus-privileged supervision ablation separate the two effects. If noise randomization alone closes the gap, the privileged-supervision idea is unnecessary.

Why not skip the solver and train an end-to-end policy, as DexPoint does? DexPoint transfers well, but it gives no constraint guarantees. Even constraint-aware generation leaves roughly 42% of raw samples infeasible in Constraint-Aware Diffusion. We report the raw, unrefined feasibility of our own samples to quantify what the solver adds.

Proposed Experiments

Metrics. All systems are evaluated on held-out simulated scenes with unseen objects, across three occlusion bins (low, medium, high occluded fraction) and three noise levels. We report:

  • end-to-end wall-clock time, including inference;
  • Ipopt iterations;
  • solve success within a fixed time budget;
  • hidden-collision rate, measured against ground-truth meshes;
  • grasp success.

First decisive check. Does privileged supervision reduce hidden collisions relative to observed-geometry supervision at matched solve time, under the observed-only constraint set? If not, the central idea fails.

Second decisive check. Under the conservative shadow-volume constraints, do our seeds recover the success rate lost to conservatism, where cold starts and complete-then-plan do not? A positive result would give a practical system that is both safer and fast.

Robustness test. We train at one noise model and evaluate at a harsher one. This probes the distribution-shift weakness noted for neural warm-starters.

Real-robot test. Given access to a Franka Panda, we run the SceneReplica protocol and compare against the 65% success and 15.4 s reported by Yu et al.. A convincing result would be a large reduction in planning time without lower success.