Navigating Against the Current: An Interactive Tour of FlowFMT*

Imagine you’re designing a balloon that explores the atmosphere of Venus. The winds blow at up to 100 m/s. Your balloon’s propellers push it at maybe 10 m/s relative to the air. Standard path planners assume you can always fight the environment to go wherever you want — that assumption fails here completely.

This post is an interactive walkthrough of FlowFMT*, a motion planning algorithm I built during my PhD. It adapts the Fast Marching Tree (FMT*) algorithm to handle environments where the surrounding flow is faster than the vehicle itself. The key idea is a reachability cone: a geometric condition that determines, given a flow vector, which directions the vehicle can actually travel toward.

The work was published in Autonomous Robots (2024). Source code is on Bitbucket.

The Environment: Double-Gyre Flow

The standard benchmark for flow-aware planners is the double-gyre flow field — two counter-rotating vortices sitting side by side. It’s a good test bed because some regions have strong flow (exceeding the vehicle’s top speed) while others are mild.

The velocity at position $(x, y)$ is:

$\vec{c}(x, y) = \begin{bmatrix} -\pi A \sin(\pi x)\cos(\pi y) \\ \pi A \cos(\pi x)\sin(\pi y) \end{bmatrix}$

With $A = 0.02$ , the peak flow speed is $\pi A \approx 0.063$ m/s. The vehicle’s top speed relative to the fluid is $v_r^{\max} = 0.05$ m/s. The flow beats the vehicle.

Drag the v_max slider to set the vehicle speed. Red regions are where the vehicle can’t fully counteract the flow; green regions are mild enough to go anywhere.

v_max (m/s): 0.05

Particle count: 80

The two gyres circulate in opposite directions. Notice how the flow speeds (arrow lengths) peak near the center of each gyre and weaken toward the edges.

The Reachability Problem

Set v_max to 0.05 and observe: a large fraction of the field is “strong flow” — the vehicle can’t fully counteract the stream there. This creates a fundamental problem: not every target position is reachable from every starting point.

Here is the geometric intuition. Place the vehicle at some point. A constant rightward flow carries it rightward at speed $\|c\|$ regardless of what it does with its propellers. Simultaneously, the vehicle can apply thrust to reach any point within a sphere of radius $v_r^{\max}\Delta t$ centered on the drifted position. The reachable set at time $\Delta t$ is therefore a sphere of radius $v_r^{\max}\Delta t$ centered at the vehicle’s drifted position, not its original position.

As the flow strengthens relative to the vehicle’s speed, the drifted sphere moves farther rightward. Eventually the origin falls behind the left edge of every sphere — there is a region behind the vehicle (against the flow) it can never reach.

Drag the c / v_max slider from 0 to 2 and watch the dead zone open up:

c / v_max: 0

When the ratio exceeds 1, a cone-shaped dead zone opens behind the vehicle. The cone narrows as the ratio grows — the only reachable directions are increasingly “downstream”.

Runnable Code

Here are both functions in runnable Python — try changing the flow vector or the target position:

Python

import math

def is_reachable(pos_from, pos_to, flow, v_max):
  """Cone feasibility check for a straight-line connection."""
  dx = pos_to[0] - pos_from[0]
  dy = pos_to[1] - pos_from[1]
  cx, cy = flow
  c_norm = math.hypot(cx, cy)
  d_norm = math.hypot(dx, dy)
  if d_norm < 1e-9:
      return False
  if c_norm < v_max:
      return True           # weak flow: all directions reachable
  cos_beta     = (dx*cx + dy*cy) / (d_norm * c_norm)
  cos_beta_max = math.sqrt(c_norm**2 - v_max**2) / c_norm
  return cos_beta >= cos_beta_max

def time_cost(pos_from, pos_to, flow, v_max):
  """Minimum travel time (analytic solution, Soulignac 2011 extended to 3D)."""
  dx = pos_to[0] - pos_from[0]
  dy = pos_to[1] - pos_from[1]
  cx, cy = flow
  A   = cx**2 + cy**2 - v_max**2
  B   = -2 * (dx*cx + dy*cy)
  C   = dx**2 + dy**2
  if abs(A) < 1e-12:          # flow speed == v_max: linear case
      return C / (-B) if -B > 1e-9 else float('inf')
  disc = B**2 - 4*A*C
  if disc < 0:
      return float('inf')     # infeasible (same result as cone check)
  t = (-B - math.sqrt(disc)) / (2*A)
  return t if t > 1e-9 else float('inf')

