Feat/showcase gallery expansion

README.md

@@ -137,15 +137,15 @@ python tutorials/multi_robot_3d_reachavoid.py
   <tr>
     <td align="center" width="33%">
       <img src="media/showcase/risk_aware_cvar.gif" width="100%" alt="Ellipsoidal-obstacle CBF"><br>
-      <sub><b>Ellipsoidal-obstacle CBF</b> <a href="scripts/render_showcase.py" title="Source: render_risk_aware_cvar">🔗</a><br>Unicycle reach-goal with linear class-K</sub>
+      <sub><b>Ellipsoidal-obstacle CBF</b> <a href="examples/unicycle/reach_goal/ellipsoidal_obstacle_cbf.py" title="Source: examples/unicycle/reach_goal/ellipsoidal_obstacle_cbf.py">🔗</a><br>Unicycle reach-goal with linear class-K</sub>
     </td>
     <td align="center" width="33%">
       <img src="media/showcase/stochastic_cbf.gif" width="100%" alt="Stochastic CBF"><br>
-      <sub><b>Stochastic CBF (SDE)</b> <a href="scripts/render_showcase.py" title="Source: render_stochastic_cbf">🔗</a><br>Safety under Brownian disturbance</sub>
+      <sub><b>Stochastic CBF (SDE)</b> <a href="examples/unicycle/reach_goal/stochastic_cbf.py" title="Source: examples/unicycle/reach_goal/stochastic_cbf.py">🔗</a><br>Safety under Brownian disturbance</sub>
     </td>
     <td align="center" width="33%">
       <img src="media/showcase/robust_cbf.gif" width="100%" alt="Robust CBF"><br>
-      <sub><b>Robust CBF</b> <a href="scripts/render_showcase.py" title="Source: render_robust_cbf">🔗</a><br>Worst-case bounded disturbance</sub>
+      <sub><b>Robust CBF</b> <a href="examples/unicycle/reach_goal/robust_cbf.py" title="Source: examples/unicycle/reach_goal/robust_cbf.py">🔗</a><br>Worst-case bounded disturbance</sub>
     </td>
   </tr>
   <tr>
@@ -155,17 +155,17 @@ python tutorials/multi_robot_3d_reachavoid.py
     </td>
     <td align="center">
       <img src="media/showcase/mppi_stl.gif" width="100%" alt="MPPI reach-avoid"><br>
-      <sub><b>MPPI reach-avoid</b> <a href="scripts/render_showcase.py" title="Source: render_mppi_stl">🔗</a><br>Sampling-based planning with goal + obstacle cost</sub>
+      <sub><b>MPPI reach-avoid</b> <a href="examples/single_integrator/mppi_reach_avoid.py" title="Source: examples/single_integrator/mppi_reach_avoid.py">🔗</a><br>Sampling-based planning with goal + obstacle cost</sub>
     </td>
     <td align="center">
       <img src="media/showcase/multi_robot_2d.gif" width="100%" alt="Multi-robot 2D coordination"><br>
-      <sub><b>Multi-robot 2D</b> <a href="scripts/render_showcase.py" title="Source: render_multi_robot_2d">🔗</a><br>Coordination via shared CBFs</sub>
+      <sub><b>Multi-robot 2D</b> <a href="examples/single_integrator/multi_robot_coordination.py" title="Source: examples/single_integrator/multi_robot_coordination.py">🔗</a><br>Coordination via shared CBFs</sub>
     </td>
   </tr>
   <tr>
     <td align="center">
       <img src="media/showcase/fixed_wing_3d.gif" width="100%" alt="Fixed-wing aerial 3D"><br>
-      <sub><b>Fixed-wing aerial 3D</b> <a href="scripts/render_showcase.py" title="Source: render_fixed_wing_3d">🔗</a><br>UAV reach-drop-point in 3D</sub>
+      <sub><b>Fixed-wing aerial 3D</b> <a href="examples/fixed_wing/reach_drop_point/ekf.py" title="Source: examples/fixed_wing/reach_drop_point/ekf.py">🔗</a><br>UAV reach-drop-point in 3D</sub>
     </td>
     <td align="center">
       <img src="media/showcase/pedestrian_head_on.gif" width="100%" alt="Pedestrian head-on"><br>
@@ -179,21 +179,21 @@ python tutorials/multi_robot_3d_reachavoid.py
   <tr>
     <td align="center" width="33%">
       <img src="media/showcase/van_der_pol_clf.gif" width="100%" alt="Van der Pol CLF"><br>
