Joint Flow Trajectory Optimization (JFTO)

Abstract

Learning from human video demonstrations offers a scalable alternative to teleoperation or kinesthetic teaching, but poses challenges for robot manipulators due to embodiment differences and joint feasibility constraints. We address this problem by proposing the Joint Flow Trajectory Optimization (JFTO) framework for grasp pose generation and object trajectory imitation under the video-based Learning-from-Demonstration (LfD) paradigm. Rather than directly imitating human hand motions, our method treats demonstrations as object-centric guides, balancing three objectives: (i) selecting a feasible grasp pose, (ii) generating object trajectories consistent with demonstrated motions, and (iii) ensuring collision-free execution within robot kinematics. To capture the multimodal nature of demonstrations, we extend flow matching to SE(3) for probabilistic modeling of object trajectories, enabling density-aware imitation that avoids mode collapse. The resulting optimization integrates grasp similarity, trajectory likelihood, and collision penalties into a unified differentiable objective. We validate our approach in both simulation and real-world experiments across diverse real-world manipulation tasks.

Method

JFTO jointly optimizes a robot's initial grasp and its subsequent motion so that the resulting object trajectory imitates a human video demonstration, while remaining kinematically feasible and collision-free. Unlike sequential approaches that first fix a grasp and then optimize the trajectory, JFTO reasons about the grasp and downstream trajectory simultaneously — letting the robot choose grasps that remain feasible throughout the whole motion.

JFTO pipeline: video input, upstream processing into background/object/trajectory, optimization loop over trajectory, grasp, and collision evaluation, and robot execution

Overview of the JFTO framework. A demonstration video is processed to extract a static background, the manipulated object, and its trajectory. These drive an optimization loop that jointly scores trajectory imitation, grasp quality, and collision avoidance to produce a feasible joint-space trajectory for execution.

A rollout ξ = {q_t} is scored by three components, combined into one differentiable objective:

S_T(ξ)

Trajectory Imitation

How well the induced object trajectory matches the demonstrated trajectory, scored under a learned density model rather than raw distance — preserving multimodal demonstration strategies instead of averaging them away.

S_G(q₀)

Grasp Feasibility

How closely the initial grasp resembles a feasible, human-like grasp, combining a learned feasibility classifier with similarity to grasps observed in the demonstration.

S_C(ξ)

Collision Avoidance

How safe the rollout is with respect to joint limits and a differentiable distance field built from the reconstructed static scene.

Extracting demonstrations from video

For each task, a human demonstration is recorded with two calibrated cameras. Each frame pair is reconstructed into a dense 3D point cloud with DUSt3R, metrically scaled via triangulation against the known camera baseline. SAM2 segments the hand and manipulated object from the static background, and HaMeR estimates hand pose at keyframes. Consecutive object point clouds are aligned with ICP to recover a temporally consistent object-pose trajectory, while the background is fused into a static point cloud used later for collision checking.

Two-view demonstration with hand and object segmentation masks, reconstructed into a 3D scene with hand and object extracted — From two calibrated views with hand/object masks, the scene is reconstructed with 3D foundation models and the segmentation masks separate points belonging to the hand and the object.

Reconstructed 3D object trajectory over a static background point cloud, used for collision-aware optimization — The upstream pipeline applied across keyframes yields the object's 3D trajectory, together with a fused static background used for distance-field collision checking.

Flow matching on SE(3) for trajectory density

To capture the multimodal nature of demonstrations, JFTO extends flow matching to model time-dependent pose distributions on SE(3). A learned velocity field transports a base Gaussian into the demonstrated pose distribution at each time step; integrating this ODE forward generates realistic poses, while integrating the instantaneous change-of-variables formula backward yields the log-density of any candidate pose. Scoring a rollout under this density — rather than distance to the mean demonstration — rewards trajectories that fall into high-probability regions of the demonstrated motion, preserving distinct demonstrated strategies instead of collapsing them into an unrealistic average.

Flow matching diagram: a velocity field transports a base Gaussian to a target pose distribution; integrating backward from a sample recovers its log-density — Flow matching learns a time-dependent velocity field v_t from a base distribution π₀(x) to a target distribution π₁(x); integrating backward from a sample in π₁(x) recovers the log-density of that sample.

A chain of pose distributions p(q_0) through p(q_T), each linked to a base Gaussian for sampling and density estimation — Each time step of a demonstration is treated as a distribution over object poses in SE(3). Flow matching enables both sampling realistic poses (forward integration) and computing their log-likelihoods (backward integration).

Real-World Evaluation

JFTO is evaluated in simulation and on a real, resource-constrained robot — an Agile-X Piper 6-DoF arm with a parallel gripper. Its limited degrees of freedom and small work envelope create a challenging setting for grasp feasibility, collision avoidance, and faithful trajectory execution. We construct a seven-task benchmark spanning tool use, pouring, cutting, drawing-like motions, and obstacle-aware transport:

