TL;DR
CS-Pi (Constraint-Satisfying Flow Policy Sampling) is a novel inference-time sampler for flow-matching robot policies that enforces safety constraints without the need for retraining. By iteratively projecting actions onto a safe set and "renoising" them back into the policy's distribution, it generates motions that are both guaranteed to be safe and consistent with the smooth, plausible behaviors learned from demonstrations.
What Problem Does This Paper Address?
Large-scale generative models, like diffusion and flow-matching policies, have become powerful "generalist priors" for robot manipulation. However, these models are trained on fixed datasets that cannot anticipate every possible deployment scenario—like a new obstacle placed in the robot's path.
When a robot encounters a new constraint (e.g., "don't touch the wine glass"), we have a few traditional options, all with significant drawbacks:
- Rejection Sampling: Keep drawing samples until one happens to be safe. This is perfectly "plausible" (it comes directly from the policy) but is computationally intractable if safe actions are rare.
- Projection: Take a sample and mathematically "snap" it to the nearest safe point. This is efficient but often results in jerky, unrealistic, or task-irrelevant motions that the policy would never naturally produce.
- Gradient Guidance: Use the gradient of a cost function to "steer" the denoising process. This often fails because it relies on a "proxy" estimate of the final action. If the policy is uncertain (e.g., should I go left or right of the glass?), the proxy is the average of those two paths—which might go straight through the obstacle.
The challenge is to find actions that are both safe (satisfy the constraint) and plausible (likely under the original policy).
Methods
The authors introduce CS-Pi, a "draw-project-renoise" loop. Instead of guiding the internal steps of the generative process, CS-Pi acts on the final output and then "backtracks" intelligently.
The algorithm follows these steps:
- Draw: Generate a candidate action chunk from the flow-matching policy.
- Project: Map this action to the nearest point in the "safe set" (the set of actions satisfying the constraint).
- Renoise: Instead of executing this potentially "weird" projected action, add noise back to it. Crucially, the amount of noise is scaled by the projection residual (how far the action had to be moved to become safe). If it was almost safe, add a little noise; if it was far off, add a lot.
- Re-denoise: Run the policy again starting from this intermediate noise level. This allows the policy to "fix" the action, making it plausible again while staying near the safe region.
- Repeat: Iterate until the policy naturally produces a safe action.
This approach also extends to continuous costs (like preferring a specific gripper orientation) by replacing the projection step with a "proximal operator" that balances cost reduction with staying close to the current sample.
Datasets
The authors evaluated CS-Pi across several challenging benchmarks:
- 2D AVOIDANCE: A planar navigation task with multimodal paths and new obstacles added at test time.
- 7-DoF Shelf Pick-and-Place: A simulated Franka robot picking cubes from a shelf with obstacles that weren't there during training.
- 3D-ORIENT: A reaching task with a continuous cost preference for a specific end-effector orientation.
- SPHERE-AVOID: A pure obstacle avoidance benchmark in 3D joint space.
- Real-world Franka Deployment: A zero-shot deployment using a VLA policy (trained on the DROID dataset) to pick an apple and place it on a plate amidst obstacles, using vision-based bounding boxes.
Key Insights and Ideas
- Flow-matching as a Resampler: The key "aha" moment is that flow-matching allows for efficient resampling from any intermediate noise level. This lets us use a projection as an "anchor" to guide the policy without being stuck with the projection's potentially poor quality.
- Adaptive Noise Levels: By tying the renoising level to the projection residual, the algorithm automatically balances exploration and exploitation. If the policy's current "mode" is blocked, the high residual adds enough noise to let the policy switch to a different, safer mode (e.g., going around the other side of an obstacle).
- Avoiding Proxy Gradients: Unlike guidance methods, CS-Pi only evaluates constraints on "clean" actions. This makes it much more robust to multimodality, where gradient-based methods often get confused by the "average" of different possible paths.
Key Results
CS-Pi significantly outperformed baselines in both safety and plausibility:
- Shelf Task: CS-Pi achieved an 81.3% safe success rate, compared to just 55.3% for simple projection and 32.0% for gradient guidance.
- Plausibility: In the 2D task, CS-Pi kept 85.7% of trajectories "on-support" (within the distribution of demonstrations), while projection's likelihood scores collapsed.
- Hardware: In zero-shot real-world tests, CS-Pi reached 55% safe success, outperforming projection (40%) and guidance (25%). It was the only method that consistently stayed close to the original policy's behavior while avoiding collisions.
Limitations and Future Work
- Convergence: While empirically successful, the paper does not provide a formal proof that the loop will always converge to a safe and plausible action.
- Myopia: Like most current policies, CS-Pi operates on "action chunks." It might commit to a path that is safe for the current chunk but leads to an unavoidable collision in the future. Trajectory-level planning remains a separate challenge.
- Perception Dependency: The system is only as good as the safety constraints provided. Errors in vision-based obstacle detection or segmentation can still lead to failures.
- Computational Cost: While much faster than rejection sampling, the iterative nature of CS-Pi means it requires more network passes (NFE) than a single-shot projection.
Summary updated