-      <sub><b>Van der Pol (CLF)</b> <a href="scripts/render_showcase.py" title="Source: render_van_der_pol_clf">🔗</a><br>Nonlinear regulation to the origin</sub>
+      <sub><b>Van der Pol (CLF)</b> <a href="examples/van_der_pol/regulation/perfect_sensing.py" title="Source: examples/van_der_pol/regulation/perfect_sensing.py">🔗</a><br>Nonlinear regulation to the origin</sub>
     </td>
     <td align="center" width="33%">
       <img src="media/showcase/mpc_double_integrator.gif" width="100%" alt="Model Predictive Control"><br>
-      <sub><b>Model Predictive Control</b> <a href="scripts/render_showcase.py" title="Source: render_mpc_double_integrator">🔗</a><br>Receding-horizon LTI tracking</sub>
+      <sub><b>Model Predictive Control</b> <a href="examples/double_integrator/mpc_tracking.py" title="Source: examples/double_integrator/mpc_tracking.py">🔗</a><br>Receding-horizon LTI tracking</sub>
     </td>
     <td align="center" width="33%">
       <img src="media/showcase/quadrotor_6dof.gif" width="100%" alt="Quadrotor 6-DOF geometric tracking"><br>
-      <sub><b>Quadrotor 6-DOF</b> <a href="scripts/render_showcase.py" title="Source: render_quadrotor_6dof">🔗</a><br>Geometric SE(3) tracking + altitude CBF</sub>
+      <sub><b>Quadrotor 6-DOF</b> <a href="examples/quadrotor_6dof/geometric_tracking.py" title="Source: examples/quadrotor_6dof/geometric_tracking.py">🔗</a><br>Geometric SE(3) tracking + altitude CBF</sub>
     </td>
   </tr>
   <tr>
     <td align="center" colspan="3">
       <img src="media/showcase/monte_carlo_safety.gif" width="45%" alt="Monte Carlo safety verification"><br>
-      <sub><b>Monte Carlo safety verification</b> <a href="scripts/render_showcase.py" title="Source: render_monte_carlo_safety">🔗</a><br>200 stochastic CBF rollouts (<code>jax.vmap</code>), live empirical risk</sub>
+      <sub><b>Monte Carlo safety verification</b> <a href="examples/single_integrator/monte_carlo_safety.py" title="Source: examples/single_integrator/monte_carlo_safety.py">🔗</a><br>200 stochastic CBF rollouts (<code>jax.vmap</code>), live empirical risk</sub>
     </td>
   </tr>
 </table>

examples/double_integrator/mpc_tracking.py

