Fix 3D multi-robot safety metric, threshold display, and animation output path

src/cbfkit/utils/visualization.py

@@ -7,7 +7,6 @@
   matplotlib (MP4/GIF), and Manim (high-quality MP4) backends.
 """
 
-import warnings
 from typing import Any, List, Optional
 
 import numpy as np
@@ -173,6 +172,7 @@ def visualize_3d_multi_robot(
     include_min_distance_to_obstacles_plot: bool = False,
     threshold: Optional[float] = None,
     backend: str = "plotly",
+    safety_radius: float = 0.25,
 ):
     """Animate a 3D multi-robot system with optional distance subplots.
 
@@ -192,6 +192,14 @@ def visualize_3d_multi_robot(
         ``"plotly"`` (default), ``"matplotlib"``, or ``"manim"``.
         Manim accepts a quality suffix: ``"manim-low"`` (default),
         ``"manim-medium"``, ``"manim-high"``, ``"manim-production"``.
+    threshold : float, optional
+        Minimum-separation threshold drawn as a dashed reference line on the
+        inter-robot distance panel.  Pass the same value the controller
+        enforces so the plot can be read as satisfied / violated.
+    safety_radius : float
+        Radius of the per-robot safety bubble drawn by the manim backend.  Two
+        bubbles touch at ``2 * safety_radius``; pass ``threshold / 2`` to keep
+        the drawing consistent with the enforced constraint.
     """
     from cbfkit.utils.visualizations.helpers_3d import _compute_distance_metrics
 
@@ -276,20 +284,6 @@ def visualize_3d_multi_robot(
         from cbfkit.utils.visualizations.manim_3d_multi_robot import render_multi_robot_3d
 
         _require_manim()
-        if include_min_distance_plot:
-            warnings.warn(
-                "Manim backend does not support inline subplot panels. "
-                "include_min_distance_plot will be ignored.",
-                UserWarning,
-                stacklevel=2,
-            )
-        if include_min_distance_to_obstacles_plot:
-            warnings.warn(
-                "Manim backend does not support inline subplot panels. "
-                "include_min_distance_to_obstacles_plot will be ignored.",
-                UserWarning,
-                stacklevel=2,
-            )
         save_path = animation_filename if save_animation else None
         return render_multi_robot_3d(
             states=states,
@@ -307,9 +301,11 @@ def visualize_3d_multi_robot(
             ellipse_rotations=ellipse_rotations,
             save_path=save_path,
             quality=quality,
+            safety_radius=safety_radius,
             goal_dists=goal_dists,
             min_dists=min_dists if include_min_distance_plot else None,
             obs_dists=obs_dists if include_min_distance_to_obstacles_plot else None,
+            threshold=threshold,
         )
     else:
         from cbfkit.utils.visualizations.matplotlib_3d_multi_robot import _visualize_3d_matplotlib

src/cbfkit/utils/visualizations/helpers_3d.py

@@ -29,7 +29,7 @@ def _point_to_ellipsoid_distance(p, c, r, R):
         return -np.min(r)
 
     u_normalized = u / norm_u
-    inv_r_squared = np.diag(1.0 / (r ** 2))
+    inv_r_squared = np.diag(1.0 / (r**2))
     K = u_normalized.T @ R @ inv_r_squared @ R.T @ u_normalized
     s = 1.0 / np.sqrt(K)
     x = c + s * u_normalized
@@ -60,9 +60,17 @@ def _ellipsoid_mesh(center, radii, rotation, n=12):
     return pts[:, 0], pts[:, 1], pts[:, 2], ii, jj, kk
 
 
-def _compute_distance_metrics(states, desired_states, num_robots, sdim,
-                              ellipse_centers, ellipse_radii, ellipse_rotations,
-                              include_min_dist, include_obs_dist):
+def _compute_distance_metrics(
+    states,
+    desired_states,
+    num_robots,
+    sdim,
+    ellipse_centers,
+    ellipse_radii,
+    ellipse_rotations,
+    include_min_dist,
+    include_obs_dist,
+):
     """Pre-compute distance arrays used by both backends."""
     N = len(states)
 
@@ -71,15 +79,19 @@ def _compute_distance_metrics(states, desired_states, num_robots, sdim,
     for i in range(num_robots):
         idx = sdim * i
         goal_dists[:, i] = np.linalg.norm(
-            states[:, idx:idx + 3] - desired_states[idx:idx + 3], axis=1,
+            states[:, idx : idx + 3] - desired_states[idx : idx + 3],
+            axis=1,
         )
 
-    # Min inter-robot distances (vectorized over robots)
+    # Min inter-robot distances (vectorized over robots).
+    # Only the first 3 columns of each robot's block are positions; including
+    # the remaining state (e.g. velocity) would report sqrt(|dp|^2 + |dv|^2),
+    # which overstates separation exactly when robots converge at speed.
     min_dists = None
     if include_min_dist:
         min_dists = np.zeros((N, num_robots))
         for t in range(N):
-            positions = states[t, :].reshape(num_robots, sdim)
+            positions = states[t, : num_robots * sdim].reshape(num_robots, sdim)[:, :3]
             diffs = positions[:, np.newaxis, :] - positions[np.newaxis, :, :]
             dists = np.linalg.norm(diffs, axis=2)
             np.fill_diagonal(dists, np.inf)
@@ -93,11 +105,14 @@ def _compute_distance_metrics(states, desired_states, num_robots, sdim,
         for t in range(N):
             for i in range(num_robots):
                 idx = sdim * i
-                p = states[t, idx:idx + 3]
+                p = states[t, idx : idx + 3]
                 dmin = np.inf
                 for j in range(num_obs):
                     d = _point_to_ellipsoid_distance(
-                        p, ellipse_centers[j], ellipse_radii[j], ellipse_rotations[j],
+                        p,
+                        ellipse_centers[j],
+                        ellipse_radii[j],
+                        ellipse_rotations[j],
                     )
                     dmin = min(dmin, d)
                 obs_dists[t, i] = dmin

src/cbfkit/utils/visualizations/manim_3d_multi_robot.py

@@ -27,6 +27,7 @@
         RIGHT,
         UP,
         Axes,
+        DashedLine,
         Dot3D,
         Line,
         Sphere,
@@ -86,21 +87,34 @@ def _build_chart_panel(
     num_robots: int,
     panel_width: float = 3.8,
     panel_height: float = 2.0,
+    threshold: float | None = None,
 ):
     """Build a 2D Axes with pre-drawn static line segments for each robot.
 
-    Returns ``(axes, title_mob, robot_lines)`` where ``robot_lines[i]`` is a
-    :class:`VGroup` of ``Line`` segments for robot *i* that will be revealed
-    progressively by an updater.
+    Returns ``(axes, title_mob, x_label, robot_lines, panel)`` where
+    ``robot_lines[i]`` is a :class:`VGroup` of ``Line`` segments for robot *i*
+    that will be revealed progressively by an updater.
 
     Parameters
     ----------
     data : np.ndarray
         Shape ``(N, num_robots)`` — one column per robot.
+    threshold : float, optional
+        Safety threshold; drawn as a dashed red horizontal line so a viewer can
+        tell satisfaction from violation.  Matches the plotly / matplotlib
+        backends, which draw the same reference line.
     """
     N = len(data)
     t_max = (N - 1) * dt
     y_max = float(np.nanmax(data)) * 1.15
+    if not np.isfinite(y_max):
+        # All-NaN data, or the all-inf min_dists a single robot produces.
+        # max() below would propagate NaN from its left argument.
+        y_max = 0.0
+    if threshold is not None and threshold >= 0:
+        # Keep the line on-screen even when every sample sits below it
+        # (i.e. when the constraint is violated throughout).
+        y_max = max(y_max, float(threshold) * 1.15)
     if y_max < 1e-6:
         y_max = 1.0
 
@@ -133,6 +147,15 @@ def _build_chart_panel(
         robot_lines.append(segs)
 
     panel = VGroup(axes, title_mob, x_label, *robot_lines)
+    if threshold is not None and threshold >= 0:
+        panel.add(
+            DashedLine(
+                axes.c2p(0, float(threshold)),
+                axes.c2p(t_max, float(threshold)),
+                stroke_width=2,
+                color="#ff0000",
+            )
+        )
     return axes, title_mob, x_label, robot_lines, panel
 
 
@@ -166,6 +189,8 @@ class MultiRobot3DScene(ThreeDScene):
     goal_dists: np.ndarray | None = None
     min_dists: np.ndarray | None = None
     obs_dists: np.ndarray | None = None
+    # Safety threshold drawn on the inter-robot panel (None = no reference line)
+    threshold: float | None = None
     # Scale factor to fit data into Manim's coordinate system (default ~7 units)
     _scale: float = 1.0
 
@@ -302,23 +327,24 @@ def updater(mob):
 
         panel_configs = []
         if has_goal:
-            panel_configs.append(("Dist to Goal", self.goal_dists))
+            panel_configs.append(("Dist to Goal", self.goal_dists, None))
         if has_min:
-            panel_configs.append(("Min Inter-Robot Dist", self.min_dists))
+            panel_configs.append(("Min Inter-Robot Dist", self.min_dists, self.threshold))
         if has_obs:
-            panel_configs.append(("Min Obstacle Dist", self.obs_dists))
+            panel_configs.append(("Min Obstacle Dist", self.obs_dists, None))
 
         panel_height = min(2.0, 5.5 / max(n_panels, 1))
         panel_gap = 0.4
 
-        for p_idx, (p_title, p_data) in enumerate(panel_configs):
+        for p_idx, (p_title, p_data, p_threshold) in enumerate(panel_configs):
             _, _, _, robot_lines, panel = _build_chart_panel(
                 title_text=p_title,
                 data=p_data,
                 dt=self.dt,
                 num_robots=n_robots,
                 panel_width=3.8,
                 panel_height=panel_height,
+                threshold=p_threshold,
             )
             all_robot_lines.append(robot_lines)
             panels_group.add(panel)
@@ -430,6 +456,7 @@ def render_multi_robot_3d(
     goal_dists: np.ndarray | None = None,
     min_dists: np.ndarray | None = None,
     obs_dists: np.ndarray | None = None,
+    threshold: float | None = None,
 ) -> str:
     """Render a multi-robot 3D animation using Manim.
 
@@ -440,7 +467,11 @@ def render_multi_robot_3d(
     desired_states : np.ndarray
         ``(state_dim,)`` goal vector.
     safety_radius : float
-        Per-robot collision avoidance bubble radius.
+        Per-robot collision avoidance bubble radius.  Two bubbles touch when
+        the robots are ``2 * safety_radius`` apart, so this should be half the
+        minimum separation the controller actually enforces.
+    threshold : float, optional
+        Minimum-separation threshold marked on the inter-robot distance panel.
     ellipse_centers : list, optional
         List of obstacle center positions ``[x, y, z]``.
     ellipse_radii : list, optional
@@ -466,13 +497,21 @@ def render_multi_robot_3d(
     """
     _require_manim()
 
+    import glob
     import os
+    import shutil
 
     # Configure Manim output
     config.quality = quality
     if save_path:
-        config.output_file = os.path.basename(save_path)
+        # Manim appends its own extension, so hand it the stem: passing
+        # "anim.gif" produced "anim.gif.mp4".  The format must be set
+        # explicitly or a .gif request silently renders MP4.
+        stem, ext = os.path.splitext(os.path.basename(save_path))
+        config.output_file = stem
         config.media_dir = os.path.dirname(save_path) or "./media"
+        if ext.lower() == ".gif":
+            config.format = "gif"
 
     # Inject data into the scene class
     MultiRobot3DScene.states = np.asarray(states)
@@ -492,9 +531,31 @@ def render_multi_robot_3d(
     MultiRobot3DScene.goal_dists = goal_dists
     MultiRobot3DScene.min_dists = min_dists
     MultiRobot3DScene.obs_dists = obs_dists
+    MultiRobot3DScene.threshold = threshold
 
     scene = MultiRobot3DScene()
     scene.render()
 
-    # Return path to rendered file
-    return str(scene.renderer.file_writer.movie_file_path)
+    rendered = str(scene.renderer.file_writer.movie_file_path)
+    if not save_path:
+        return rendered
+
+    # For GIF output the file writer still reports the .mp4 name, so fall back
+    # to the newest matching file under the media dir.
+    if not os.path.exists(rendered):
+        pattern = os.path.join(config.media_dir, "videos", "**", f"*{ext or '.mp4'}")
+        candidates = glob.glob(pattern, recursive=True)
+        if not candidates:
+            raise FileNotFoundError(
+                f"Manim did not produce a {ext or '.mp4'} file under {config.media_dir!r}."
+            )
+        rendered = max(candidates, key=os.path.getmtime)
+
+    # Manim writes under <media_dir>/videos/<quality>/; copy to the path the
+    # caller asked for so the returned path is the file that actually exists.
+    # Without this the caller's path silently keeps whatever was there before.
+    if os.path.abspath(rendered) != os.path.abspath(save_path):
+        out_dir = os.path.dirname(os.path.abspath(save_path))
+        os.makedirs(out_dir, exist_ok=True)
+        shutil.copy2(rendered, save_path)
+    return os.path.abspath(save_path)

src/cbfkit/utils/visualizations/matplotlib_3d_multi_robot.py

@@ -8,12 +8,27 @@
 
 
 def _visualize_3d_matplotlib(
-    states, desired_states, desired_state_radius, num_robots,
-    ellipse_centers, ellipse_radii, ellipse_rotations,
-    x_lim, y_lim, z_lim, dt, sdim, title, save_animation,
-    animation_filename, include_min_distance_plot,
-    include_min_distance_to_obstacles_plot, threshold,
-    goal_dists, min_dists, obs_dists,
+    states,
+    desired_states,
+    desired_state_radius,
+    num_robots,
+    ellipse_centers,
+    ellipse_radii,
+    ellipse_rotations,
+    x_lim,
+    y_lim,
+    z_lim,
+    dt,
+    sdim,
+    title,
+    save_animation,
+    animation_filename,
+    include_min_distance_plot,
+    include_min_distance_to_obstacles_plot,
+    threshold,
+    goal_dists,
+    min_dists,
+    obs_dists,
 ):
     import matplotlib.pyplot as plt
     from matplotlib.animation import FuncAnimation
@@ -61,8 +76,12 @@ def _visualize_3d_matplotlib(
         color = colors[i % num_robots]
 
         ax_traj.scatter(
-            desired_states[idx], desired_states[idx + 1], desired_states[idx + 2],
-            color=color, s=50, label=f"Desired State {i + 1}",
+            desired_states[idx],
+            desired_states[idx + 1],
+            desired_states[idx + 2],
+            color=color,
+            s=50,
+            label=f"Desired State {i + 1}",
         )
         u = np.linspace(0, 2 * np.pi, 50)
         v = np.linspace(0, np.pi, 50)
@@ -89,7 +108,15 @@ def _visualize_3d_matplotlib(
         ax_min_dist.set_ylabel("Minimum Distance [m]")
         ax_min_dist.set_title("Min Distance Between Robots")
         ax_min_dist.grid(True)
-        ax_min_dist.set_ylim(0, float(np.max(min_dists)) * 1.1)
+        # Include the threshold in the range, else a run that violates the
+        # constraint throughout renders as a clean plot with the reference
+        # line clipped off-screen.
+        md_top = float(np.max(min_dists)) * 1.1
+        if not np.isfinite(md_top):
+            md_top = 0.0
+        if threshold is not None and threshold >= 0:
+            md_top = max(md_top, float(threshold) * 1.15)
+        ax_min_dist.set_ylim(0, md_top if md_top > 1e-6 else 1.0)
         if threshold is not None:
             ax_min_dist.axhline(y=threshold, color="red", linestyle="--", label="Threshold")
         for i in range(num_robots):
@@ -145,8 +172,12 @@ def update(num):
         return artists
 
     ani = FuncAnimation(
-        fig, update, frames=N,
-        init_func=init, blit=True, interval=dt * 1000,
+        fig,
+        update,
+        frames=N,
+        init_func=init,
+        blit=True,
+        interval=dt * 1000,
     )
 
     plt.tight_layout()