# --- Experiment ---
A_gyre, s = 0.02, 1.0
v_max     = 0.05

def double_gyre(x, y):
  u = -math.pi * A_gyre * math.sin(math.pi*x/s) * math.cos(math.pi*y/s)
  v =  math.pi * A_gyre * math.cos(math.pi*x/s) * math.sin(math.pi*y/s)
  return (u, v)

start   = (0.1, 0.1)
targets = [(0.3, 0.1), (1.0, 1.0), (1.9, 0.9), (0.1, 1.9), (0.0, 0.1)]
flow    = double_gyre(start[0], start[1])
c_norm  = math.hypot(*flow)

print(f"Start: {start}")
print(f"Flow at start: ({flow[0]:.4f}, {flow[1]:.4f}),  speed = {c_norm:.4f} m/s")
print(f"v_max = {v_max},  ratio c/v_max = {c_norm/v_max:.2f}  ({'strong' if c_norm >= v_max else 'weak'} flow)")
print()
print(f"{'Target':<18} {'Reachable':<12} {'Time (s)'}")
print("-" * 42)
for tgt in targets:
  ok   = is_reachable(start, tgt, flow, v_max)
  cost = time_cost(start, tgt, flow, v_max)
  cost_str = f"{cost:.2f}" if math.isfinite(cost) else "inf"
  print(f"  {str(tgt):<16} {str(ok):<12} {cost_str}")

Click "Run" to see output...

The Algorithm in Action

FlowFMT* replaces the standard ball-shaped neighborhood of FMT* with two cone-aware neighborhood sets:

Posterior neighborhood of vertex $v$ : samples reachable from $v$ (inside $v$ ‘s forward cone, within radius $r_n$ ).
Anterior neighborhood of vertex $v$ : samples from which $v$ can be reached (vertices whose forward cone contains $v$ , within radius $r_n$ ).

Building the tree: starting from the source, the algorithm expands the lowest-cost open vertex, connecting it to unvisited posterior neighbors through their best anterior neighbor already in the open set. No cone-infeasible edges are ever evaluated.

The Policy version roots the tree at the goal and expands outward to cover the entire space, producing a vector field that gives the optimal heading from any position.

Below: watch FlowFMT* grow a tree across the double-gyre, then reveal the optimal path. Start is blue, goal is green. Increase the sample count for a denser tree and better path quality. Click Reset to replay the animation.

Sample count: 200

v_max (m/s): 0.05

Start X: 0.1

Start Y: 0.1

Goal X: 1.9

Goal Y: 0.9

Neighborhood r: 0.46

The tree grows outward from Start following the cheapest-cost wave front. Notice how the edges respect the flow field: branches lean with the gyres rather than crossing them at steep angles. Once the tree reaches Goal, the optimal path (thick blue line) is traced back.

With more samples, the path quality improves as the cost estimate approaches the true optimum. Computational time stays low — this is one of the advantages of graph-based methods over optimal control solvers, which need a good initial guess and can get stuck in the wrong homotopy (going “the wrong way around” a vortex).

Summary

FlowFMT* makes three contributions over standard FMT*:

Cone-aware neighborhoods — only cone-feasible connections are ever added to the search graph, eliminating the need to post-check feasibility edge by edge.
Correct cost functions — both minimum-time and minimum-energy costs are derived analytically from the vehicle kinematics, handling the strong-flow regime properly.
Policy form — rooting the tree at the goal and expanding outward produces a vector field policy. If the vehicle drifts due to a fault, the policy guides it back to an optimal trajectory from wherever it ends up — no replanning needed.

Paper: B. Martinez Rocamora Jr. and G. A. S. Pereira, “Optimal Policies for Autonomous Navigation in Strong Currents Using Fast Marching Trees,” Autonomous Robots, 48, 27, 2024. doi:10.1007/s10514-024-10179-z

Code: bitbucket.org/wvufarolab/flowfmtstar