Constraining Streaming Flow Models for Adapting Learned Robot Trajectory Distributions

1. Problem Statement

Modern robot control increasingly relies on generative models (e.g., Diffusion Policies, Flow Matching) to learn complex, multi-modal trajectory distributions from demonstrations. While these models offer high expressivity and reactivity, they face a critical limitation: safety and feasibility constraints.

• The Gap: Existing generative policies are trained on data distributions that may not cover all safety constraints (e.g., joint limits, self-collisions, dynamic obstacles).
• The Failure Mode: Directly executing these policies in real-world environments often leads to collisions or infeasible motions.
• Limitations of Current Solutions:
  • Post-hoc Projection/Filtering: Methods like Control Barrier Functions (CBFs) or hard projection correct unsafe actions after generation. These often introduce discontinuities, oscillations, or high computational overhead (solving QPs at every step), degrading the smoothness and fidelity of the learned policy.
  • Retraining: Retraining the model with new constraints is computationally expensive and lacks flexibility for dynamic environments.

The paper addresses the need for a mechanism to adapt learned streaming flow policies in real-time to enforce safety constraints without retraining or disrupting the continuous, reactive nature of the flow.

2. Methodology: Constraint-Aware Streaming Flow (CASF)

The authors propose CASF, a framework that reshapes the learned velocity field of a Streaming Flow Policy (SFP) during inference by modifying the underlying geometry of the action space.

Core Concept: Metric-Weighted Flow

Instead of projecting the output of the policy, CASF modifies the Riemannian metric of the control space. It treats the robot's configuration space as a deformable manifold where the local geometry changes based on proximity to constraints.

• Distance-Induced Metrics:

  • Constraints (obstacles, joint limits) are modeled as differentiable distance functions, $d_k(a)$, representing the distance from the current state $a$ to the infeasible region boundary.
  • A local metric tensor $M(a)$ is constructed. Far from constraints, $M(a) \approx I$ (Identity). Near constraints, the metric stretches the space in the direction of the constraint normal (increasing "cost" to move toward the obstacle) and compresses it tangentially.
  • Formula: The metric is defined as:
    $$M(a) = I + \sum_k w_k(d_k) \left[ \alpha_k(d_k) n_k n_k^\top + \beta_k(d_k) (I - n_k n_k^\top) \right]$$
    Where $n_k$ is the surface normal, and $\alpha, \beta$ control stiffness along and tangent to the boundary.

• Metric-Weighted Streaming:

  • The original learned velocity field $v_\theta$ is pre-multiplied by the inverse of the metric $M(a)^{-1}$ to generate the constrained velocity $\tilde{v}$:
    $$\frac{da(t)}{dt} = \tilde{v}(a, t) = M(a)^{-1} v_\theta(a, t)$$
  • This operation effectively performs a soft, continuous projection. It attenuates motion toward obstacles while preserving motion along safe surfaces, analogous to fluid flowing around a solid object.

Technical Components

1. Neural Signed Distance Fields (Neural SDFs): For complex geometries where analytic distance functions are unavailable, the authors train neural networks to regress distance fields from sampled points. These networks are constrained by the Eikonal equation ($\|\nabla \phi\| = 1$) to ensure metric consistency.
2. Metric Pullback (Workspace to Configuration Space):
   • Constraints are often defined in the workspace (e.g., obstacle proximity), but SFPs operate in configuration space (joint angles).
   • CASF uses the robot's Jacobian ($J$) to pull back workspace metrics into configuration space:
     $$M_q(q) = J(q)^\top M_x(f(q)) J(q)$$
   • This allows constraints on specific body points (links, end-effector) to be aggregated into a unified configuration-space metric, enabling whole-robot collision avoidance.

3. Key Contributions

1. CASF Framework: A novel method for reshaping streaming flow policies via constraint-dependent metric fields, enabling continuous, differentiable safety enforcement at runtime without retraining.
2. Modular Constraint Composition: A unified scheme that fuses constraints from both workspace (obstacles) and configuration space (joint limits) into a single control-space geometry via metric pullback.
3. Theoretical Connection to CBFs: The authors demonstrate that metric-weighted projection is a continuous, analytic approximation of Control Barrier Functions (CBFs), offering a lightweight alternative to solving Quadratic Programs (QPs) at every timestep.
4. Empirical Validation: Extensive evaluation in simulation (PushT, LASA, RoboMimic) and real-world experiments on a Universal Robot (UR) manipulator.

4. Experimental Results

The paper evaluates CASF against baselines: No Shaping (raw SFP), Hard Projection, and CBF filtering.

• Safety Performance:
  • CASF achieves near-zero constraint violations (Maximum Penetration Depth and Integral Violation) across all tasks, matching the strict safety of CBFs and Hard Projection.
  • In contrast, the raw SFP baseline frequently collides with obstacles.
• Trajectory Fidelity & Smoothness:
  • Masked Fréchet Distance: CASF significantly outperforms Hard Projection and CBFs, maintaining a trajectory shape much closer to the original learned policy.
  • Qualitative Behavior: While Hard Projection and CBFs often cause abrupt stops, oscillations, or "blocking" effects due to discontinuous corrections, CASF produces fluid, anticipatory deviations. The robot smoothly steers around obstacles while maintaining task progress.
• Whole-Body Avoidance:
  • In 3D manipulation tasks, CASF successfully prevents collisions on intermediate links (elbow, forearm) by redistributing avoidance behavior across joints, whereas baselines focusing only on the end-effector fail.
• Real-World Deployment:
  • Experiments on a UR robot with tasks like "Tidy Up," "Pick & Place," "Sawing," and "Hammer Hitting" demonstrate that CASF generalizes to unseen obstacle configurations, preventing collisions while completing tasks successfully.

5. Significance and Impact

• Bridging Learning and Safety: CASF provides a principled way to integrate geometric safety constraints into data-driven generative policies without sacrificing the multi-modal expressivity or low-latency inference that makes these models attractive.
• Efficiency: By avoiding the computational cost of solving optimization problems (QPs) at every control step, CASF is computationally lightweight and suitable for high-frequency closed-loop control.
• Robustness: The ability to handle whole-robot collisions and dynamic obstacles in real-time makes this approach highly relevant for deploying learned policies in unstructured, real-world environments.
• Future Direction: The authors suggest extending the metric to non-reversible structures (e.g., Finsler metrics) to handle asymmetric constraints and non-convex corridors more effectively.

In summary, CASF transforms the "hard" problem of safety constraints into a "soft" geometric deformation of the policy's velocity field, resulting in robot behaviors that are simultaneously safe, smooth, and faithful to learned demonstrations.