CS-Pi: Constraint-Satisfying Flow Policy Sampling

Introduction

Large-scale flow-matching and diffusion policies have become capable generalist priors for robot manipulation[1, 2]: trained once on broad demonstrations, a single model can pick and place objects, open drawers, and fold clothes. Yet no training set anticipates every deployment, and such a policy will inevitably meet safety-critical scenarios its demonstrations never contained, for example: a wine glass left beside the drawer it must open, which the arm must not touch. This requirement is a constraint, supplied only at deployment. It defines a safe set: the set of actions that satisfy it; in our example, every action that does not touch the glass. Our goal is to sample actions that both come from the policy and lie in the safe set, without retraining the policy for each new constraint. The natural approach is to correct the policy’s output: sample an action and project it onto the safe set, moving it to the nearest safe action, or steer the denoising trajectory using a function of the constraint. Such corrections address the second half of this goal, moving the action into the safe set. They can lose the first: the correction can move the action somewhere the policy would rarely sample. This matters because the policy is what captures the task. An action it considers unlikely may clear the wine glass yet no longer reach the drawer handle. Such an action also loses what the policy learned from the demonstrations, beyond the task itself: smooth, kinematically feasible motion. We therefore require the sampled action to be both safe, satisfying the constraint, and plausible, remaining likely under the policy.

Among such training-free samplers, the most common is gradient guidance [3, 4, 5], which nudges each denoising step along the gradient of a cost on the action. Guidance has two shortcomings. First, safety constraints are typically hard constraints: an action either satisfies them or violates them, with no degree in between. An action either touches the wine glass or it does not; this yes-or-no answer says nothing about which way to move the arm, so a hard constraint offers no gradient to follow. Guidance must instead use a smooth cost, such as a collision cost that grows as the arm nears the glass, which discourages contact but does not rule it out. Second, even a smooth cost is meaningful only on a clean action, while guidance acts on noisy intermediate states. Guidance therefore evaluates the cost at a proxy: the policy’s running estimate of the clean action, an average over every clean action the noisy state could still become. This proxy is reliable only when the cost changes slowly across those actions, so sharpening the cost toward the true constraint makes guidance less reliable, not more. The proxy fails when the policy is uncertain between distinct modes, such as passing left or right of the glass. The average of the two can pass straight through the glass, an action the policy would never sample. The gradient evaluated there may point towards neither mode (App. § 10).

We instead build on rejection and projection, two classical mechanisms that act only on the final sampled action rather than guiding the intermediate noise levels. Rejection sampling redraws complete actions from the policy until one satisfies the constraint: every output is plausible, but the wait is intractable when safe actions are rare. Projection maps a sampled action to the nearest constraint-satisfying point: it is efficient, but the projected action need not be plausible and can be jerky or task-irrelevant. Our key insight is that flow-matching sampling lets us combine the two, keeping projection’s efficiency and rejection’s plausibility. When a projected action is implausible, we need not take rejection’s drastic step of restarting sampling from pure noise; we can instead resample from a reduced noise level around the projection, so the policy returns a safe and plausible action near the projection.

We operationalize this insight in CS-Pi, a draw-project-renoise sampler (Figure 2). CS-Pi starts from a single draw from the policy and refines it in a loop. Each iteration projects the current draw onto the safe set and, rather than executing the possibly implausible projection, adds noise back to it, scaled to how far the projection moved the draw. It then re-runs the unmodified policy from that noise level to obtain the next draw. The loop ends once a draw is safe. Each draw comes from the policy and tends to land closer to the safe set than the last, so the draws approach the safe set while remaining plausible, at a compute cost far below rejection and with no constraint gradient at any noisy state. We first present CS-Pi for hard constraints, then extend it to continuous costs, such as an orientation preference, by replacing only the projection step. The same change lets CS-Pi combine hard constraints with costs. Across a 2D diagnostic, a 7-DoF shelf pick-and-place with deployment-time obstacles, the orientation preference above, and a zero-shot Franka deployment, CS-Pi is the only sampler that satisfies the constraint while staying plausible wherever the constraint permits; on the shelf it reaches 81% safe success (completing the task without collision) against 55% for projection and 32% for gradient guidance.

Related Work

Gradient guidance. In the robotic settings we consider, constraints and preferences are typically expressed as analytic costs on the action, such as a signed-distance collision cost, a joint-limit penalty or an orientation error. The standard way to steer a diffusion or flow-matching policy [6] toward low cost at deployment is gradient guidance, which nudges each denoising step along the gradient of that cost: classifier-free guidance [4, 7], DPS [8], ΠGDM [9], MPGD [10], AdjointDPM [11], Diffuser [3], and Lan-o3dp [12]. But the cost is meaningful only on a clean action, so each method evaluates it at a proxy, the policy’s running estimate of the clean action. The proxy is accurate when the cost changes slowly and the policy has settled on one mode, and it breaks down otherwise, pushing the sample toward implausible actions (Appendix § 10). Trust Sampling [13] takes several guidance steps per denoising step, capping their number by how far the proxy can be trusted at that noise level and stopping early when the sample leaves the region the policy was trained on. It shares our aim of keeping samples plausible, but every step still follows the proxy’s gradient.

Rejection, reweighting, and projection. A constraint can also be met without following the proxy’s gradient, by acting on the policy’s clean actions. Rejection sampling redraws complete actions until one is safe: every output is plausible, but the wait is intractable when safe actions are rare. Sequential Monte Carlo (SMC) samplers reweight and resample a population of particles rather than discarding them (the Twisted Diffusion Sampler [14], Feynman–Kac samplers [15, 16]); they need more particles as the safe set shrinks, and the weight that steers each particle at a noisy step is still evaluated at the proxy (Appendix § 10). Projection moves a sample to the nearest safe action without any cost gradient, either at every denoising step [17] or once, on the final action, which is then executed; in both cases the projected action need not be plausible. CS-Pi also avoids the proxy’s gradient but combines rejection and projection: it projects the final action, then renoises the projection rather than executing it and re-denoises it with the policy, returning a plausible draw closer to the safe set without rejection’s wait or SMC’s particle budget.

Control-barrier-function samplers. SafeDiffuser [18] enforces forward invariance by solving a control-barrier-function (CBF) quadratic program at each denoising step; SafeFlowMatcher [19] alternates flow prediction with barrier-based correction; and SAD-Flower [20] augments a flow with a virtual safe control input and a learned dynamics model. These methods are designed to certify safety under their respective assumptions, but the certificate says nothing about plausibility. Their corrections continuously deform the current sample: when a constraint blocks the mode the sample has committed to, a correction can push it out of the unsafe region but does not redraw it from another mode of the policy. CS-Pi instead projects, renoises and re-denoises with the policy, which allows such a mode switch while keeping each draw plausible.

CBF samplers also require a differentiable barrier with the structure needed to evaluate its Lie derivatives and relative degree; SAD-Flower additionally assumes a learned dynamics model. CS-Pi requires only a projection onto the safe set, which enables its zero-shot use with existing policies, such as $\pi_{0.5}$, and obstacle bounding boxes estimated from vision (§ 4.4). We test SafeDiffuser and SafeFlowMatcher on the same policies and report them in Tables 1 and Table 2 and App. § 7. SAD-Flower is not evaluated as it requires a learned dynamics model, which our setups do not provide.

Method	Safe-SR (%) $\uparrow$	Goal reaching (%) $\uparrow$	Collision (%) $\downarrow$	$\log L \uparrow$	NFE $\downarrow$
Unconstrained	3.3	75.3	80.0	15.02	240
Gradient guidance ($\alpha=0.20$)	32.0	32.0	0.7	10.42	240
Trust Sampling ($w=0.1$, $J=5$)	53.3	60.7	12.0	12.23	1104
Rejection ($B=100$)	34.7	68.0	38.7	14.22	8745
Projection	55.3	58.7	6.0	11.41	240
SafeDiffuser	13.3	16.7	19.3	$7.50^{\dagger}$	240
SafeFlowMatcher	44.7	48.7	8.0	10.90	432
CS-Pi ($k=50$, $K_{\max}=5$)	81.3	84.7	3.3	17.96	271

Deployment-time safety for vision-language-action models. Recent work shares our practical motivation of enforcing constraints on a pretrained vision-language-action (VLA) policy at deployment. OmniGuide [5] steers flow-matching VLAs [2, 21] toward collision avoidance and semantic targets with gradient guidance on differentiable energy fields, and thus evaluates its cost at the proxy. VLSA [22] instead acts once, on the final action, much as projection does: a CBF filter changes the VLA’s output action as little as possible to satisfy the barrier condition, and the robot executes the filtered action. Its authors report that this filter causes a safety-induced distribution shift. Our hardware experiment uses a similar setup, a $\pi_{0.5}$ policy on a Franka arm, and there CS-Pi improves on both gradient guidance and projection (§ 4.4).