Task 1 — Hammer Hitting Nail: pick up a hammer and strike a nail.
Task 2 — Cup Pouring Water: grasp a cup and pour water into a bowl.
Task 3 — Saw Cutting Wood: lift a saw, reorient upright, and cut.
Task 4 — Drawing Figure 8: trace a figure-eight with a held bowl.
Task 5 — Drilling Foam: grasp a toy drill and drill into foam.
Task 6 — Solder Putting Back: return a soldering tool to its station.
Task 7 — Shifting Meter Tape: transport a tape across the workspace, with additional obstacles introduced at inference.

Grid of robot execution rollouts across the seven real-world manipulation tasks

Robot execution rollouts across all seven real-world tasks. JFTO successfully executes every task, reproducing feasible grasps and demonstration-consistent object motion despite the arm's limited degrees of freedom.

Joint vs. sequential optimization

We compare JFTO against a sequential baseline that first optimizes a grasp to match the demonstration, then holds it fixed while optimizing the trajectory. Both methods deliver similarly low positional error, but the sequential baseline incurs substantially higher rotational error and markedly lower log-density on several tasks — it can find a grasp near the demonstrated one, but often fails to maintain the correct object orientation during execution. By reasoning about execution feasibility during grasp selection, JFTO produces object poses that remain consistent over time.

Table I — Trajectory similarity between joint flow trajectory optimization and sequential optimization, across the seven-task benchmark. Lower is better for Avg Dist / Rot Diff; higher is better for Avg Log Density.
Method / Metric	Hammer	Cup	Saw	Figure 8	Drill	Solder	Meter Tape
Joint Optimization (JFTO)
Avg Dist Diff ↓	0.0108	0.0150	0.0137	0.0116	0.0181	0.0169	0.0270
Avg Rot Diff ↓	0.2572	0.3180	0.2160	0.1787	0.4543	0.3647	0.1686
Avg Log Density ↑	7.0973	6.2451	7.2066	7.4351	7.0655	6.0329	6.8647
Sequential Optimization
Avg Dist Diff ↓	0.0171	0.0110	0.0123	0.0080	0.0087	0.0296	0.0253
Avg Rot Diff ↓	0.5748	0.4075	0.4263	0.2758	0.3968	0.8603	0.1518
Avg Log Density ↑	1.4823	−2.7326	1.6412	6.5238	5.7580	−1.6030	5.7229

Multi-Modality: Why Flow Matching

Many manipulation tasks admit multiple valid strategies for the same goal — an object might be moved around an obstacle to the left or to the right. Traditional distance-based objectives collapse this into an unrealistic "average" trajectory that is often physically infeasible. Flow matching instead models the full density of the demonstration distribution, guiding optimization toward one of several high-density modes — each of which stays consistent with at least one real demonstrated strategy.

Trajectory-density visualization, view 1: yellow/purple regions indicate high/low demonstration density — Two viewing angles of the same trajectory-density visualization. Yellow/purple indicates high/low density regions of the demonstration distribution. The flow-based objective (red) converges to one of the demonstrated trajectories, while a distance-based objective (green) instead collapses toward an unrealistic average that matches no real demonstration.

Trajectory-density visualization, view 2: flow matching converges to a demonstrated mode while a distance-based objective collapses to the mean — Two viewing angles of the same trajectory-density visualization. Yellow/purple indicates high/low density regions of the demonstration distribution. The flow-based objective (red) converges to one of the demonstrated trajectories, while a distance-based objective (green) instead collapses toward an unrealistic average that matches no real demonstration.

This property carries through to real robot execution. For the meter-tape task, some demonstrations route the tape in front of an obstacle, others behind it. Depending on initialization, JFTO reliably reproduces one of these demonstrated strategies. A naive distance-based objective instead averages across all demonstrations, producing a path that cuts directly through the obstacle — a motion that resembles none of the human demonstrations.

Real robot execution of the meter-tape task showing two distinct demonstration-consistent trajectories produced by flow matching, versus a third trajectory from a distance-based objective that cuts through the obstacle

Given multi-modal demonstrations around an obstacle, the flow-matching model reliably reproduces Trajectory 1 or Trajectory 2 depending on initialization — both faithful to a demonstrated strategy. A distance-based objective instead mode-seeks to an averaged Trajectory 3 that is consistent with no human demonstration.

BibTeX

@article{dong2025jfto,
  title   = {Joint Flow Trajectory Optimization For Feasible Robot Motion Generation from Video Demonstrations},
  author  = {Dong, Xiaoxiang and Johnson-Roberson, Matthew and Zhi, Weiming},
  journal = {arXiv preprint arXiv:2509.20703},
  year    = {2025},
  note    = {To appear at IEEE International Conference on Robotics and Automation (ICRA), 2026}
}