Velocity obstacles¶

cuplan.VelocityObstacleSim — decentralized collision avoidance with velocity obstacles (Fiorini and Shiller 1998, Motion planning in dynamic environments using velocity obstacles, IJRR 17(7)), mirroring pymapf.decentralized.velocity_obstacle.

Semantics¶

Each timestep, each agent:

computes a desired velocity — full speed toward its goal, zero once within radius / 5 of it;
builds, for every other agent and moving obstacle, a collision cone widened to 2.2 x radius and translated by that obstacle's velocity, expressed as two half-planes;
samples candidate velocities on a polar grid (20 angles x 5 speeds by default, as in pymapf), discards samples inside any cone, and takes the feasible sample closest to the desired velocity — or stops when nothing is feasible.

One deliberate difference from pymapf¶

Updates are synchronous: all agents choose against the same snapshot of the world, then move together. pymapf updates agents in registration order within a timestep — earlier agents do not see later ones at all. The synchronous rule is order-independent, which is what makes it parallel, and is the standard formulation of the decentralized problem. Because the two simulators integrate different dynamics, the benchmark reports cuplan CPU vs CUDA only for this family rather than a misleading cross-library wall-clock number.

Parallelization¶

Every (agent, sample) pair is independent — the step is one embarrassingly parallel evaluation:

CPU reference — one broadcast expression over (agents, others, samples).
CUDA — one thread per (agent, sample), looping over the others; the per-agent argmin is a device-side reduction.

CPU and CUDA trajectories agree to floating-point tolerance (tested).

Decentralized collision avoidance with velocity obstacles.

Mirrors pymapf.decentralized.velocity_obstacle (Fiorini and Shiller 1998): each agent samples candidate velocities on a polar grid, discards those inside any neighbour's collision cone — widened to 2.2 x radius and translated by the neighbour's velocity, expressed as a pair of half-planes — and takes the feasible sample closest to its desired velocity toward the goal.

Every (agent, sample) pair is independent: the whole step is one embarrassingly parallel evaluation, n_agents x n_samples threads on the CUDA backend, one broadcast expression on the NumPy one.

One deliberate difference from pymapf: updates are synchronous. All agents choose their velocity against the same snapshot of the world, then move together. pymapf updates agents in registration order inside a timestep, so earlier agents ignore later ones; the synchronous rule is order-independent, which is what makes it parallel — and is also the standard formulation of the decentralized problem.

VOResult `dataclass` ¶

Trajectories and summary metrics of a velocity-obstacle run.

Source code in cuplan/velocity_obstacles.py

@dataclass
class VOResult:
    """Trajectories and summary metrics of a velocity-obstacle run."""

    positions: np.ndarray  # (T+1, n_agents, 2)
    velocities: np.ndarray  # (T, n_agents, 2)
    runtime: float
    backend: str
    extra: dict = field(default_factory=dict)

    def goals_reached(self, goals: np.ndarray, tolerance: float) -> int:
        """Number of agents ending within ``tolerance`` of their goal."""
        final = self.positions[-1]
        return int(
            (np.linalg.norm(final - goals, axis=1) <= tolerance).sum()
        )

    def min_separation(self) -> float:
        """Smallest pairwise agent distance over the whole run."""
        n = self.positions.shape[1]
        if n < 2:
            return float("inf")
        best = np.inf
        for frame in self.positions:
            diff = frame[:, None, :] - frame[None, :, :]
            dist = np.linalg.norm(diff, axis=-1)
            dist[np.arange(n), np.arange(n)] = np.inf
            best = min(best, float(dist.min()))
        return best

goals_reached ¶

goals_reached(goals, tolerance)

Number of agents ending within tolerance of their goal.

Source code in cuplan/velocity_obstacles.py

def goals_reached(self, goals: np.ndarray, tolerance: float) -> int:
    """Number of agents ending within ``tolerance`` of their goal."""
    final = self.positions[-1]
    return int(
        (np.linalg.norm(final - goals, axis=1) <= tolerance).sum()
    )

min_separation ¶

min_separation()

Smallest pairwise agent distance over the whole run.

Source code in cuplan/velocity_obstacles.py

def min_separation(self) -> float:
    """Smallest pairwise agent distance over the whole run."""
    n = self.positions.shape[1]
    if n < 2:
        return float("inf")
    best = np.inf
    for frame in self.positions:
        diff = frame[:, None, :] - frame[None, :, :]
        dist = np.linalg.norm(diff, axis=-1)
        dist[np.arange(n), np.arange(n)] = np.inf
        best = min(best, float(dist.min()))
    return best