Partial renoising. Adding noise to an input and denoising it again is an established way to make an edited input plausible under a generative model. SDEDIT [23] renoises a user’s stroke painting and denoises it into a realistic image; RePaint [24] renoises around known pixels to inpaint the rest; and DenoDiffusion [25] reuses a human demonstration retargeted to the robot into actions the policy would plausibly produce. In each, the renoised input is given in advance and the amount of noise is set by hand, once. CS-Pi instead renoises the policy’s own draw after projecting it onto the safe set, sets the amount of noise from how far the projection moved the draw, and repeats until a draw is safe. This turns a one-shot edit into a loop that drives the draw toward the safe set (§ 3).

Approach

We want to sample from a flow-matching policy under a constraint supplied only at deployment time. The action we draw must satisfy two equally important requirements: it must be safe, satisfying the constraint, and plausible, remaining likely under the policy. An action the policy would essentially never draw, even one that satisfies the constraint, may be jerky, kinematically infeasible, or task-inappropriate.

Setup and notation. The policy $\pi_{\theta}$ generates a clean action $\mathbf{a} \in\mathbb{R}^{D}$ by integrating a learned velocity field along an ODE, from Gaussian noise $\mathbf{a}_{0} \sim\mathcal{N}(\mathbf{0}, I)$ at time $t = 0$ to the clean action at $t = 1$. We write $\mathbf{a} = \pi_{\theta}(\mathbf{a}_{n}; n)$, integrating from an intermediate state $\mathbf{a}_{n}$ at time $t = n \in[0,1]$ to $t = 1$, so an ordinary draw is $\mathbf{a} = \pi_{\theta}(\mathbf{a}_{0}; 0)$ with $\mathbf{a}_{0} \sim\mathcal{N}(\mathbf{0}, I)$, abbreviated $\mathbf{a} \sim\pi_{\theta}$; we overload $\pi_{\theta}(\mathbf{a})$ for the marginal density it induces over clean actions. At deployment we are given a safe set $\mathcal{C} \subseteq\mathbb{R}^{D}$, absent at training, through its Euclidean projection $\mathcal{P}(\mathbf{a}) = \operatorname*{arg\,min}_{\mathbf{a}' \in\mathcal{C}} \lVert\mathbf{a}' - \mathbf{a} \rVert$, which is closed-form or a cheap iterative solve for the constraint we consider. Our goal is to draw $\mathbf{a} \sim\pi_{\theta}$ with $\mathbf{a} \in\mathcal{C}$. We generalize our method to admit continuous cost functions at the end of the section.

Overview. We want samples that are both safe and plausible. Rejection sampling and projection are two classical approaches, each achieving one of these: rejection redraws unmodified policy samples until one is safe, so every draw is plausible, but almost all are wasted when safe actions are rare; projection maps the clean sample to the nearest point in the safe set, so every action is safe, but the projected action is one the policy is unlikely to produce. Our insight is to strike a balance between the two, projection’s efficiency and rejection’s plausibility, by using the projection not as the action to execute but as an anchor for the next draw. Concretely, we draw a clean action, project it onto $C$, renoise the projection to a noise level set by how far the projection moved the draw (the projection residual), and re-denoise from there; we iterate until a draw is itself safe.

From projection to a resampling target. Projection gives us a useful starting point for the next draw. The projected sample $\mathbf{a}_c^{(i)} = \mathcal{P}(\hat{\mathbf{a}}^{(i)})$ of the current clean draw $\hat{\mathbf{a}}^{(i)}$, which we call the anchor, is cheap to compute and safe by construction, and it sits closer than $\hat{\mathbf{a}}^{(i)}$ to the safe actions the policy is likely to draw. It therefore tells us where the next draw should concentrate, i.e. near $\mathbf{a}_c^{(i)}$, without executing $\mathbf{a}_c^{(i)}$. The corresponding idealized target biases the policy toward $\mathbf{a}_c^{(i)}$ while preserving its own structure:

Drawing from Eq. (1), projecting the result, and recentering the next target on that projection defines one iteration of a loop that terminates when a draw lands in $C$.

Setting $\sigma^{(i)}$ from the projection residual. We set $\sigma^{(i)}$ from the projection residual $\delta^{(i)} = \lVert\hat{\mathbf{a}}^{(i)} - \mathbf{a}_c^{(i)}\rVert$ itself, because $\delta^{(i)}$ already encodes how much trust to place in $\mathbf{a}_c^{(i)}$ as an anchor. A small $\delta^{(i)}$ means the current draw was nearly safe, so plausible actions likely lie near $\mathbf{a}_c^{(i)}$ and the next draw should concentrate there. A large $\delta^{(i)}$ means the current draw was far from $C$, so $\mathbf{a}_c^{(i)}$ may sit in a region the policy assigns vanishing mass; the next draw should be loose enough to give a different policy mode (e.g., passing right of an obstacle rather than left) a chance to be reached. We therefore set $\sigma^{(i)} = \delta^{(i)}/k$ for a positive coefficient $k$, the only hyperparameter introduced in the loop. The hyperparameter $k$ slides between the two extremes: $k \to0$ gives $\sigma^{(i)} \to\infty$ and recovers rejection, while $k \to\infty$ gives $\sigma^{(i)} \to0$ and recovers projection.

Realising the target via partial renoising. Sampling from Eq. (1) for an arbitrary $\sigma^{(i)}$ is intractable in general as the policy density $\pi_\theta(\mathbf{a})$ is implicit, defined only through the ODE, and its product with a Gaussian has no closed form. We therefore renoise the projected point to the intermediate time $n^{(i)} = k/(k + \delta^{(i)})$,

and re-denoise from $\mathbf{a}_n^{(i)}$. This choice of $n^{(i)}$ ties the amount of noise to $\sigma^{(i)}$: $\left(1 - n^{(i)}\right)/n^{(i)} = \delta^{(i)}/k = \sigma^{(i)}$, so a larger residual adds more noise. Appendix § 11 states the exact rule for the next draw under this construction and its scope.

Iterating to safety. We pass each new draw back as the next $\hat{\mathbf{a}}$: project, renoise, re-run the policy, repeat, until the draw lands in $C$. We refer to this draw-project-renoise loop as Constraint-Satisfying Flow Policy Sampling (CS-Pi; Figure 2), stated in Algorithm 1; it terminates when $\delta\le\tau$, i.e., once a draw lies within $\tau$ of $C$, after a cap of $K_{\max}$ iterations. On average each successive draw lands closer to the safe set (Appendix § 13), so the residual $\delta$ shrinks (resulting in a larger $n$) with each iteration, bringing the current draw closer to a safe and plausible draw.

	Constraint-Satisfying Flow Policy Sampling
1:	Draw $\mathbf{a}_0 \sim\mathcal{N}(\mathbf{0}, I)$; $\hat{\mathbf{a}}^{(0)} \leftarrow\pi_\theta(\mathbf{a}_0; 0)$	▷ initial policy draw
2:	for $i = 0, \ldots, K_{\max} - 1$ do
3:	$\mathbf{a}_c^{(i)} \leftarrow\mathcal{P}(\hat{\mathbf{a}}^{(i)}); \ \delta^{(i)} \leftarrow\lVert\hat{\mathbf{a}}^{(i)} - \mathbf{a}_c^{(i)}\rVert$
4:	if $\delta^{(i)} \le\tau$ then
5:	return $\hat{\mathbf{a}}^{(i)}$
6:	end if
7:	$n^{(i)} \leftarrow k/(k + \delta^{(i)}); \ \boldsymbol{\epsilon}^{(i)} \sim\mathcal{N}(\mathbf{0}, I)$
8:	$\mathbf{a}_n^{(i)} \leftarrow n^{(i)}\mathbf{a}_c^{(i)} + (1 - n^{(i)})\boldsymbol{\epsilon}^{(i)}$
9:	$\hat{\mathbf{a}}^{(i+1)} \leftarrow\pi_\theta(\mathbf{a}_n^{(i)}; n^{(i)})$	▷ re-denoise from time $n$
10:	end for
11:	return $\mathcal{P}(\hat{\mathbf{a}}^{(K_{\max})})$	▷ optional terminal projection

Incorporating soft preferences as continuous costs. So far we have enforced a hard constraint, i.e. an action either lies in $C$ or it does not. Deployment also brings softer preferences, behaviors to favor rather than boundaries to clear, such as keeping the gripper near-vertical while carrying a full cup (§ 4.3), and we generalize the same loop to express these preferences alongside hard constraints. This generalization needs only one change: CS-Pi uses the projection only to produce an anchor for the next draw; for a continuous cost $L$, that anchor comes instead from the proximal operator

which trades reducing $L$ against staying close to the current draw $\hat{a}$, with $\beta$ setting the balance; the rest of the algorithm is unchanged. Hard constraints and continuous costs are then composed by being applied in sequence to the same clean draw, with the operators encoding priority by their order. For example, CS-Pi can first pull toward the preferred orientation with the proximal operator and then project onto the safe set, so the constraint takes precedence. A gradient method would instead have to merge both into a single objective.

Optional terminal projection. If no draw lies within $\tau$ of $C$ after $K_{\max}$ iterations, CS-Pi can project the last draw and execute the result, trading plausibility for safety when safety must come first. Even then the loop pays off: in simulation, the executed actions are more likely under the policy than a typical one-shot projection. App. § 15 studies how often the terminal projection is triggered and how likely the resulting actions are.

Experiments

Throughout, we measure two things for each sampler: whether it satisfies the constraint and whether its samples are plausible. We evaluate four settings: a 2D navigation task where the whole action distribution is easy to visualize, a 7-DoF shelf pick-and-place, an orientation-preference cost (3D-ORIENT), and a real-world Franka deployment; App. § 7 adds a 3D obstacle-avoidance benchmark, SPHERE-AVOID.

Our primary metric is the safe success rate (Safe-SR): the fraction of rollouts that satisfy the constraint and complete the task. Plausibility is measured by $\log L$, the mean log-likelihood $\pi_{\theta}$ assigns to a method’s actions (nats/dim; higher is more plausible). Compute is reported as NFE, the flow-network forward passes per rollout (not comparable across benchmarks). We compare against gradient guidance, Trust Sampling (step size $w$, at most $J$ guidance steps per denoising step), rejection sampling (redraw budget $B$), projection, the unconstrained policy, and two CBF samplers (SafeDiffuser [18] and SafeFlowMatcher [19]), each reported at the best value of its main hyperparameters (App. § 12). CS-Pi has a single hyperparameter, $k$, and is robust across a broad range of it (App. § 17). It uses the terminal projection on every obstacle benchmark but not on 3D-ORIENT, where the target is a preference.

2D Avoidance: constraint satisfaction without distribution shift

AVOIDANCE is a 2D planar environment in which an agent travels from a fixed start position to a goal region. We collect demonstrations that reach the goal along distinct paths, so the policy is multimodal, mirroring real manipulation data where demonstrations of one task take different routes or have different styles. Because the action space is 2D, we can plot each sampler’s full distribution. Plausibility is measured by how many of the rollouts stay inside the demonstration support: the part of the plane the demonstrations pass through. At deployment we impose obstacles the policy never saw, in six scenarios of varying difficulty. Figure 3 shows rollouts for one scenario; the remaining five are in App. § 9.

Three methods reach at least 99% safe success: SafeDiffuser, projection and CS-Pi (Table 1). Among them, CS-Pi keeps the most rollouts fully on-support (0.857) and has the highest log-likelihood (1.53), while projection’s log-likelihood collapses to −85.89. Gradient guidance, Trust Sampling and rejection stay plausible but reach at most 79.4% safe success.

Composing a hard constraint with a soft preference. A safe set cannot express a preference among feasible actions. On the same 2D AVOIDANCE environment we add to the obstacle constraint a soft preference toward a height $y_{\mathrm{ref}}=0.7$. CS-Pi applies the two in sequence by priority, whereas the baselines must fold them into one weighted cost; only CS-Pi satisfies both (App. § 9, Figure 8).

Method	Safe-SR (%) ↑	fully-in-supp. ↑	logL ↑	NFE ↓
Unconstrained	5.2	0.800	1.66	456
Gradient guidance	67.2	0.410	1.49	298
Trust Sampling	34.9	0.350	1.43	4421
Rejection	79.4	0.750	1.66	73921
Projection	99.8	0.640	−85.89	52
SafeDiffuser	100.0	0.830	1.44	92
SafeFlowMatcher	45.2	0.000	−463	220
CS-Pi	99.0	0.857	1.53	283

Shelf pick-and-place with deployment-time obstacles

The shelf benchmark tests whether a sampler can satisfy a deployment-time obstacle constraint without discarding the task semantics encoded by the policy. A 7-DoF Franka picks a 4 cm cube from a three-rung shelf and places it on the table; at inference we add obstacles absent at training (App. § 8). A rollout is safe and successful if it is collision-free, picks the cube, and places it. CS-Pi and projection share the same L-BFGS [26] proximal step, so any gap between them is due to the iterative loop of CS-Pi.

CS-Pi reaches the highest safe success rate (81.3%; Table 2) and is the only method whose actions score above the unconstrained policy in mean log-likelihood; projection and the gradient baselines satisfy the obstacle only by making the chunk implausible. CS-Pi, projection, gradient guidance and SafeFlowMatcher collide on at most 8% of rollouts, so among them performance depends mainly on whether the gripper still reaches the cube. Projection and the gradient baselines avoid the obstacles by moving the arm away from them, and often away from the cube as well; CS-Pi instead re-denoises a trajectory that passes between the obstacles and still reaches the cube (84.7% goal reaching, above even the unconstrained policy’s 75.3%).

3D-Orient: reaching with an orientation preference

3D-Orient tests CS-Pi on a continuous cost. The task is a 7-DoF reach to a goal sampled anywhere in the robot’s reachable workspace; on top of it we add a soft preference to hold a top-down orientation $R^*$, encoded as the cost $L(q)=\sum_{t=1}^{H}\lVert R(q_t)-R^*\rVert_F^2$. The test is whether a method can achieve the orientation without giving up the reach. Projection, rejection and the CBF samplers require a safe set and do not apply to a continuous cost.

CS-Pi is the only method that does both (Table 3): 65.6% success, with goal reaching (98.9%) and end-effector step (0.07 m) matching the unconstrained policy. Gradient guidance loses the reach and roughens the path (< 7% success); Trust Sampling keeps the reach and a high log-likelihood only by barely changing the orientation (1.1% success). CS-Pi’s lower log-likelihood (10.21 vs. 13.46) is the cost of moving the gripper away from the policy’s preferred orientations. Guidance fails because of IK redundancy: many configurations reach the same point at different orientations, so the proxy averages them into an invalid configuration with an uninformative gradient (App. E).

Method (best cfg)	Success $\uparrow$	Goal reaching $\uparrow$	$\log L \uparrow$	Max EE step (m)	NFE $\downarrow$
Unconstrained	2.2%	98.9%	13.46	0.07	138
Gradient guidance ($\alpha=0.003$)	2.2%	70.0%	12.30	0.08	154
Gradient guidance ($\alpha=0.03$)	6.7%	7.8%	9.62	0.40	160
Trust Sampling ($w=0.001$)	1.1%	98.9%	13.44	0.07	1379
CS-Pi + EE-prox ($k=10$, $K_{\max}=3$)	65.6%	98.9%	10.21	0.07	210

Real-world deployment

We evaluate CS-Pi on a Franka arm with a Robotiq 2F-85 gripper, ZED 2 external and ZED Mini wrist cameras, and a DROID-finetuned $\pi_{0.5}$[27] policy used without further finetuning. The task is pick up the apple and place it on the blue plate. A vision pipeline conditioned on the ZED 2 RGB and depth images generate obstacle bounding boxes that form the constraint $C$; deployment is zero-shot. We test over two layouts (Figure 4), 10 trials per method per layout, and compare CS-Pi, gradient guidance, and projection against the unguided policy. Table 4 reports the safe-success rate, per-step latency, and the log-likelihood the frozen prior assigns to the executed actions.

CS-Pi reaches the highest safe success rate in both layouts (60% and 50%) and averaged over both (55%), versus 40% for projection, 25% for gradient guidance, 20% for Trust Sampling, and 10%

for the unconstrained policy. Its per-step latency (0.143 s) matches gradient guidance (0.139 s) and Trust Sampling (0.135 s); projection is cheaper (0.099 s) but enforces $C$ with a one-shot projection that drops its log-likelihood below the unconstrained policy’s (−473.2 versus −470.2), whereas CS-Pi stays close to it (−471.8) while satisfying the constraint. The runs are small-sample (10 trials per layout), but CS-Pi leads in both layouts.

Conclusion

CS-Pi samples safe and plausible actions from a flow-matching policy with a draw-project-renoise loop: it projects each draw onto the safe set, adds noise back scaled to how far the projection moved the draw, and re-runs the unmodified policy. It needs no constraint gradient at noisy states and no auxiliary network. CS-Pi also extends to continuous costs by replacing only the projection step with a proximal operator. Across a 2D diagnostic, a 7-DoF shelf pick-and-place, an orientation preference, and a zero-shot Franka deployment, CS-Pi is the only sampler that satisfies the constraint while staying plausible wherever the constraint permits. On hardware it reaches 55% safe success against 40% for projection and 25% for gradient guidance. A policy can thus meet a safety constraint it never trained on, without surrendering the behaviour that made it useful.

Limitations

Our theory covers a single iteration, we characterize the per-iteration sampling distribution, the distribution of the next draw given the current one (App. § 11), but we do not prove that the loop converges. In practice, CS-Pi does not always reach an action that is both safe and plausible. Therefore, when no draw lies within $\tau$ of $C$ after $K_{\max}$ iterations, it must choose between the last draw, which may be unsafe, and the terminal projection, which is safe but often less likely under the policy (App. § 15).

CS-Pi also needs a usable projection $\mathcal{P}$ (or proximal operator $\operatorname{prox}_{\beta L}$): the obstacle and joint-limit sets we use admit closed-form or cheap iterative solutions, but more complex sets need more complex solvers. It is also only as good as $C$: on hardware we estimate $C$ from vision once per rollout and evaluate it in static scenes, so dynamic obstacles and errors from segmentation, depth and registration can misplace or mis-size $C$. Finally, like every sampler we evaluate, CS-Pi operates on action chunks, the fixed-length sequences of actions a policy predicts at each query, and is therefore myopic: when the policy commits to a mode that is safe for the current chunk but has no safe continuation, CS-Pi cannot recover; trajectory-level planning is out of scope.

Project page

Qualitative videos of the 2D, simulated, and hardware rollouts, along with additional visualizations, are available at the project page:

https://babayaga840.github.io/CS-pi_site/

3D SPHERE-AVOID: pure obstacle avoidance

This benchmark complements the shelf result (§ 4.2) by isolating obstacle avoidance, with no task-semantic tension between the constraint and the policy. SPHERE-AVOID uses a simulated 7-DoF Franka Panda and a joint-space action: a flow-matching policy moves between a start and a goal configuration without colliding with sphere obstacles. The safe set $C$ is the collision-free set of the swept-sphere geometry plus joint limits. We test two layouts: L1 (10 spheres) and L2 (16 spheres). The policy was trained on demonstrations from both layouts.

Vector (PDF)

CS-Pi with the terminal projection reaches 99.9% safe success on L1 and 91.8% on L2, the highest in both layouts, and of the methods that enforce the constraint it loses the least from L1 to L2 (8.1 points, against 26.1 for projection; Table 5). The strongest baselines are projection (88.2% / 62.1% on L1 / L2) and Trust Sampling (77.0% / 38.7% at 64–69 NFE), followed by SafeFlowMatcher (61.8% / 21.9%), rejection (56.7% / 14.1% at 388–492 NFE), SafeDiffuser (36.4% / 7.4%), gradient guidance (23.6% / 4.7%) and the unconstrained policy (8.9% / 0.8%). CS-Pi uses 20–26 NFE. Its log-likelihood matches the unconstrained policy on L1 (2.87 vs. 2.88) and is 0.30 lower on L2; rejection has the highest log-likelihood on both layouts.

Method	L1 (10 spheres)			L2 (16 spheres)
	Safe-SR	logL	NFE	Safe-SR	logL	NFE
Unconstrained	8.9	2.88	10	0.8	2.84	10
Gradient guidance ($\alpha=0.01$)	23.6	2.73	10	4.7	2.32	10
Trust Sampling ($w=0.3$, $J=5$)	77.0	3.03	64	38.7	2.68	69
Rejection	56.7	3.47	388	14.1	3.19	492
Projection	88.2	2.67	10	62.1	2.28	10
SafeDiffuser	36.4	2.51{sup}†`	10	7.4	1.83{sup}†`	10
SafeFlowMatcher	61.8	2.30	12	21.9	1.82	12
CS-Pi ($k=50$, $K_{\max}=20$)	99.9	2.87	20	91.8	2.54	26

Shelf task scenarios

Method	Clutter	Pillar	Flankers	Mid-rung	Low-rung	Mean
Unconstrained	3.3	3.3	3.3	6.7	0.0	3.3
Gradient guidance	16.7	70.0	16.7	33.3	23.3	32.0
Rejection	20.0	83.3	20.0	33.3	16.7	34.7
Projection	40.0	63.3	46.7	66.7	60.0	55.3
SafeDiffuser	6.7	33.3	3.3	23.3	0.0	13.3
SafeFlowMatcher	43.3	46.7	53.3	56.7	23.3	44.7
CS-Pi	80.0	93.3	83.3	76.7	73.3	81.3

Additional 2D Avoidance results

Composing a hard constraint with a soft preference. On the cluttered scenario we add to the obstacle constraint the preference $L(a) = (y-y_{\mathrm{ref}})^2$ with $y_{\mathrm{ref}}=0.7$. CS-Pi applies the proximal step for the preference and then the projection, so the constraint takes precedence; gradient guidance and Trust Sampling descend a single weighted sum of the collision cost and the preference. A rollout meets both if it is collision-free and its mean height error $|y_t-0.7|$ is below 0.05. CS-Pi meets both on every rollout for $k \in\{30, 100, 200\}$; no setting of gradient guidance or Trust Sampling meets both (Table 8, Figure 8): low settings collide and high settings stay far from $y_{\mathrm{ref}}$. Projection alone and rejection cannot target a preference and are omitted.

Method	Setting	Both met (%)	Collision (%)	Mean $|y-0.7|$	NFE
Unconstrained	–	0	35.3	0.195	10
Gradient guidance	$\alpha=0.1$	0	31.3	0.194	10
	$\alpha=0.3$	0	28.0	0.193	10
	$\alpha=1$	0	19.3	0.189	10
	$\alpha=3$	0	0	0.178	10
	$\alpha=10$	0	0	0.150	10
Trust Sampling	$w=0.01$	0	25.3	0.190	70
	$w=0.05$	0	0	0.166	70
	$w=0.1$	0	0	0.135	70
CS-Pi	$k=1$	0	0	0.188	110
	$k=10$	0	0	0.099	82
	$k=20$	0	0	0.065	63
	$k=30$	100	0	0.043	45
	$k=100$	100	0	0.010	23
	$k=200$	100	0	0.007	21

Why gradient guidance fails

Gradient guidance steers a flow-matching policy toward a cost $L$ defined on the clean action $a_1$, such as a collision cost or an orientation error. During sampling the integrator only ever holds a partially denoised state $a_t$ at time $t \in(0,1)$, and the cost of a noisy state carries no information about the cost of the clean action it will become: $L(a_t)$ and $L(a_1)$ are unrelated. Guidance therefore cannot evaluate the cost at $a_t$. It must instead predict the clean action the current state will denoise into and evaluate the cost there. This requirement, forced by the cost being defined on $a_1$, is the root of the failure.

The guidance direction one would want. Guidance aims to sample from the tilted distribution $p^*(a) \propto\pi_\theta(a)\exp(-\beta L(a))$, which concentrates the policy on low-cost actions with strength $\beta$. The exact correction this adds to the velocity at $a_t$ is the score of the tilt, $\nabla_{a_t}\log\mathbb{E}_{p(a_1\mid a_t)}[\exp(-\beta L(a_1))]$. To first order in $\beta$ this reduces to the posterior-mean gradient

the cost gradient averaged over every clean action the noisy state could still become, weighted by the denoising posterior $p(a_1 \mid a_t)$. Equation (4) is the guidance one would want: it accounts for the policy’s remaining uncertainty about $a_1$.

The Jensen swap. The expectation in Eq. (4) is intractable. The posterior $p(a_1 \mid a_t)$ is defined only implicitly through the flow and has no closed form. Every gradient-guidance method makes the same substitution: it moves the gradient outside the expectation,

replacing the expected gradient by the gradient at the expected action, the proxy $\bar{a}(a_t) = \mathbb{E}[a_1 \mid a_t]$. This is exact only when $\nabla L$ is affine over the support of the posterior, that is, when $L$ is quadratic there. For a general cost the two sides differ by the Jensen gap; a second-order expansion of $L$ about $\bar{a}$ gives

and differentiating leaves a drift error of the same character: it scales with the curvature of the cost and with the spread of the denoising posterior. Both factors are largest exactly where guidance must act. The covariance $\operatorname{Cov}(a_1 \mid a_t)$ is wide at high noise, before the policy has committed to an action; and a hard constraint is the limit of a cost that is $0$ on $C$ and unbounded off it, whose curvature diverges at the boundary.

The proxy is the Tweedie mean. The proxy $\bar{a}(a_t) = \mathbb{E}[a_1 \mid a_t]$ is the posterior mean of the clean action (the Tweedie estimate); for a flow policy it is recovered by a single integration step from $a_t$ to $t = 1$. When the posterior is unimodal and narrow, the proxy is accurate and the swap is benign. When the policy is still uncertain between distinct clean actions, the posterior is multimodal and its mean lies between the modes, at an action the policy assigns almost no mass and that need not be valid. The gradient $\nabla L(\bar{a})$ is then computed at a point on no mode. No choice of $\beta$ or step size repairs this, because the error is in where the gradient is evaluated, not in its magnitude.

3D-ORIENT: a worked failure. The orientation preference of § 4.3 is exactly this multimodal case. The cost $L(q) = \lambda\lVert R(q) - R^\star\rVert_2^2$ on the seven joint angles has several minima because of inverse-kinematics redundancy: many distinct configurations reach the target orientation. When the denoising posterior places mass on two such branches, the proxy is their joint-space average, which lies between the basins and is not itself a valid configuration; it breaks both the position and the orientation target. Figure 9 shows two minima 0.02 mm apart in end-effector position whose joint-space mean lands 31 cm away. The gradient there has no consistent relation to the gradient at either branch, so guidance that descends it cannot honor the orientation without abandoning the reach (Table 3). CS–Pi evaluates the cost only on clean draws, each inside a single basin.

Exact per-iteration sampling law

This section makes precise the Gaussian-window interpretation of one CS-Pi iteration. The model’s conditional law admits the closed form below, whereas a deterministic numerical flow defines a pushforward kernel whose density need not equal that conditional law.

Setup. Let the flow-matching interpolant be

so $p(A_t \mid A_1 = a) = \mathcal{N}(A_t;ta,(1-t)^2I)$. At iteration $i$, define

For $\delta^{(i)} > 0$, the rule for $n$ gives

Proof. Bayes’ rule and Eq. (7) give

Using Eq. (10), $a_n^{(i)}-n^{(i)}a = n^{(i)}\left(a_c^{(i)}+\frac{1-n^{(i)}}{n^{(i)}}\mu^{(i)}-a\right)$. Substituting Eq. (9) yields Eq. (11). ∎

Center and endpoint cases. For $\mu^{(i)}=0$ the window is centered on the projection $a_{\mathrm{c}}^{(i)}$. Algorithm 1 uses $\mu^{(i)}=\epsilon^{(i)}\sim\mathcal{N}(0,I)$; conditional on that draw, Proposition 1 remains exact with center $a_{\mathrm{c}}^{(i)}+\sigma\epsilon^{(i)}$. At $n=0$ the iteration is an unconditional redraw, as in rejection sampling; at $n=1$ it returns the anchor, as in projection.

Scope. Proposition 1 is a per-iteration statement. It assumes exact posterior re-denoising; it does not establish that deterministic ODE integration samples this conditional, nor does it imply a stationary distribution, a convergence rate, or a bound after several iterations. Empirically, the projection residual decreases across iterations (App. § 13).

Hyperparameter sweeps and configuration selection

Every method in the main tables is reported at its best swept configuration. $\mathit{logL}$ is the mean exact log-likelihood under $\pi_\theta$ (nats/dim); NFE is the mean number of flow-model calls per rollout.

Swept hyperparameters. For CS-Pi we sweep $k$, the iteration cap $K_{\max}$ and the terminal projection (on/off). For gradient guidance we sweep the step size $\alpha$. For Trust Sampling we sweep the step size $w$, the maximum number of guidance steps per denoising step $J$, the schedule (constant vs. linear), whether a projection is applied after each step (“post-projection”), and, where a joint-limit penalty applies, its weight.

3D SPHERE-AVOID. Gradient guidance was swept over $\alpha\in[3\cdot10^{-4},0.3]$ (best $\alpha=0.01$: 23.6% / 4.7%) and Trust Sampling over $w\in[0.01,1.0]$ with constant and linear schedules (best $w=0.3$, $J=5$: 77.0% / 38.7%). Table 9 gives CS-Pi across $k$ ($K_{\max}=20$, no early stopping). Success rises with $k$ and saturates on L1 by $k=20$. Table 5 uses $k=50$; $k=100$ is 4.3 points higher on L2.

$k$	1	10	20	50	100
Safe-SR L1 / L2	90.1 / 59.8	97.8 / 77.7	99.6 / 88.3	99.4 / 90.2	99.8 / 94.5
NFE L1	72.0	24.4	20.9	19.2	18.7

2D AVOIDANCE. Gradient guidance uses the best $\alpha$ per scenario (macro 67.2%; Table 7); Trust Sampling reaches 34.9%. CS-Pi reaches 99.2–99.3% for $k\in\{5,10,20\}$ (Table 19).

Method	left	right	both	clutt.	narrow	narrower	Mean
Unconstrained	20.0	0.0	0.0	1.0	10.0	0.0	5.2
Gradient guidance	99.5	84.0	15.0	98.0	100	7.0	67.2
Trust Sampling	9.0	65.5	1.0	18.0	99.0	17.0	34.9
Rejection	60.0	95.0	70.5	71.5	86.0	93.5	79.4
Projection	100	100	100	100	99.0	100	99.8
SafeDiffuser	100	100	100	100	100	100	100
SafeFlowMatcher	95.0	77.5	88.0	10.5	0.0	0.0	45.2

Schedule	Fire rate	Safe-SR	logL median	NFE
adaptive, $k=5$ / 10 / 20	15.0 / 9.5 / 8.7%	99.2 / 99.3 / 99.3	1.42 / 0.38 / 1.38	180 / 153 / 145
constant $n=0.5$	27.7%	99.1	0.88	181
constant $n=0.8$	15.6%	99.6	−2.23	161
zero noise, $k=5$ / 10 / 20	15.0 / 9.0 / 8.6%	99.0 / 99.6 / 99.8	1.16 / 1.38 / 1.42	183 / 150 / 144

Shelf. Table 10 gives the full Trust Sampling sweep. Without post-projection Trust Sampling peaks at 53.3% ($w=0.1$, $J=5$), the reported configuration. With post-projection it peaks at 43.3%; post-projection is a terminal projection, which no baseline uses in the main tables. CS-Pi across $k$ ($K_{\max}=20$, no early stopping): 70.7 / 78.7 / 80.0 / 76.7 / 69.3% at $k=5$ / 10 / 20 / 50 / 100 (NFE 398 / 376 / 353 / 306 / 290); Table 2 uses $k=50$, $K_{\max}=5$ (81.3%).

$w$	no post-projection $J=2$	no post-projection $J=5$	no post-projection $J=10$	post-projection $J=2$	post-projection $J=5$	post-projection $J=10$
0.005	2.7	6.0	4.0	29.3	30.7	32.7
0.01	3.3	4.0	11.3	30.0	33.3	34.0
0.05	9.3	38.7	—	31.3	36.0	35.3
0.1	28.0	53.3	51.3	33.3	32.0	43.3

3D-Orient. We report gradient guidance at two step sizes: $\alpha=0.003$ keeps most of the reach (70.0%) and $\alpha=0.03$ gives its best success (6.7%); larger and smaller values are worse. Trust Sampling uses $w=0.001$ with the joint-limit penalty. CS-$\Pi$ uses $k=10$, $K_{\max}=3$, $\lambda_{ee}=3000$, $\beta=10^{6}$.

Empirical convergence: time $n$ and projection residual

A successful CS-Pi loop drives the projection residual $\delta$ to zero, and therefore $n = k/(k + \delta)$ to 1. We log both per iteration (Table 11; for 3D-ORIENT and the preference, $\delta$ is the proximal-step residual). Means are over loops still running at that iteration; for 2D we give the first and last iteration per group of scenarios.

Benchmark ($k$)	loops	mean $n$ per iteration	residual first $\to$ last
2D left/right/narrow (10)	–	0.64–0.81 $\to$ 0.99+	2.7–6.3 $\to$ ≈ 0.1
2D both/cluttered (10)	–	0.63–0.68 $\to$ 0.98	6–8 $\to$ 0.5–0.7
2D narrower (10)	–	0.73 $\to$ 0.80 (stalls)	4.1 $\to$ 2.9
2D constraint + preference (100)	150	0.743, 0.881, 0.919, 0.936, 0.946, 0.953	34.6 $\to$ 3.42
SPHERE-AVOID L1 (50)	120	0.991, 0.997, 0.999, 0.999, 0.999, 0.999	0.478 $\to$ 0.0071
SPHERE-AVOID L2 (50)	120	0.985, 0.993, 0.995, 0.997, 0.997, 0.997	0.767 $\to$ 0.0497
SPHERE-AVOID L1 (10)	119	0.955, 0.977, 0.986, 0.991, 0.993, 0.994	0.467 $\to$ 0.0225
Shelf (50)	1508	0.994, 0.998, 0.999, 0.999, 0.999	0.287 $\to$ 0.0376
Shelf (1)	1602	0.808, 0.818, 0.817, 0.817, 0.817	0.237 $\to$ 0.224
3D-ORIENT (10)	1440	0.940, 0.966, 0.978	0.633 $\to$ 0.222
3D-ORIENT (1)	1440	0.633, 0.667, 0.673	0.579 $\to$ 0.487

Where $n$ approaches 1. On SPHERE-AVOID and Shelf at their chosen $k$ the residual falls 7× (Shelf) to 67× (L1). Because the residuals are small relative to $k$, $n$ is close to 1 from the first iteration: the loop re-runs the policy from a barely noised anchor, so each draw stays close to the projection. Lowering $k$ to 10 on SPHERE-AVOID lowers the first-iteration $n$ to 0.955 and the loop still converges.

Where it does not. With $k=1$ on Shelf and 3D-ORIENT the loop stalls ($n$ stays at 0.82 and 0.67; the residual barely falls). On Shelf this costs safe success (66.7 vs. 81.3%); on 3D-ORIENT it costs NFE (348 vs. 210) and log-likelihood (9.53 vs. 10.21) but not success (64.4 vs. 65.6%). Under the preference the residual falls tenfold but never reaches zero: the proximal step always has a lower cost to move toward, so the loop refines rather than terminates. 3D-ORIENT keeps a small residual (0.22) because the orientation target is never met exactly. On the easy 2D scenarios $n$ rises above 0.99 within about four iterations and the residual falls about 100×; on narrower, the hardest scene, $n$ rises only from 0.73 to 0.80 and the residual stalls (4.1 $\to$ 2.9).

Early stopping

When the residual stops improving, further iterations cost compute without helping. We compare four stopping rules: none; stop when $n > 0.8$; stop when $n > 0.9$; and patience, which stops when the last five iterations close less than 5% of the remaining gap between $n$ and 1 (Table 13–Table 14; $K_{\max}=20$).

Rule	$k=1$	$k=10$	$k=20$	$k=50$	$k=100$
none	90.1/59.8 (72)	97.8/77.7 (24)	99.6/88.3 (21)	99.4/90.2 (19)	99.8/94.5 (19)
$n > 0.8$	90.4/62.9 (54)	88.2/62.1 (10)	88.2/62.1 (10)	88.2/62.1 (10)	88.2/62.1 (10)
$n > 0.9$	91.2/64.5 (68)	88.2/60.2 (10)	88.2/62.1 (10)	88.2/62.1 (10)	88.2/62.1 (10)
patience	89.5/64.5 (31)	96.6/75.8 (22)	98.3/81.6 (20)	99.5/84.0 (18)	99.2/88.3 (17)

Rule	$k=5$	$k=10$	$k=20$	$k=50$	$k=100$
none	70.7 (398)	78.7 (376)	80.0 (353)	76.7 (306)	69.3 (290)
$n > 0.8$	56.0 (255)	56.0 (240)	55.3 (240)	56.7 (240)	55.3 (240)
$n > 0.9$	62.7 (272)	62.0 (253)	58.0 (240)	57.3 (240)	57.3 (240)
patience	72.7 (312)	76.0 (307)	80.7 (307)	79.3 (291)	70.7 (280)

The threshold rules are tuned to 2D and do not transfer. On SPHERE-AVOID and Shelf, $n$ is close to 1 from the first iteration at moderate and large $k$, so these rules stop almost immediately and CS-Pi performs close to projection (88.2 / 62.1% on SPHERE-AVOID, 55–63% on Shelf). The patience rule is a trade-off rather than a free saving. Patience matches no stopping on Shelf (80.7 vs. 80.0% at $k=20$) at 13% fewer NFE, and on SPHERE-AVOID L1 (99.5 vs. 99.4% at $k=50$) at slightly lower NFE, but costs 6 points on L2 (84.0 vs. 90.2%). On 2D (Table 12; means over 11 values of $k$ and the six scenarios, 200 rollouts each) the patience rule matches the best threshold rule (0.868 vs. 0.864 success) at 36% fewer NFE than it and 54% fewer than no stopping. It also removes an over-refinement failure: at high $k$ on narrower, running without stopping collapses to ∼ 0%, while the rule holds 100%.

Rule	Success	NFE	Iterations
none	0.825	447	3.94
$n > 0.8$	0.864	322	2.62
$n > 0.9$	0.863	336	2.85
patience	0.868	206	1.59

Terminal projection: guarantee, how often it fires, and its cost

Guarantee and per-benchmark use. With the terminal projection in Algorithm 1, every executed action lies in $\mathcal{C}$, provided $\mathcal{C}$ is nonempty and $\mathcal{P}$ is exact. This is the projection’s guarantee, not one supplied by the loop; the loop’s role is to reduce the residual before that step. We use the terminal projection on 2D AVOIDANCE, SPHERE-AVOID, Shelf and hardware, and not on 3D-ORIENT, where the target is a preference. No baseline uses one.

On versus off. Table 15 runs CS-Pi with and without the terminal projection. Without it, CS-Pi still has the highest safe success on Shelf (74.0%, against 55.3% for projection and 53.3% for Trust Sampling) and on SPHERE-AVOID L2 (69.9%, against 62.1% and 38.7%); log-likelihood barely changes on these benchmarks.

Benchmark	Terminal proj.	Safe-SR	logL mean	logL median	Fire rate	NFE
SPHERE-AVOID L1	on	99.9	2.87	2.81	6.7%	20
	off	98.1	2.87	2.81	–	20
SPHERE-AVOID L2	on	91.8	2.54	2.27	46.9%	26
	off	69.9	2.55	2.27	–	26
Shelf	on	81.3	17.96	9.68	14.7%	271
	off	74.0	17.59	9.94	–	270

Log-likelihood of the chunks where it fires. Table 16 splits CS-Pi’s chunks by whether the terminal projection fired. On SPHERE-AVOID L1 and Shelf, the chunks where it fired keep a median log-likelihood at or above one-shot projection’s (2.90 vs. 2.41; 14.70 vs. 8.45); on L2 they match it (2.11 vs. 2.13). These chunks are the hardest subset by construction, whereas the projection column is the median over all of projection’s chunks.

Benchmark	Fires	Converged	Fired	Projection	Unconstrained
SPHERE-AVOID L1	6.7%	2.81	2.90	2.41	2.74
SPHERE-AVOID L2	46.9%	2.43	2.11	2.13	2.90
Shelf	14.7%	9.36	14.70	8.45	10.00
Hardware	4.8%	−469.1	−518.0	−473.2	−470.2

On hardware the terminal projection fires on 91 of 1879 chunks (4.8%; 2.3% in layout 1, 6.2% in layout 2; in 15 of 20 trials), and those chunks score lower than projection (−518.0 vs. −473.2). The raw policy draws in these chunks are typical (median −463.1, against −464.2 for chunks that converged), so the drop comes from the terminal projection rather than from atypical draws. Before it fires, the loop has removed about 41% of the initial violation; the projection then moves the chunk by a median of 13.7 mm of end-effector motion (0.56× the chunk’s own motion, 1.6% of the workspace; p95 170 mm), and leaves a median residual penetration of 9.7 mm (p95 40.3 mm), so on hardware the terminal projection at the iteration cap does not certify feasibility. Chunks that converged execute an unmodified policy sample with median residual penetration 0.0 mm (p95 0.76 mm).

When $C$ misses the policy’s support. If $C$ is nonempty but the policy assigns it negligible mass, no sampler can return an action that is both safe and plausible. CS-Pi then degrades toward its terminal projection. A residual that stops shrinking, equivalently an $n$ that stops rising, is a runtime signal of this case; App. § 14 turns it into a stopping rule. On 2D narrower the residual falls from 4.1 to 2.9 and then stalls, with $n$ stuck near 0.80 (App. § 13).

Compute

NFE counts flow-network calls but omits projection and proximal solves and the backward passes of gradient guidance and Trust Sampling. Table 17 reports wall-clock time per rollout, measured on a single machine. CS-Pi costs 2.1× projection on SPHERE-AVOID and 1.6× on Shelf, because of its projection and proximal solves. It is 67× faster than Trust Sampling on 2D, 1.2× faster on Shelf, equal on 3D-ORIENT, and 2.1× slower on SPHERE-AVOID. The CBF samplers are the most expensive on SPHERE-AVOID and Shelf. Hardware compute is in App. § 21.§ 1.

Method	2D	SPHERE-AVOID	Shelf	3D-ORIENT
Unconstrained	0.19	0.19	4.59	0.47
Gradient guidance	0.52	0.58	22.1	1.56
Trust Sampling	8.70	2.02	89.7	12.48
Projection	0.08	1.99	49.0	—
SafeDiffuser	0.13	7.14	164	—
SafeFlowMatcher	0.16	13.99	167	—
CS-Pi	0.13	4.24	76.0	12.70

Mechanism, sensitivity, and additional analyses

Why support membership can exceed the unconstrained policy’s. Renoising and re-denoising pulls a sample toward high-density regions, as in SDEdit, and feasibility and support membership are positively correlated in these tasks, so stopping on feasibility further favors on-support modes. Applying one renoise and re-denoise to an unmodified draw, with no constraint or projection, raises the fully-on-support rate by up to 7.5 points and the mean fraction of on-support waypoints from 96.9% to 98.6%.

Sensitivity to $k$. Table 18 compares the tuned $k$ with a single fixed value for every benchmark. A single $k=50$ is within 1.2 points of the best $k$ on 2D, SPHERE-AVOID L1, Shelf and 3D-ORIENT, and within 4.3 points on SPHERE-AVOID L2. A single $k=20$ loses up to 7.4 points (SPHERE-AVOID L2). Within a benchmark, safe success varies by a few points over a broad range of $k$ on SPHERE-AVOID L1, Shelf and 3D-ORIENT; SPHERE-AVOID L2 is more sensitive (Table 9, Table 14).

Benchmark	Reported $k$	Safe-SR (reported $k$)	Safe-SR ($k=20$)	Safe-SR ($k=50$)
2D	20	99.0	99.0	99.1
SPHERE-AVOID L1	50	99.9	99.2	99.9
SPHERE-AVOID L2	50	91.8	84.4	91.8
Shelf	50	81.3	74.7	81.3
3D-ORIENT	10	65.6	64.4	64.4

Schedule for $n$. Table 19 compares the rule $n=k/(k+\delta)$ with a constant $n$ and with renoising the projection with zero noise (adaptive $n$, $\epsilon=0$), on 2D AVOIDANCE ($K_{\max}=5$, $6 \times200$ rollouts). The adaptive rule fires the terminal projection least (8.7% at $k=20$, against 15.6% and 27.7% for constant $n=0.8$ and 0.5). Zero-noise renoising matches the adaptive rule on fire rate and median log-likelihood.

Smoothness. Table 20 reports the per-rollout maximum acceleration and jerk (second and third differences of the executed trajectory). On Shelf CS-Pi is within 5% of the unconstrained policy (acceleration +4.5%, jerk +4.8%), and projection is the least smooth. On SPHERE-AVOID CS-Pi is within 1% on L1 and +14% / +11% on L2, matching projection. On 2D the means are similar across methods, but CS-Pi and projection have heavier 95th-percentile tails (2.2–4.2× the unconstrained policy). On 3D-ORIENT CS-Pi’s end-effector step matches the unconstrained policy (0.07 m), but its joint-space acceleration is about 14× and its jerk about 11× higher: the end-effector proximal step uses the arm’s redundant joints. We therefore claim end-effector, not joint-space, smoothness on 3D-ORIENT.

Benchmark	Method	Acceleration mean	Acceleration p95	Jerk mean	Jerk p95
Shelf	Unconstrained	0.518	0.626	0.841	1.000
Shelf	CS-Pi	0.541	0.651	0.882	1.073
Shelf	Projection	0.549	0.666	0.907	1.097
Shelf	Gradient guidance	0.521	0.619	0.874	1.013
Shelf	Trust Sampling	0.524	0.614	0.878	0.983
Shelf	Rejection	0.521	0.618	0.845	0.966
SPHERE-AVOID L1	Unconstrained	0.165	0.243	0.138	0.221
SPHERE-AVOID L1	CS-Pi	0.166	0.241	0.137	0.215
SPHERE-AVOID L1	Projection	0.171	0.248	0.142	0.222
SPHERE-AVOID L2	Unconstrained	0.166	0.216	0.139	0.196
SPHERE-AVOID L2	CS-Pi	0.189	0.247	0.155	0.217
SPHERE-AVOID L2	Projection	0.192	0.246	0.155	0.201
2D	Unconstrained	0.0058	0.0088	0.0061	0.0092
2D	CS-Pi	0.0068	0.0196	0.0090	0.0306
2D	Projection	0.0060	0.0198	0.0087	0.0390
3D-ORIENT	Unconstrained	0.165	0.204	0.277	0.347
3D-ORIENT	CS-Pi	2.254	4.574	2.958	8.812
3D-ORIENT	Gradient guidance ($\alpha=0.03$)	0.905	1.499	1.355	2.667

CBF certificates. A safety certificate alone does not make a sample useful: projection already supplies one. Only SafeDiffuser matches CS-Pi, and only on 2D (Table 1); both CBF samplers trail it on SPHERE-AVOID (Table 5) and Shelf (Table 2). SafeFlowMatcher on Shelf uses $\rho=0.7$, $\epsilon=5$, $\alpha=0.07$, $T_p=10$, $T_c=8$ (swept $\rho\in\{0.6, 0.7, 0.8\}$).

Blocked-mode diagnostic

This experiment isolates the claim in § 2 that a correction applied during denoising can move a sample out of an unsafe region but cannot move it to another mode of the policy. A flow-matching policy is trained on a symmetric two-mode Gaussian mixture in 2D (modes at $(\pm1.2, 0)$, standard deviation 0.18). A disk of radius 0.55 centered on the left mode blocks it entirely. Because the problem is 2D, exact log-likelihoods and exact rejection sampling are tractable, so every method is compared with the correct answer: the policy restricted to the safe set. We evaluate the blocked subset, the 241 of 500 paired initial draws that land in the blocked mode (Figure 12).

All methods are safe on every draw, so safety does not separate them (Table 21). CS-Pi moves 85.9% of blocked draws to the surviving mode at a log-likelihood within 0.05 nats of exact rejection and at the same cost. Projection, SafeDiffuser and SafeFlowMatcher at its published defaults move none: each applies a deterministic correction to the blocked sample, so its output stays near the blocked mode. In a control where the constraint touches neither mode, CS-Pi is statistically indistinguishable from the unconstrained policy. SafeFlowMatcher was swept over 577 configurations, SafeDiffuser over 3, and CS-Pi over 27. One SafeFlowMatcher setting (a single prediction step) does move most blocked draws, but it moves samples to one mode even when the constraint touches neither mode, and returns unsafe actions when the policy has a single mode, so the move is not driven by the constraint.

Method	Moved to the surviving mode	median $\Delta\log p$	NFE
Rejection (exact reference)	92.5%	−0.03	284
CS-Pi	85.9%	−0.08	282
Projection	0.0%	−3.56	100
SafeDiffuser ($\lambda=50$)	0.0%	−3.56	100
SafeFlowMatcher (published defaults)	0.0%	−6.42	257

Reliability of posterior-mean guidance and approximate projection

The guidance proxy in Eq. (5) can fail even for a unimodal posterior. The relevant quantity is how quickly the cost curvature varies over the posterior’s spread.

Proof. Taylor expansion of $\nabla L$ about $\bar{a}$ gives $\nabla L(a_1) = \nabla L(\bar{a}) + \nabla^2 L(\bar{a})(a_1 - \bar{a}) + r(a_1)$ with $\lVert r(a_1)\rVert\leq\frac{L_H}{2}\lVert a_1 - \bar{a}\rVert^2$ [28 Lemma 1.2.4]. Taking expectations cancels the linear term; then $\lVert\mathbb{E}[r]\rVert\leq\mathbb{E}\lVert r\rVert\leq(L_H/2)\mathbb{E}\lVert a_1 - \bar{a}\rVert^2$ and $\mathbb{E}\lVert a_1 - \bar{a}\rVert^2 = \operatorname{tr}\operatorname{Cov}(a_1 \mid a_t)$.

The proxy is therefore reliable when the curvature varies slowly at the scale of the posterior’s spread. Multimodality is not required for bias: a wide unimodal posterior suffices. Multimodality creates the more severe case, in which the posterior mean lies off-support between modes.

Approximate and non-convex projections. For non-convex $C$ the projection can be set-valued, and iterative solvers may return only a local or approximate solution. During CS-Pi iterations this point is an anchor rather than the executed action, so renoising and re-denoising can absorb anchor error and move to another mode. Projection has no such correction. Only CS-Pi’s optional terminal step executes a projection; exact safety of the executed action still requires that step to be exact.

Benchmarks, metrics, and implementation details

Environments. 2D AVOIDANCE is a planar reaching task in $[0,1]^2$ with start $(0.1, 0.5)$ and goal $(0.9, 0.5)$; the state is $(x, y, g_x, g_y) \in\mathbb{R}^4$ and the action a velocity $(v_x, v_y)$, in chunks of $H=16$ steps with 4 executed per replan. Six training obstacles are present in every scenario; each test scenario adds obstacles that close off some of the demonstrated paths (left, right, both, cluttered, narrow, narrower). The safe set is the collision-free set. 3D SPHERE-AVOID (App. § 7) is a simulated 7-DoF Franka Panda planning task in joint space, parameterized as a B-spline with 28 control points per joint; the safe set is the collision-free set of the swept-sphere geometry plus joint limits. Shelf (§ 4.2) uses the same simulated arm for pick-and-place in five scenarios (App. § 8); actions are chunks of 16 absolute joint configurations. 3D-ORIENT is a 7-DoF reach to a goal anywhere in the reachable workspace, with an orientation preference handled by a proximal step on the end-effector orientation cost.

Metrics. Safe-SR is the fraction of rollouts that complete the task and satisfy the constraint (2D: reach the goal collision-free; SPHERE-AVOID: reach the goal with a collision-free trajectory; Shelf: pick and place the cube collision-free). On 3D-ORIENT, success requires a step within 10 cm of the target position and within 0.36 rad of the target orientation. fully-in-supp. is the fraction of rollouts whose every waypoint lies inside a dilated convex hull of the demonstrations. logL is the exact log-likelihood under $\pi_\theta$ (nats/dim), averaged over chunks. NFE is the mean number of flow-model calls per rollout.

Configuration. All simulated experiments use deterministic Euler integration. Table 22 lists the per-benchmark settings.

Benchmark	Action	CS-Pi	Term. proj.	$\tau$	Rollouts
2D AVOIDANCE	$(v_x, v_y)$, $H=16$	$k=20$, $K_{\max}=5$	on	0.01	$6 \times200$
SPHERE-AVOID	B-spline, $28 \times7$	$k=50$, $K_{\max}=20$	on	0.001	$120 \times8$
Shelf	joint chunk, $16 \times7$	$k=50$, $K_{\max}=5$	on	0.001	$150\ (30 \times5)$
3D-ORIENT	7-DoF joints	$k=10$, $K_{\max}=3$	off	0.001	$90\ (30 \times3)$
Real world	joint velocities	$k=5$, $K_{\max}=5$	on	0.001	$20\ (10 \times2)$

Hardware deployment setup

We deploy CS-Pi zero-shot on a Franka FR3 with a Robotiq 2F-85 gripper, controlled through FrankaPy at 15 Hz. The policy is $\pi_{0.5}$ finetuned on DROID; it acts in joint-velocity space, so every Cartesian constraint is enforced through forward kinematics and Jacobians on the integrated joint trajectory $Q[t] = q_0 + \Delta t \sum_{\leq t} V$, with $\Delta t$ the control period.

Perception to constraints. The ZED 2 side camera (RGB and metric depth) drives the constraint pipeline; the ZED mini wrist camera is used only by the policy. All geometry is expressed in the robot base frame. For benchmarking we assume a static scene and run the perception pipeline once per rollout: SAM2 segments the scene with no text prompt, each mask is unprojected through depth into a point cloud, and a vision-language model (Gemini) assigns only semantic roles (which segment to grasp, which to place on, which to ignore). It never produces coordinates; all positions and extents come from depth. Each obstacle is fit with a tight oriented bounding box; the grasp and target objects reduce to centroids. Because a single depth view sees only an object’s front, we mirror the occluded half across the camera-ray mid-plane before fitting, which enlarges the geometry in the safe direction.

Constraints and execution. A task state machine (REACH $\rightarrow$ GRASP $\rightarrow$ TRANSPORT $\rightarrow$ PLACE) rebuilds the active constraints every chunk: a goal attractor at the grasp or target centroid, a collision constraint between the robot and gripper sphere model and the obstacle boxes, a table half-space, and, once grasped, a sphere for the held object. CS-Pi, gradient guidance, Trust Sampling and projection receive the identical obstacle set, $q_0$ and $\Delta t$, so the comparison differs only in the sampler. The collision cost takes the form each method is designed for: a hinge penetration penalty for CS-Pi and projection, and an OmniGuide-style log-barrier on clearance for gradient guidance and Trust Sampling. The perceived scene is saved to a scene file and replayed across methods.

Planned versus realized motion. Collisions are scored on the realized joint configuration each tick with zero safety margin. FrankaPy controller lag leaves the arm short of each commanded waypoint ($\sim50$ mm of end-effector travel), so a chunk with zero planned constraint cost can still log a contact. This execution gap applies equally to every method.

Hardware execution analysis

The following statistics are computed offline from the logged trials (20 CS-Pi trials, 1879 chunks).

What executes. On 73.9% of chunks the raw $\pi_{0.5}$ draw is already safe and executes unchanged (bit-identical); on 21.2% the loop converges and executes an unmodified re-denoised policy sample; on 4.8% the terminal projection fires (App. § 15). Iterations per chunk: 1 (1389 chunks), 2 (198), 3 (120), 4 (56), 5 (116). The median ratio of successive residuals is 0.20, 0.33, 0.61 and 0.77 over the first four iterations (96, 91, 81 and 75% of ratios below 1): contraction is strongest at the first iteration, and the violation that survives to the later iterations is the one that ends in the terminal projection.

Smoothness. Table 23 reports the per-trial maximum end-effector jerk, differentiated against measured timestamps. CS-Pi’s median is the highest (the correction adds motion), but it has the lightest tail: the ratio of the 95th percentile to the median is 1.64, against 1.80 for the unconstrained policy and 5.69 for projection.

Method	Trials	Median	p95	p95 / median
Unconstrained	20	116.9	210.3	1.80
Gradient guidance	20	90.9	322.7	3.55
Trust Sampling	10	178.8	333.5	1.87
Projection	17	167.0	949.7	5.69
CS-Pi	20	181.6	298.7	1.64

Dynamic validity. The projection certifies joint-position limits, collision and the table, but not velocity or acceleration limits. The realized joint velocity never exceeded the Franka limits (0 of 130,361 joint-ticks; worst 0.77$\times$ the limit), because the impedance controller tracks only part of each commanded step. The commanded stream does exceed them occasionally: 0.08% of joint-ticks over the velocity limit within converged chunks, which execute unmodified policy samples, and 2.0% within chunks where the terminal projection fired. Adding velocity and acceleration bounds to the projection would extend the certificate to them.

Compute. CS-Pi makes 1.62 policy calls per chunk on average (0.62 re-denoises) and 2.57 projection solves; its mean NFE is 10.9 against 10 for a single draw, because with $k=5$ the time $n$ is close to 1 and a re-denoise needs about one integration step. Per-step latency is reported in Table 4.

Method	Safe-SR (%) $\uparrow$			Time/step (s) $\downarrow$	$\log L \uparrow$
	Layout 1	Layout 2	Averaged
Unconstrained	20	0	10	0.069	$-470.2\pm1.0$
Gradient guidance	40	10	25	0.139	$-464.9\pm1.4$
Trust Sampling	20	20	20	0.135	$-474.8\pm2.1$
Projection	40	40	40	0.099	$-473.2\pm2.3$
CS-Pi	60	50	55	0.143	$-471.8\pm1.7$