From 6f807f8828a7dc80283397c6070d3e389832915f Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 3 Oct 2026 21:16:06 -0700 Subject: [PATCH] Add Olfati-Saber flocking simulation --- PathPlanning/Flocking/flocking.py | 155 ++++++++++++++++++ README.md | 11 ++ .../flocking/flocking_main.rst | 87 ++++++++++ .../5_path_planning/path_planning_main.rst | 1 + tests/test_flocking.py | 115 +++++++++++++ 5 files changed, 369 insertions(+) create mode 100644 PathPlanning/Flocking/flocking.py create mode 100644 docs/modules/5_path_planning/flocking/flocking_main.rst create mode 100644 tests/test_flocking.py diff --git a/PathPlanning/Flocking/flocking.py b/PathPlanning/Flocking/flocking.py new file mode 100644 index 0000000000..f2d588a9a0 --- /dev/null +++ b/PathPlanning/Flocking/flocking.py @@ -0,0 +1,155 @@ +"""Olfati-Saber's Algorithm 2 for flocking in an obstacle-free plane. + +Run this file to animate 25 double-integrator agents following a moving +reference. Arrows show velocities; lines connect neighbors within range. + +Reference: R. Olfati-Saber, Flocking for Multi-Agent Dynamic Systems: +Algorithms and Theory, IEEE TAC 51(3), 401-420, 2006. +https://doi.org/10.1109/TAC.2005.864190 +""" + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +from matplotlib.collections import LineCollection +import numpy as np + +EPSILON = 0.1 +DESIRED_DISTANCE = 5.0 +INTERACTION_RANGE = 1.2 * DESIRED_DISTANCE +BUMP_H = 0.2 +POTENTIAL_GAIN = 5.0 # Symmetric action function: a = b = 5, c = 0. +POSITION_GAIN = 0.1 +VELOCITY_GAIN = 2.0 * np.sqrt(POSITION_GAIN) +DT = 0.02 +show_animation = True + + +def sigma_norm(vectors): + """Smooth norm of vectors along the last axis (equation 8).""" + squared_norm = np.sum(np.asarray(vectors) ** 2, axis=-1) + # Rationalized form avoids cancellation close to zero. + return squared_norm / (np.sqrt(1.0 + EPSILON * squared_norm) + 1.0) + + +def bump_function(z): + """Smooth adjacency weight, with a finite cutoff at z = 1 (equation 10).""" + phase = np.clip((np.asarray(z) - BUMP_H) / (1.0 - BUMP_H), 0.0, 1.0) + return np.where(np.asarray(z) >= 0.0, 0.5 * (1.0 + np.cos(np.pi * phase)), 0.0) + + +def interaction_terms(positions, velocities): + """Return separation/cohesion and alignment accelerations (equation 23). + + Both inputs have shape (number of agents, 2). All agents use the same + state snapshot; self-interactions are excluded from the adjacency matrix. + """ + displacement = positions[np.newaxis, :, :] - positions[:, np.newaxis, :] + distance = sigma_norm(displacement) + adjacency = bump_function(distance / sigma_norm([INTERACTION_RANGE])) + np.fill_diagonal(adjacency, 0.0) + + offset = distance - sigma_norm([DESIRED_DISTANCE]) + action = POTENTIAL_GAIN * adjacency * offset / np.sqrt(1.0 + offset ** 2) + direction = displacement / (1.0 + EPSILON * distance[:, :, np.newaxis]) + gradient = np.sum(action[:, :, np.newaxis] * direction, axis=1) + + velocity_difference = velocities[np.newaxis, :, :] - velocities[:, np.newaxis, :] + alignment = np.sum(adjacency[:, :, np.newaxis] * velocity_difference, axis=1) + return gradient, alignment + + +def flocking_control(positions, velocities, reference_position, reference_velocity): + """Compute Algorithm 2 acceleration with linear navigation (equation 24).""" + gradient, alignment = interaction_terms(positions, velocities) + navigation = (-POSITION_GAIN * (positions - reference_position) + - VELOCITY_GAIN * (velocities - reference_velocity)) + return gradient + alignment + navigation + + +def simulate(simulation_time=40.0, dt=DT, seed=0): + """Return times, position/velocity histories and moving reference positions. + + The fixed-seed perturbed grid avoids coincident initial agents. Controls + are held over each step, integrating the double-integrator model exactly + for that constant acceleration. The reference has constant velocity. + """ + rng = np.random.default_rng(seed) + x, y = np.meshgrid(np.arange(5), np.arange(5)) + initial_positions = 4.5 * np.column_stack((x.ravel(), y.ravel())) + initial_positions += rng.uniform(-0.6, 0.6, initial_positions.shape) + times = np.arange(int(round(simulation_time / dt)) + 1) * dt + positions = np.empty((len(times), len(initial_positions), 2)) + velocities = np.empty_like(positions) + positions[0] = initial_positions + velocities[0] = rng.uniform(-1.0, 1.0, initial_positions.shape) + reference_velocity = np.array([1.0, 0.5]) + reference_start = initial_positions.mean(axis=0) + [4.0, 2.0] + references = reference_start + times[:, np.newaxis] * reference_velocity + + for k in range(len(times) - 1): + acceleration = flocking_control(positions[k], velocities[k], + references[k], reference_velocity) + positions[k + 1] = positions[k] + dt * velocities[k] + 0.5 * dt ** 2 * acceleration + velocities[k + 1] = velocities[k] + dt * acceleration + return times, positions, velocities, references + + +def create_animation(times, positions, velocities, references): + """Animate a simulation result; the returned animation can also be saved.""" + fig, (ax, error_ax) = plt.subplots(1, 2, figsize=(10, 4.5)) + colors = plt.colormaps["viridis"](np.linspace(0.1, 0.9, positions.shape[1])) + edges = LineCollection([], colors="0.8", linewidths=0.7, zorder=1) + ax.add_collection(edges) + agents = ax.scatter(*positions[0].T, c=colors, s=25, zorder=3, label="Agents") + arrows = ax.quiver(*positions[0].T, *velocities[0].T, color=colors, + angles="xy", scale_units="xy", scale=0.6) + reference, = ax.plot([], [], "r*", markersize=12, label="Moving reference") + ax.plot(*references.T, "r--", alpha=0.4) + all_points = np.concatenate((positions.reshape(-1, 2), references)) + ax.set(xlim=(all_points[:, 0].min() - 3, all_points[:, 0].max() + 3), + ylim=(all_points[:, 1].min() - 3, all_points[:, 1].max() + 3), + xlabel="x [m]", ylabel="y [m]", aspect="equal") + ax.legend(loc="upper left") + ax.grid(True) + + reference_velocity = (references[-1] - references[0]) / (times[-1] - times[0]) + velocity_error = velocities - reference_velocity + rms_error = np.sqrt(np.mean(np.sum(velocity_error ** 2, axis=2), axis=1)) + error_ax.plot(times, rms_error, color="0.85") + error_line, = error_ax.plot([], [], color="tab:blue") + error_ax.set(xlabel="Time [s]", ylabel="RMS velocity error [m/s]", + title="Velocity convergence", xlim=(times[0], times[-1]), + ylim=(0.0, max(0.1, rms_error.max() * 1.1))) + error_ax.grid(True) + fig.tight_layout() + + def update(k): + q, p = positions[k], velocities[k] + distance = np.linalg.norm(q[:, np.newaxis, :] - q[np.newaxis, :, :], axis=2) + i, j = np.nonzero(np.triu(distance < INTERACTION_RANGE, k=1)) + edges.set_segments(np.stack((q[i], q[j]), axis=1)) + agents.set_offsets(q) + arrows.set_offsets(q) + arrows.set_UVC(*p.T) + reference.set_data([references[k, 0]], [references[k, 1]]) + error_line.set_data(times[:k + 1], rms_error[:k + 1]) + ax.set_title(f"Olfati-Saber flocking: t = {times[k]:.1f} s") + return edges, agents, arrows, reference, error_line + + stride = max(1, int(round(0.1 / (times[1] - times[0])))) + frames = list(range(0, len(times) - 1, stride)) + [len(times) - 1] + return FuncAnimation(fig, update, frames=frames, interval=100, repeat=False) + + +def main(): + """Run the flocking example with optional Matplotlib animation.""" + result = simulate() + if show_animation: + animation = create_animation(*result) + plt.show() + return animation + return None + + +if __name__ == "__main__": + main() diff --git a/README.md b/README.md index f828d90e1d..049c8be8da 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index. * [FastSLAM 1.0](#fastslam-10) * [Path Planning](#path-planning) * [Dynamic Window Approach](#dynamic-window-approach) + * [Flocking](#flocking) * [Grid based search](#grid-based-search) * [Dijkstra algorithm](#dijkstra-algorithm) * [A* algorithm](#a-algorithm) @@ -294,6 +295,16 @@ This is a 2D navigation sample code with Dynamic Window Approach. ![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif) +## Flocking + +Olfati-Saber’s Algorithm 2 coordinates a group of agents through local spacing +and velocity alignment while following a moving reference in free space. + +![Flocking](https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/ca4eb94b0876aa666e44b2125bcce450f7594f30/PathPlanning/Flocking/animation.gif) + +- [Algorithm documentation](https://atsushisakai.github.io/PythonRobotics/modules/5_path_planning/flocking/flocking.html) +- [Flocking for Multi-Agent Dynamic Systems: Algorithms and Theory](https://doi.org/10.1109/TAC.2005.864190) + ## Grid based search ### Dijkstra algorithm diff --git a/docs/modules/5_path_planning/flocking/flocking_main.rst b/docs/modules/5_path_planning/flocking/flocking_main.rst new file mode 100644 index 0000000000..dffcab6867 --- /dev/null +++ b/docs/modules/5_path_planning/flocking/flocking_main.rst @@ -0,0 +1,87 @@ +Flocking +======== + +Olfati-Saber's Algorithm 2 coordinates double-integrator agents in free space: +:math:`\dot q_i=p_i,\ \dot p_i=u_i`. +Nearby agents adjust their spacing and velocities while tracking a shared, +constant-velocity reference :math:`(q_r,p_r)`. + +.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/ca4eb94b0876aa666e44b2125bcce450f7594f30/PathPlanning/Flocking/animation.gif + :alt: Twenty-five agents forming a flock and following a moving reference + +Local interactions +------------------ + +For :math:`\epsilon>0`, use the smooth distance and its gradient: + +.. math:: + + \|z\|_\sigma=\frac{\sqrt{1+\epsilon\|z\|^2}-1}{\epsilon}, + \qquad n_{ij}=\frac{q_j-q_i}{\sqrt{1+\epsilon\|q_j-q_i\|^2}}. + +The adjacency weight vanishes beyond interaction range :math:`r`: + +.. math:: + + \rho_h(s)=\begin{cases} + 1 & 0\le s`__, IEEE Transactions on Automatic +Control, 51(3), 401–420, 2006. +`Author manuscript `__. diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst index 5132760dc5..61bb428f43 100644 --- a/docs/modules/5_path_planning/path_planning_main.rst +++ b/docs/modules/5_path_planning/path_planning_main.rst @@ -10,6 +10,7 @@ Path planning is the ability of a robot to search feasible and efficient path to :caption: Contents dynamic_window_approach/dynamic_window_approach + flocking/flocking bugplanner/bugplanner grid_base_search/grid_base_search time_based_grid_search/time_based_grid_search diff --git a/tests/test_flocking.py b/tests/test_flocking.py new file mode 100644 index 0000000000..e5980060c0 --- /dev/null +++ b/tests/test_flocking.py @@ -0,0 +1,115 @@ +import conftest # Add root path to sys.path +import matplotlib.pyplot as plt +from matplotlib.animation import PillowWriter +import numpy as np +from numpy.testing import assert_allclose +import pytest + +from PathPlanning.Flocking import flocking as m + + +def test_sigma_norm_and_gradient_at_zero(): + assert m.sigma_norm([0.0, 0.0]) == 0.0 + assert_allclose(m.sigma_norm([3.0, 4.0]), + (np.sqrt(1.0 + 25.0 * m.EPSILON) - 1.0) / m.EPSILON) + assert m.sigma_norm([1e-10, 0.0]) > 0.0 + gradient, alignment = m.interaction_terms(np.zeros((2, 2)), np.zeros((2, 2))) + assert_allclose(gradient, 0.0) + assert_allclose(alignment, 0.0) + + +def test_bump_cutoff_and_transition(): + values = m.bump_function(np.array([-0.1, 0.0, m.BUMP_H, + (1.0 + m.BUMP_H) / 2, 1.0, 1.1])) + assert_allclose(values, [0.0, 1.0, 1.0, 0.5, 0.0, 0.0], atol=1e-15) + + +@pytest.mark.parametrize("distance, sign", [(2.5, -1), (5.0, 0), (5.5, 1), + (6.0, 0), (7.0, 0)]) +def test_pair_repulsion_equilibrium_attraction_and_cutoff(distance, sign): + positions = np.array([[0.0, 0.0], [distance, 0.0]]) + gradient, _ = m.interaction_terms(positions, np.zeros((2, 2))) + assert np.sign(gradient[0, 0]) == sign + assert_allclose(gradient[0], -gradient[1]) + assert_allclose(gradient[:, 1], 0.0) + + +def test_alignment_dissipates_relative_velocity_and_has_finite_range(): + positions = np.array([[0.0, 0.0], [m.DESIRED_DISTANCE, 0.0]]) + velocities = np.array([[2.0, -1.0], [-1.0, 3.0]]) + _, alignment = m.interaction_terms(positions, velocities) + assert_allclose(alignment.sum(axis=0), 0.0) + assert np.sum(velocities * alignment) < 0.0 + _, matched = m.interaction_terms(positions, np.ones((2, 2))) + assert_allclose(matched, 0.0) + positions[1, 0] = m.INTERACTION_RANGE + _, disconnected = m.interaction_terms(positions, velocities) + assert_allclose(disconnected, 0.0) + + +def test_centroid_acceleration_depends_only_on_navigation(): + rng = np.random.default_rng(5) + positions = rng.uniform(-5.0, 5.0, (10, 2)) + velocities = rng.normal(size=(10, 2)) + reference, reference_velocity = np.array([7.0, 3.0]), np.array([1.0, 0.5]) + acceleration = m.flocking_control(positions, velocities, reference, reference_velocity) + expected = (-m.POSITION_GAIN * (positions.mean(axis=0) - reference) + - m.VELOCITY_GAIN * (velocities.mean(axis=0) - reference_velocity)) + assert_allclose(acceleration.mean(axis=0), expected, atol=1e-14) + # With one agent, the navigation feedback is the entire controller. + assert_allclose(m.flocking_control(positions[:1], velocities[:1], + positions[0], velocities[0]), 0.0) + + +def test_controller_is_invariant_to_agent_order_and_coordinate_frame(): + rng = np.random.default_rng(4) + q, p = rng.normal(size=(2, 6, 2)) + qr, pr = np.array([5.0, 2.0]), np.array([1.0, 0.5]) + rotation = np.array([[0.0, -1.0], [1.0, 0.0]]) + translation, boost = np.array([4.0, -8.0]), np.array([-2.0, 3.0]) + order = np.array([3, 0, 5, 2, 4, 1]) + expected = m.flocking_control(q, p, qr, pr)[order] @ rotation + transformed = m.flocking_control(q[order] @ rotation + translation, + p[order] @ rotation + boost, + qr @ rotation + translation, pr @ rotation + boost) + assert_allclose(transformed, expected, atol=1e-13) + + +@pytest.mark.parametrize("seed", [0, 1, 2]) +def test_flock_converges_without_collisions_for_demo_initial_conditions(seed): + times, q, p, references = m.simulate(seed=seed) + assert np.isfinite(q).all() and np.isfinite(p).all() + velocity_error = np.sqrt(np.mean(np.sum((p[-1] - [1.0, 0.5]) ** 2, axis=1))) + assert velocity_error < 0.01 + assert np.linalg.norm(q[-1].mean(axis=0) - references[-1]) < 0.01 + distance = np.linalg.norm(q[:, :, None, :] - q[:, None, :, :], axis=-1) + distance[:, np.arange(25), np.arange(25)] = np.inf + assert distance.min() > 3.0 + adjacency = (distance[-1] < m.INTERACTION_RANGE).astype(float) + laplacian = np.diag(adjacency.sum(axis=1)) - adjacency + assert np.linalg.eigvalsh(laplacian)[1] > 0.1 # One connected flock. + + # Independent closed-form solution for critically damped centroid motion. + omega = np.sqrt(m.POSITION_GAIN) + e0 = q[0].mean(axis=0) - references[0] + v0 = p[0].mean(axis=0) - [1.0, 0.5] + t = times[:, None] + expected_error = (e0 + (v0 + omega * e0) * t) * np.exp(-omega * t) + assert_allclose(q.mean(axis=1) - references, expected_error, atol=0.025) + + +def test_animation_can_be_rendered(tmp_path): + animation = m.create_animation(*m.simulate(simulation_time=0.1)) + output = tmp_path / "flocking.gif" + animation.save(output, writer=PillowWriter(fps=10)) + assert output.stat().st_size > 0 + plt.close("all") + + +def test_main_without_animation(monkeypatch): + monkeypatch.setattr(m, "show_animation", False) + m.main() + + +if __name__ == "__main__": + conftest.run_this_test(__file__)