@@ -0,0 +1,99 @@
+"""Receding-horizon MPC tracking a goal with an LTI double integrator."""
+import os
+import sys
+
+# Add the project root to the path so we can import cbfkit + examples.
+root_path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../.."))
+if root_path not in sys.path:
+    sys.path.insert(0, root_path)
+
+import jax.numpy as jnp
+import numpy as np
+
+from cbfkit.optimization.mpc.quadratic_cost_linear_dynamics import (
+    generate_mpc_solver_quadratic_cost_linear_dynamics,
+)
+
+# In test mode we shorten the horizon loop and skip the (slow) GIF render.
+TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE"))
+
+
+def main() -> float:
+    """Run the receding-horizon MPC loop and (optionally) save the animation.
+
+    Returns the final Euclidean position error to the goal.
+    """
+    dt = 0.1
+    # Discrete-time double integrator: state [px, py, vx, vy], control [ax, ay].
+    A = jnp.array(
+        [[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
+    )
+    B = jnp.array([[0.0, 0.0], [0.0, 0.0], [dt, 0.0], [0.0, dt]])
+    Q = jnp.diag(jnp.array([10.0, 10.0, 1.0, 1.0]))
+    R = 0.1 * jnp.eye(2)
+    Qn = 50.0 * Q
+    N = 20
+    solve = generate_mpc_solver_quadratic_cost_linear_dynamics(A, B, Q, R, Qn, N)
+
+    goal = jnp.array([4.0, 4.0, 0.0, 0.0])
+    ref_horizon = jnp.tile(goal, (N, 1))  # (N, 4) constant reference over the horizon
+    x = jnp.array([0.0, 0.0, 0.0, 0.0])
+    n_steps = 40 if not TEST_MODE else 5
+
+    xs = [np.asarray(x)]
+    preds = []
+    for _ in range(n_steps):
+        concatenated_x_xr = jnp.vstack([x.reshape(1, -1), ref_horizon])  # (N+1, 4)
+        x_opt, u_opt = solve(concatenated_x_xr)  # x_opt (4, N+1), u_opt (2, N)
+        u = u_opt[:, 0]
+        x = A @ x + B @ u
+        xs.append(np.asarray(x))
+        preds.append(np.asarray(x_opt.T))  # (N+1, 4) predicted state horizon
+    xs = np.array(xs)
+
+    final_err = float(np.linalg.norm(xs[-1, :2] - np.asarray(goal)[:2]))
+    print(f"Final position error to goal: {final_err:.4f}")
+
+    if TEST_MODE:
+        return final_err
+
+    import matplotlib.pyplot as plt
+    from matplotlib.animation import FuncAnimation, PillowWriter
+
+    fig, ax = plt.subplots(figsize=(6, 6))
+    ax.plot(float(goal[0]), float(goal[1]), "g*", markersize=18, label="Goal")
+    (realized,) = ax.plot([], [], "b-", lw=2, label="Realized")
+    (pred,) = ax.plot(
+        [], [], color="orange", ls="--", lw=1.5, alpha=0.85, label="Predicted horizon"
+    )
+    dot = ax.scatter([], [], s=80, color="blue", zorder=5)
+    ax.set_xlim(-0.5, 4.5)
+    ax.set_ylim(-0.5, 4.5)
+    ax.set_aspect("equal")
+    ax.set_xlabel("x")
+    ax.set_ylabel("y")
+    ax.set_title("Model Predictive Control — receding-horizon LTI tracking", fontsize=10)
+    ax.legend(loc="lower right", fontsize=9)
+    ax.grid(True, alpha=0.3)
+
+    def update(i):
+        realized.set_data(xs[: i + 1, 0], xs[: i + 1, 1])
+        dot.set_offsets([[xs[i, 0], xs[i, 1]]])
+        p = preds[min(i, len(preds) - 1)]
+        pred.set_data(p[:, 0], p[:, 1])
+        return realized, pred, dot
+
+    anim = FuncAnimation(fig, update, frames=len(xs), interval=100, blit=True)
+
+    results_dir = os.path.join(os.path.dirname(__file__), "results")
+    os.makedirs(results_dir, exist_ok=True)
+    out = os.path.join(results_dir, "mpc_tracking.gif")
+    anim.save(out, writer=PillowWriter(fps=10))
+    plt.close(fig)
+    print(f"Saved animation to {out}")
+
+    return final_err
+
+
+if __name__ == "__main__":
+    main()

examples/quadrotor_6dof/geometric_tracking.py

@@ -0,0 +1,150 @@
+"""6-DOF quadrotor geometric SE(3) tracking with a live altitude-CBF barrier value."""
+import os
+import sys
+
+# Add the project root to the path so cbfkit + examples imports resolve.
+root_path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../.."))
+if root_path not in sys.path:
+    sys.path.insert(0, root_path)
+
+import jax.numpy as jnp
+import numpy as np
+
+import cbfkit.simulation.simulator as sim
+from cbfkit.estimators import naive as estimator
+from cbfkit.integration import runge_kutta_4 as integrator
+from cbfkit.sensors import perfect as sensor
+from cbfkit.systems.quadrotor_6dof.certificates.barrier_functions import h_alt
+from cbfkit.systems.quadrotor_6dof.controllers.geometric import geometric_controller
+from cbfkit.systems.quadrotor_6dof.models.quadrotor_6dof_dynamics import (
+    quadrotor_6dof_dynamics,
+)
+
+TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE"))
+
+
+def main() -> None:
+    # Mass/inertia must be consistent between plant and controller: geometric_controller's
+    # default gains are tuned for m≈4.34 kg, while quadrotor_6dof_dynamics defaults to
+    # m=0.25 kg. Mismatch -> instant integration NaN. Use the heavier plant.
+    m, jx, jy, jz = 4.34, 0.0820, 0.0845, 0.1377
+    three_tuple = quadrotor_6dof_dynamics(m=m, jx=jx, jy=jy, jz=jz)
+
+    def dyn(x):
+        f, g, _s = three_tuple(x)
+        return f, g
+
+    desired = jnp.array([2.0, 1.5, 3.0])  # target (pn, pe, h)
+    dt = 0.01
+    tf = 6.0 if not TEST_MODE else 0.5
+    n = int(tf / dt)
+
+    # state layout: [pn, pe, h, u, v, w, phi, theta, psi, p, q, r]
+    x0 = jnp.array([0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+
+    nominal = geometric_controller(
+        dynamics=dyn, desired_state=desired, dt=dt, m=m, jx=jx, jy=jy, jz=jz
+    )
+
+    res = sim.execute(
+        x0=x0,
+        dt=dt,
+        num_steps=n,
+        dynamics=dyn,
+        integrator=integrator,
+        nominal_controller=nominal,
+        sensor=sensor,
+        estimator=estimator,
+        use_jit=True,
+    )
+    states = np.asarray(res["states"])  # (n+1, 12)
+
+    # Altitude-CBF barrier value h_alt(z, alt_limit). z = hstack([x, t]).
+    # alt_limit must comfortably exceed our setpoint altitude (3 m) — pick 5 m.
+    alt_limit = 5.0
+    n_states_full = states.shape[0]
+    ts = np.linspace(0.0, tf, n_states_full)
+    h_vals = np.array(
+        [
+            float(h_alt(jnp.hstack([jnp.asarray(states[i]), jnp.asarray(ts[i])]), alt_limit))
+            for i in range(n_states_full)
+        ]
+    )
+
+    final_pos = states[-1, :3]
+    dist = float(np.linalg.norm(final_pos - np.asarray(desired)))
+    print(f"Final distance to goal: {dist:.4f} m")
+    print(f"Minimum altitude-CBF value h_alt: {float(h_vals.min()):.4f} (positive => safe)")
+
+    if TEST_MODE:
+        return
+
+    import matplotlib.pyplot as plt
+    from matplotlib.animation import FuncAnimation, PillowWriter
+    from mpl_toolkits.mplot3d import Axes3D  # noqa: F401 — registers 3D projection
+
+    # Subsample frames for a compact GIF.
+    stride = max(1, n_states_full // 80)
+    idx = np.arange(0, n_states_full, stride)
+    pn, pe, h_alt_traj = states[idx, 0], states[idx, 1], states[idx, 2]
+
+    fig = plt.figure(figsize=(10, 5))
+    ax3d = fig.add_subplot(1, 2, 1, projection="3d")
+    ax_h = fig.add_subplot(1, 2, 2)
+
+    ax3d.scatter(
+        [float(desired[0])],
+        [float(desired[1])],
+        [float(desired[2])],
+        color="green",
+        s=120,
+        marker="*",
+        label="Goal",
+        zorder=10,
+    )
+    (line3d,) = ax3d.plot([], [], [], "b-", lw=2, label="Quadrotor")
+    dot3d = ax3d.scatter([], [], [], s=60, color="blue", zorder=11)
+    pad = 0.5
+    ax3d.set_xlim(min(pn.min(), float(desired[0])) - pad, max(pn.max(), float(desired[0])) + pad)
+    ax3d.set_ylim(min(pe.min(), float(desired[1])) - pad, max(pe.max(), float(desired[1])) + pad)
+    ax3d.set_zlim(0, alt_limit + 0.5)
+    ax3d.set_xlabel("pn [m]")
+    ax3d.set_ylabel("pe [m]")
+    ax3d.set_zlabel("h [m]")
+    ax3d.set_title("Quadrotor 6-DOF — geometric SE(3) tracking", fontsize=10)
+    ax3d.legend(loc="upper right", fontsize=8)
+    ax3d.view_init(elev=22, azim=-60)
+
+    # h(z) trace: stays >0 ⇒ altitude envelope satisfied.
+    ax_h.plot(ts, h_vals, color="purple", lw=1.5)
+    (h_dot,) = ax_h.plot([], [], "o", color="purple", markersize=7)
+    ax_h.axhline(0.0, color="red", ls="--", lw=1, alpha=0.7, label="Safety boundary h=0")
+    ax_h.set_xlim(0, tf)
+    ax_h.set_ylim(min(0.0, float(h_vals.min())) - 0.1, max(1.0, float(h_vals.max())) + 0.1)
+    ax_h.set_xlabel("t [s]")
+    ax_h.set_ylabel("$h_{\\rm alt}(z)$")
+    ax_h.set_title("Altitude-CBF barrier value (positive ⇒ safe)", fontsize=10)
+    ax_h.legend(loc="lower right", fontsize=8)
+    ax_h.grid(True, alpha=0.3)
+
+    def update(i):
+        line3d.set_data(pn[: i + 1], pe[: i + 1])
+        line3d.set_3d_properties(h_alt_traj[: i + 1])
+        dot3d._offsets3d = ([pn[i]], [pe[i]], [h_alt_traj[i]])
+        # Map subsampled index back to full-resolution h_vals index for the dot.
+        full_i = idx[i]
+        h_dot.set_data([ts[full_i]], [h_vals[full_i]])
+        return line3d, dot3d, h_dot
+
+    anim = FuncAnimation(fig, update, frames=len(idx), interval=100, blit=False)
+
+    results_dir = os.path.join(os.path.dirname(__file__), "results")
+    os.makedirs(results_dir, exist_ok=True)
+    out = os.path.join(results_dir, "geometric_tracking.gif")
+    anim.save(out, writer=PillowWriter(fps=10))
+    plt.close(fig)
+    print(f"Saved animation to {out}")
+
+
+if __name__ == "__main__":
+    main()