diff --git a/README.md b/README.md index e14c11ad..607de288 100755 --- a/README.md +++ b/README.md @@ -137,15 +137,15 @@ python tutorials/multi_robot_3d_reachavoid.py Ellipsoidal-obstacle CBF
- Ellipsoidal-obstacle CBF 🔗
Unicycle reach-goal with linear class-K + Ellipsoidal-obstacle CBF 🔗
Unicycle reach-goal with linear class-K Stochastic CBF
- Stochastic CBF (SDE) 🔗
Safety under Brownian disturbance + Stochastic CBF (SDE) 🔗
Safety under Brownian disturbance Robust CBF
- Robust CBF 🔗
Worst-case bounded disturbance + Robust CBF 🔗
Worst-case bounded disturbance @@ -155,17 +155,17 @@ python tutorials/multi_robot_3d_reachavoid.py MPPI reach-avoid
- MPPI reach-avoid 🔗
Sampling-based planning with goal + obstacle cost + MPPI reach-avoid 🔗
Sampling-based planning with goal + obstacle cost Multi-robot 2D coordination
- Multi-robot 2D 🔗
Coordination via shared CBFs + Multi-robot 2D 🔗
Coordination via shared CBFs Fixed-wing aerial 3D
- Fixed-wing aerial 3D 🔗
UAV reach-drop-point in 3D + Fixed-wing aerial 3D 🔗
UAV reach-drop-point in 3D Pedestrian head-on
@@ -179,21 +179,21 @@ python tutorials/multi_robot_3d_reachavoid.py Van der Pol CLF
- Van der Pol (CLF) 🔗
Nonlinear regulation to the origin + Van der Pol (CLF) 🔗
Nonlinear regulation to the origin Model Predictive Control
- Model Predictive Control 🔗
Receding-horizon LTI tracking + Model Predictive Control 🔗
Receding-horizon LTI tracking Quadrotor 6-DOF geometric tracking
- Quadrotor 6-DOF 🔗
Geometric SE(3) tracking + altitude CBF + Quadrotor 6-DOF 🔗
Geometric SE(3) tracking + altitude CBF Monte Carlo safety verification
- Monte Carlo safety verification 🔗
200 stochastic CBF rollouts (jax.vmap), live empirical risk + Monte Carlo safety verification 🔗
200 stochastic CBF rollouts (jax.vmap), live empirical risk diff --git a/examples/double_integrator/__init__.py b/examples/double_integrator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/examples/double_integrator/mpc_tracking.py b/examples/double_integrator/mpc_tracking.py new file mode 100644 index 00000000..53eda273 --- /dev/null +++ b/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() diff --git a/examples/quadrotor_6dof/__init__.py b/examples/quadrotor_6dof/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/examples/quadrotor_6dof/geometric_tracking.py b/examples/quadrotor_6dof/geometric_tracking.py new file mode 100644 index 00000000..1c913ef6 --- /dev/null +++ b/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() diff --git a/examples/single_integrator/monte_carlo_safety.py b/examples/single_integrator/monte_carlo_safety.py new file mode 100644 index 00000000..c2ee5bc7 --- /dev/null +++ b/examples/single_integrator/monte_carlo_safety.py @@ -0,0 +1,238 @@ +"""GPU/vmap Monte Carlo safety verification: 200 stochastic single-integrator CBF-QP rollouts.""" +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 contextlib + +import jax.numpy as jnp +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np +from jax import random +from matplotlib.animation import FuncAnimation, PillowWriter +from matplotlib.collections import LineCollection +from matplotlib.lines import Line2D + +from cbfkit.controllers.cbf_clf.cbf_clf_qp_generator import cbf_clf_qp_generator +from cbfkit.controllers.cbf_clf.generate_constraints import ( + generate_compute_vanilla_clf_constraints, + generate_compute_zeroing_cbf_constraints, +) +from cbfkit.integration import forward_euler +from cbfkit.modeling.additive_disturbances import generate_stochastic_perturbation +from cbfkit.simulation.monte_carlo_gpu import MonteCarloSetup, conduct_monte_carlo_gpu +from cbfkit.utils.user_types import CertificateCollection, ControllerData, PlannerData + +# --- Scenario (verified-clean: low alpha keeps the jaxopt QP stable under vmap) --- +GOAL = jnp.array([4.0, 4.0]) +OBS = jnp.array([2.0, 2.0]) +R = 0.6 +ALPHA = 1.0 +NOISE = 0.4 +DT = 0.05 +# CBFKIT_TEST_MODE: short horizon, few trials, and skip the GIF render entirely. +TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE")) +NSTEPS = 20 if TEST_MODE else 100 +N_TRIALS = 20 if TEST_MODE else 200 + +RESULTS_DIR = os.path.join(os.path.dirname(__file__), "results") +GIF_PATH = os.path.join(RESULTS_DIR, "monte_carlo_safety.gif") + + +def dynamics(x): + return jnp.zeros(2), jnp.eye(2) + + +# h(x) = ||x - c||^2 - r^2, relative-degree-1 zeroing barrier (single integrator). +f_h = lambda _t, x: jnp.sum((x - OBS) ** 2) - R**2 # noqa: E731 +j_h = lambda _t, x: 2.0 * (x - OBS) # noqa: E731 +h_h = lambda _t, _x: 2.0 * jnp.eye(2) # noqa: E731 +p_h = lambda _t, _x: 0.0 # noqa: E731 +a_h = lambda h: ALPHA * h # noqa: E731 +barriers = CertificateCollection([f_h], [j_h], [h_h], [p_h], [a_h]) + +controller = cbf_clf_qp_generator( + generate_compute_zeroing_cbf_constraints, + generate_compute_vanilla_clf_constraints, +)( + control_limits=jnp.array([8.0, 8.0]), + dynamics_func=dynamics, + barriers=barriers, + relaxable_cbf=False, + relaxable_clf=True, +) + + +def nominal_controller(t, x, _key, _ref): + return 2.0 * (GOAL - x), None + + +def initial_state_sampler(key): + return jnp.array([0.0, 0.0]) + 0.18 * random.normal(key, (2,)) + + +def _sensor(t, x, *, sigma=None, key=None): + return x + + +def _estimator(t, y, z, u, c): + return y, (c if c is not None else jnp.zeros((len(y), len(y)))) + + +# The CBF-QP controller emits batched jax.debug.print spam under vmap (every branch of +# its status lax.switch fires); silence it at the fd level around the kernel run. +@contextlib.contextmanager +def _silence_fds(): + saved = os.dup(1), os.dup(2) + devnull = os.open(os.devnull, os.O_WRONLY) + os.dup2(devnull, 1) + os.dup2(devnull, 2) + try: + yield + finally: + os.dup2(saved[0], 1) + os.dup2(saved[1], 2) + os.close(devnull) + os.close(saved[0]) + os.close(saved[1]) + + +def main(): + # Pass the perturbation UNWRAPPED so its `.is_increment` flag survives (Euler-Maruyama). + perturbation = generate_stochastic_perturbation(sigma=lambda x: NOISE * jnp.eye(2), dt=DT) + + _, c_data = controller(0.0, jnp.zeros(2), jnp.zeros(2), random.PRNGKey(0), ControllerData()) + setup = MonteCarloSetup( + dt=DT, + num_steps=NSTEPS, + dynamics=dynamics, + integrator=forward_euler, + initial_state_sampler=initial_state_sampler, + nominal_controller=nominal_controller, + controller=controller, + sensor=_sensor, + estimator=_estimator, + perturbation=perturbation, + sigma=jnp.zeros(0), + controller_data=c_data, + planner=None, + planner_data=PlannerData(), + ) + + print(f"[monte_carlo_safety] running {N_TRIALS} vmap'd stochastic rollouts...") + with _silence_fds(): + results = conduct_monte_carlo_gpu(setup, n_trials=N_TRIALS, seed=0) + states = np.asarray(results.states) # (N_TRIALS, NSTEPS, 2) + print(f"[monte_carlo_safety] kernel wall time: {results.wall_time_s:.2f}s") + + # Geometric safety check (independent of the controller's internal barrier bookkeeping). + dist = np.linalg.norm(states - np.asarray(OBS), axis=-1) # (N, NSTEPS) + inside = dist < R # (N, NSTEPS) + ever_inside = inside.any(axis=1) # (N,) + # Cumulative empirical violation rate up to each step. + cum_viol_rate = np.array([float(inside[:, : k + 1].any(axis=1).mean()) for k in range(NSTEPS)]) + overall_rate = float(ever_inside.mean()) + print( + f"[monte_carlo_safety] overall empirical violation rate: {overall_rate:.3f} " + f"(min dist to obstacle center {dist.min():.3f}, R={R})" + ) + + if TEST_MODE: + # Fast path: skip the GIF render, just report the safety metric. + print(f"[monte_carlo_safety] CBFKIT_TEST_MODE: skipping GIF render.") + return overall_rate + + # Draw a representative subset to keep the GIF small (the full 200-line translucent tangle + # bloats the palette), but ALWAYS include every breaching trial so the red paths shown stay + # consistent with the empirical-risk counter, which is computed over ALL N_TRIALS. + N_DRAW = 60 + rng = np.random.default_rng(0) + viol_idx = np.flatnonzero(ever_inside) + safe_idx = np.flatnonzero(~ever_inside) + n_safe_draw = min(len(safe_idx), max(0, N_DRAW - len(viol_idx))) + safe_draw = rng.choice(safe_idx, size=n_safe_draw, replace=False) + draw_idx = np.concatenate([safe_draw, viol_idx]).astype(int) + draw_states = states[draw_idx] # (N_DRAW, NSTEPS, 2) + draw_colors = ["tab:red" if ever_inside[i] else "tab:blue" for i in draw_idx] + + fig, ax = plt.subplots(figsize=(5.0, 5.0)) + ax.add_patch(plt.Circle((float(OBS[0]), float(OBS[1])), R, color="red", alpha=0.3, zorder=1)) + ax.add_patch( + plt.Circle((float(OBS[0]), float(OBS[1])), R, fill=False, color="red", lw=1.5, zorder=2) + ) + ax.plot(float(GOAL[0]), float(GOAL[1]), "g*", markersize=18, zorder=6) + ax.plot(0.0, 0.0, "ks", markersize=6, zorder=6) + + lc = LineCollection([], colors=draw_colors, linewidths=0.5, alpha=0.3, zorder=3) + ax.add_collection(lc) + dots = ax.scatter( + draw_states[:, 0, 0], draw_states[:, 0, 1], s=6, c=draw_colors, alpha=0.7, zorder=4 + ) + txt = ax.text( + 0.03, + 0.97, + "", + transform=ax.transAxes, + va="top", + ha="left", + fontsize=9, + family="monospace", + bbox=dict(boxstyle="round", facecolor="white", alpha=0.85), + ) + + legend_handles = [ + Line2D([0], [0], color="tab:blue", lw=1.5, label="safe rollout"), + Line2D( + [0], [0], marker="*", color="w", markerfacecolor="g", markersize=12, lw=0, label="Goal" + ), + Line2D( + [0], [0], marker="s", color="w", markerfacecolor="k", markersize=7, lw=0, label="Start" + ), + ] + if len(viol_idx) > 0: + legend_handles.insert(1, Line2D([0], [0], color="tab:red", lw=1.5, label="breached")) + + ax.set_xlim(-1.0, 5.0) + ax.set_ylim(-1.0, 5.0) + ax.set_aspect("equal") + ax.set_xlabel("x") + ax.set_ylabel("y") + ax.set_title( + f"Monte Carlo safety verification — {N_TRIALS} stochastic CBF rollouts", fontsize=9 + ) + ax.legend(handles=legend_handles, loc="lower right", fontsize=8) + ax.grid(True, alpha=0.3) + + stride = max(1, NSTEPS // 30) + frame_idx = list(range(0, NSTEPS, stride)) + + def update(k): + lc.set_segments([draw_states[i, : k + 1, :] for i in range(len(draw_idx))]) + dots.set_offsets(draw_states[:, k, :]) + rate = cum_viol_rate[k] + n_viol = int(round(rate * N_TRIALS)) + txt.set_text( + f"step {k + 1:3d}/{NSTEPS}\n" + f"trials {N_TRIALS}\n" + f"violations {n_viol}\n" + f"empirical risk {rate * 100:4.1f}%" + ) + return lc, dots, txt + + os.makedirs(RESULTS_DIR, exist_ok=True) + anim = FuncAnimation(fig, update, frames=frame_idx, interval=100, blit=False) + anim.save(GIF_PATH, writer=PillowWriter(fps=10), dpi=80) + plt.close(fig) + print(f"[monte_carlo_safety] saved GIF -> {GIF_PATH}") + return overall_rate + + +if __name__ == "__main__": + main() diff --git a/examples/single_integrator/mppi_reach_avoid.py b/examples/single_integrator/mppi_reach_avoid.py new file mode 100644 index 00000000..7d94f9dc --- /dev/null +++ b/examples/single_integrator/mppi_reach_avoid.py @@ -0,0 +1,146 @@ +"""MPPI sampling-based reach-avoid planning for a 2D single 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 matplotlib + +matplotlib.use("Agg") +import numpy as np +from jax import Array, jit +from matplotlib.animation import FuncAnimation, PillowWriter + +import cbfkit.controllers.mppi as mppi_planner +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 + +# CBFKIT_TEST_MODE: short horizon and skip the GIF render entirely. +TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE")) + +DT = 0.1 +TF = 1.0 if TEST_MODE else 8.0 +N_STEPS = int(TF / DT) + 1 +x0 = jnp.array([0.0, 0.0]) +goal = jnp.array([4.0, 4.0]) +obstacle = jnp.array([3.0, 3.0]) +obstacle_radius = 0.6 + +RESULTS_DIR = os.path.join(os.path.dirname(__file__), "results") +GIF_PATH = os.path.join(RESULTS_DIR, "mppi_reach_avoid.gif") + + +def plant(): + def dynamics(x): + return jnp.zeros(2), jnp.eye(2) + + return dynamics + + +dynamics = plant() + + +@jit +def stage_cost(state_and_time: Array, action: Array) -> Array: + x = state_and_time + dist_goal_sq = (x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2 + margin = jnp.maximum(jnp.linalg.norm(x[0:2] - obstacle[0:2]) - obstacle_radius, 0.01) + return 5.0 * dist_goal_sq + 8.0 / margin + 0.1 * (action[0] ** 2 + action[1] ** 2) + + +@jit +def terminal_cost(state_and_time: Array, action: Array) -> Array: + x = state_and_time + return 50.0 * ((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) + + +def main(): + mppi_args = { + "robot_state_dim": 2, + "robot_control_dim": 2, + "prediction_horizon": 25, + "num_samples": 2000, + "plot_samples": 30, + "time_step": DT, + "use_GPU": False, + "costs_lambda": 0.03, + "cost_perturbation": 0.1, + } + planner = mppi_planner.vanilla_mppi( + control_limits=jnp.array([5.0, 5.0]), + dynamics_func=dynamics, + trajectory_cost=None, + stage_cost=stage_cost, + terminal_cost=terminal_cost, + mppi_args=mppi_args, + ) + + res = sim.execute( + x0=x0, + dt=DT, + num_steps=N_STEPS, + dynamics=dynamics, + integrator=integrator, + planner=planner, + nominal_controller=None, + controller=None, + sensor=sensor, + estimator=estimator, + planner_data={ + "u_traj": jnp.ones((mppi_args["prediction_horizon"], mppi_args["robot_control_dim"])), + }, + controller_data={}, + ) + states = np.asarray(res["states"]) + + final_dist = float(np.linalg.norm(states[-1] - np.asarray(goal))) + print(f"[mppi_reach_avoid] final distance to goal: {final_dist:.3f}") + + if TEST_MODE: + # Fast path: skip the GIF render, just report the reach metric. + print("[mppi_reach_avoid] CBFKIT_TEST_MODE: skipping GIF render.") + return final_dist + + import matplotlib.pyplot as plt + + fig, ax = plt.subplots(figsize=(6, 6)) + ax.add_patch( + plt.Circle( + (float(obstacle[0]), float(obstacle[1])), + obstacle_radius, + color="red", + alpha=0.35, + ) + ) + ax.plot(float(goal[0]), float(goal[1]), "g*", markersize=18, label="Goal") + (line,) = ax.plot([], [], "b-", lw=2) + dot = ax.scatter([], [], s=80, color="blue", zorder=5) + ax.set_xlim(-1, 7) + ax.set_ylim(-1, 7) + ax.set_aspect("equal") + ax.set_title("MPPI — sampling-based reach-avoid planning", fontsize=10) + ax.legend(loc="lower right", fontsize=9) + ax.grid(True, alpha=0.3) + + def update(i): + line.set_data(states[: i + 1, 0], states[: i + 1, 1]) + dot.set_offsets([[states[i, 0], states[i, 1]]]) + return line, dot + + stride = max(1, len(states) // 60) + os.makedirs(RESULTS_DIR, exist_ok=True) + anim = FuncAnimation(fig, update, frames=range(0, len(states), stride), interval=100, blit=True) + anim.save(GIF_PATH, writer=PillowWriter(fps=10)) + plt.close(fig) + print(f"[mppi_reach_avoid] saved GIF -> {GIF_PATH}") + return final_dist + + +if __name__ == "__main__": + main() diff --git a/examples/single_integrator/multi_robot_coordination.py b/examples/single_integrator/multi_robot_coordination.py new file mode 100644 index 00000000..92d0c338 --- /dev/null +++ b/examples/single_integrator/multi_robot_coordination.py @@ -0,0 +1,185 @@ +"""Multi-robot 2D coordination: 6 single integrators on a ring swapping to opposite positions via pairwise distance CBFs.""" +import os +import sys + +# Add the project root to the path so we can import cbfkit. +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 jax import jacfwd, jacrev + +import cbfkit.simulation.simulator as sim +from cbfkit.certificates.conditions.barrier_conditions import zeroing_barriers +from cbfkit.controllers.cbf_clf import vanilla_cbf_clf_qp_controller +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.utils.user_types import CertificateCollection + +# CBFKIT_TEST_MODE: short horizon + skip the GIF render/save entirely. +TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE")) + +NUM = 6 +DIM = 2 * NUM +RADIUS = 2.0 +SAFE_DIST = 0.55 +DT = 0.05 +TF = 0.5 if TEST_MODE else 4.0 + +RESULTS_DIR = os.path.join(os.path.dirname(__file__), "results") +GIF_PATH = os.path.join(RESULTS_DIR, "multi_robot_coordination.gif") + +INITIAL = np.zeros(DIM) +GOALS = np.zeros(DIM) +rng = np.random.default_rng(7) +for i in range(NUM): + ang = 2 * np.pi * i / NUM + rng.normal(0, 0.03) + INITIAL[2 * i] = RADIUS * np.cos(ang) + INITIAL[2 * i + 1] = RADIUS * np.sin(ang) + # Goal is opposite side of the ring. + GOALS[2 * i] = -RADIUS * np.cos(2 * np.pi * i / NUM) + GOALS[2 * i + 1] = -RADIUS * np.sin(2 * np.pi * i / NUM) +goal_arr = jnp.asarray(GOALS) + + +def dynamics(x): + return jnp.zeros(DIM), jnp.eye(DIM) + + +def nominal(t, x, *args, **kwargs): + u = -1.5 * (x - goal_arr) + return u, {} + + +# Build pairwise distance barriers: h = dx^2 + dy^2 - SAFE_DIST^2. +def make_h(i, j): + def h(t, x): + dx = x[2 * i] - x[2 * j] + dy = x[2 * i + 1] - x[2 * j + 1] + return dx * dx + dy * dy - SAFE_DIST**2 + + return h + + +funcs = [] +jacs = [] +hess = [] +partials = [] +conds = [] + +cond_factory = zeroing_barriers.linear_class_k(alpha=2.0) + +for i in range(NUM): + for j in range(i + 1, NUM): + h = make_h(i, j) + grad = jacfwd(lambda x, _h=h: _h(0.0, x)) + hess_fn = jacfwd(jacrev(lambda x, _h=h: _h(0.0, x))) + + def partial_t(t, x, _h=h): + return 0.0 + + funcs.append(h) + jacs.append(lambda t, x, _g=grad: _g(x)) + hess.append(lambda t, x, _H=hess_fn: _H(x)) + partials.append(partial_t) + conds.append(cond_factory) + +barriers = CertificateCollection( + functions=funcs, + jacobians=jacs, + hessians=hess, + partials=partials, + conditions=conds, +) + +controller = vanilla_cbf_clf_qp_controller( + control_limits=100.0 * jnp.ones(DIM), + nominal_input=nominal, + dynamics_func=dynamics, + barriers=barriers, +) + + +def main(): + N = int(TF / DT) + res = sim.execute( + x0=jnp.asarray(INITIAL), + dt=DT, + num_steps=N, + dynamics=dynamics, + integrator=integrator, + nominal_controller=nominal, + controller=controller, + sensor=sensor, + estimator=estimator, + ) + states = np.asarray(res["states"]) + + # Minimum pairwise distance over the whole run (safety metric, SAFE_DIST is the bound). + min_pair_dist = np.inf + for i in range(NUM): + for j in range(i + 1, NUM): + dx = states[:, 2 * i] - states[:, 2 * j] + dy = states[:, 2 * i + 1] - states[:, 2 * j + 1] + min_pair_dist = min(min_pair_dist, float(np.sqrt(dx * dx + dy * dy).min())) + print( + f"[multi_robot_coordination] min pairwise distance over run: {min_pair_dist:.3f} " + f"(safety bound SAFE_DIST={SAFE_DIST})" + ) + + if TEST_MODE: + # Fast path: skip the GIF render, just report the safety metric. + print("[multi_robot_coordination] CBFKIT_TEST_MODE: skipping GIF render.") + return min_pair_dist + + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + from matplotlib.animation import FuncAnimation, PillowWriter + + fig, ax = plt.subplots(figsize=(6, 6)) + cmap = plt.get_cmap("tab10") + colors = [cmap(i) for i in range(NUM)] + dots = [] + lines = [] + for i in range(NUM): + (ln,) = ax.plot([], [], "-", color=colors[i], lw=1.5, alpha=0.7) + dot = ax.scatter([], [], s=80, color=colors[i], zorder=5) + lines.append(ln) + dots.append(dot) + ax.plot( + float(GOALS[2 * i]), + float(GOALS[2 * i + 1]), + "*", + color=colors[i], + markersize=14, + markeredgecolor="black", + alpha=0.5, + ) + ax.set_xlim(-RADIUS - 1, RADIUS + 1) + ax.set_ylim(-RADIUS - 1, RADIUS + 1) + ax.set_aspect("equal") + ax.set_title(f"Multi-robot 2D coordination ({NUM} agents, pairwise CBF)", fontsize=10) + ax.grid(True, alpha=0.3) + + def update(k): + for i in range(NUM): + lines[i].set_data(states[: k + 1, 2 * i], states[: k + 1, 2 * i + 1]) + dots[i].set_offsets([[states[k, 2 * i], states[k, 2 * i + 1]]]) + return tuple(lines) + tuple(dots) + + os.makedirs(RESULTS_DIR, exist_ok=True) + stride = max(1, len(states) // 70) + anim = FuncAnimation(fig, update, frames=range(0, len(states), stride), interval=100, blit=True) + anim.save(GIF_PATH, writer=PillowWriter(fps=10)) + plt.close(fig) + print(f"[multi_robot_coordination] saved GIF -> {GIF_PATH}") + return min_pair_dist + + +if __name__ == "__main__": + main() diff --git a/examples/unicycle/reach_goal/ellipsoidal_obstacle_cbf.py b/examples/unicycle/reach_goal/ellipsoidal_obstacle_cbf.py new file mode 100644 index 00000000..34ea5e1e --- /dev/null +++ b/examples/unicycle/reach_goal/ellipsoidal_obstacle_cbf.py @@ -0,0 +1,122 @@ +"""Unicycle reach-goal with a vanilla CBF-CLF QP filter avoiding one ellipsoidal obstacle.""" +import os +import sys + +# Add the project root to the path so we can import 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 + +import cbfkit.simulation.simulator as sim +import cbfkit.systems.unicycle.models.olfatisaber2002approximate as unicycle +from cbfkit.certificates import concatenate_certificates, rectify_relative_degree +from cbfkit.certificates.conditions.barrier_conditions import zeroing_barriers +from cbfkit.controllers.cbf_clf import vanilla_cbf_clf_qp_controller +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.unicycle import proportional_controller +from cbfkit.utils.user_types import PlannerData +from examples.unicycle.common.ellipsoidal_obstacle import cbf as ellipsoid_cbf + +# Test mode: short horizon, skip the GIF render so tests stay fast. +TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE")) + +RESULTS_DIR = os.path.join(os.path.dirname(__file__), "results") +GIF_PATH = os.path.join(RESULTS_DIR, "ellipsoidal_obstacle_cbf.gif") + + +def main() -> str: + dyn = unicycle.plant(lam=1.0) + x0 = jnp.array([0.0, 0.0, jnp.pi / 2]) + xg = jnp.array([4.0, 4.0, 0.0]) + obs = jnp.array([2.0, 2.0, 0.0]) + ell = jnp.array([0.6, 0.6]) + barriers = concatenate_certificates( + rectify_relative_degree( + function=ellipsoid_cbf(obs, ell), + system_dynamics=dyn, + state_dim=3, + form="exponential", + roots=jnp.array([-1.0]), + )(certificate_conditions=zeroing_barriers.linear_class_k(alpha=2.0)) + ) + nominal = proportional_controller(dynamics=dyn, Kp_pos=1, Kp_theta=0.01) + controller = vanilla_cbf_clf_qp_controller( + control_limits=jnp.array([5.0, 5.0]), + nominal_input=nominal, + dynamics_func=dyn, + barriers=barriers, + ) + tf = 8.0 if not TEST_MODE else 1.0 + dt = 0.02 + n = int(tf / dt) + res = sim.execute( + x0=x0, + dt=dt, + num_steps=n, + dynamics=dyn, + integrator=integrator, + nominal_controller=nominal, + controller=controller, + sensor=sensor, + estimator=estimator, + planner_data=PlannerData( + u_traj=None, + x_traj=jnp.tile(xg.reshape(-1, 1), (1, n + 1)), + prev_robustness=None, + ), + use_jit=True, + ) + states = np.asarray(res["states"]) + + final_dist = float(np.linalg.norm(states[-1, :2] - np.asarray(xg[:2]))) + print(f"Final distance to goal: {final_dist:.4f}") + + if TEST_MODE: + return GIF_PATH + + import matplotlib.pyplot as plt + from matplotlib.animation import FuncAnimation, PillowWriter + + fig, ax = plt.subplots(figsize=(6, 6)) + ax.add_patch( + plt.matplotlib.patches.Ellipse( + (float(obs[0]), float(obs[1])), + float(ell[0]) * 2, + float(ell[1]) * 2, + facecolor="red", + alpha=0.35, + edgecolor="red", + lw=1.5, + ) + ) + ax.plot(float(xg[0]), float(xg[1]), "g*", markersize=18, label="Goal") + (line,) = ax.plot([], [], "b-", lw=2) + dot = ax.scatter([], [], s=80, color="blue", zorder=5) + ax.set_xlim(-1, 5) + ax.set_ylim(-1, 5) + ax.set_aspect("equal") + ax.set_title("Ellipsoidal-obstacle CBF — unicycle reach-goal", fontsize=10) + ax.legend(loc="lower right", fontsize=9) + ax.grid(True, alpha=0.3) + + def update(i): + line.set_data(states[: i + 1, 0], states[: i + 1, 1]) + dot.set_offsets([[states[i, 0], states[i, 1]]]) + return line, dot + + stride = max(1, len(states) // 70) + anim = FuncAnimation(fig, update, frames=range(0, len(states), stride), interval=100, blit=True) + os.makedirs(RESULTS_DIR, exist_ok=True) + anim.save(GIF_PATH, writer=PillowWriter(fps=10)) + plt.close(fig) + print(f"Saved animation to {GIF_PATH}") + return GIF_PATH + + +if __name__ == "__main__": + main() diff --git a/examples/unicycle/reach_goal/robust_cbf.py b/examples/unicycle/reach_goal/robust_cbf.py new file mode 100644 index 00000000..7ce7a0d3 --- /dev/null +++ b/examples/unicycle/reach_goal/robust_cbf.py @@ -0,0 +1,130 @@ +"""Unicycle reach-goal with robust CBF-CLF QP controller under worst-case bounded disturbance.""" +import os +import sys + +# Add the project root to the path so we can import 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 matplotlib.pyplot as plt +import numpy as np +from matplotlib.animation import FuncAnimation, PillowWriter +from matplotlib.patches import Ellipse + +import cbfkit.simulation.simulator as sim +import cbfkit.systems.unicycle.models.olfatisaber2002approximate as unicycle +from cbfkit.certificates import concatenate_certificates, rectify_relative_degree +from cbfkit.certificates.conditions.barrier_conditions import zeroing_barriers +from cbfkit.controllers.cbf_clf import robust_cbf_clf_qp_controller +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.unicycle import proportional_controller +from cbfkit.utils.user_types import PlannerData +from examples.unicycle.common.ellipsoidal_obstacle import cbf as ellipsoid_cbf + +TEST_MODE = bool(os.getenv("CBFKIT_TEST_MODE")) + + +def main() -> float: + dyn = unicycle.plant(lam=1.0) + x0 = jnp.array([0.0, 0.0, jnp.pi / 2]) + xg = jnp.array([4.0, 4.0, 0.0]) + + # Two obstacles between start and goal + obs_list = [ + (jnp.array([1.5, 1.5, 0.0]), jnp.array([0.5, 0.5])), + (jnp.array([3.0, 3.0, 0.0]), jnp.array([0.5, 0.5])), + ] + barriers = concatenate_certificates( + *[ + rectify_relative_degree( + function=ellipsoid_cbf(o, e), + system_dynamics=dyn, + state_dim=3, + form="exponential", + roots=jnp.array([-1.0]), + )(certificate_conditions=zeroing_barriers.linear_class_k(alpha=2.0)) + for o, e in obs_list + ] + ) + nominal = proportional_controller(dynamics=dyn, Kp_pos=1.0, Kp_theta=0.01) + controller = robust_cbf_clf_qp_controller( + control_limits=jnp.array([5.0, 5.0]), + nominal_input=nominal, + dynamics_func=dyn, + barriers=barriers, + disturbance_norm=2, + disturbance_norm_bound=0.25, + ) + tf, dt = (8.0, 0.02) if not TEST_MODE else (0.5, 0.02) + n = int(tf / dt) + res = sim.execute( + x0=x0, + dt=dt, + num_steps=n, + dynamics=dyn, + integrator=integrator, + nominal_controller=nominal, + controller=controller, + sensor=sensor, + estimator=estimator, + planner_data=PlannerData( + u_traj=None, + x_traj=jnp.tile(xg.reshape(-1, 1), (1, n + 1)), + prev_robustness=None, + ), + ) + states = np.asarray(res["states"]) + + final_dist = float(np.linalg.norm(states[-1, :2] - np.asarray(xg)[:2])) + print(f"Final distance to goal: {final_dist:.4f}") + + if TEST_MODE: + return final_dist + + fig, ax = plt.subplots(figsize=(6, 6)) + + for o, e in obs_list: + ax.add_patch( + Ellipse( + (float(o[0]), float(o[1])), + float(e[0]) * 2, + float(e[1]) * 2, + facecolor="red", + alpha=0.35, + edgecolor="red", + lw=1.5, + ) + ) + ax.plot(float(xg[0]), float(xg[1]), "g*", markersize=18, label="Goal") + (line,) = ax.plot([], [], "b-", lw=2) + dot = ax.scatter([], [], s=80, color="blue", zorder=5) + ax.set_xlim(-1, 5) + ax.set_ylim(-1, 5) + ax.set_aspect("equal") + ax.set_title("Robust CBF — safety under worst-case bounded disturbance", fontsize=10) + ax.legend(loc="lower right", fontsize=9) + ax.grid(True, alpha=0.3) + + def update(i): + line.set_data(states[: i + 1, 0], states[: i + 1, 1]) + dot.set_offsets([[states[i, 0], states[i, 1]]]) + return line, dot + + stride = max(1, len(states) // 70) + anim = FuncAnimation(fig, update, frames=range(0, len(states), stride), 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, "robust_cbf.gif") + anim.save(out, writer=PillowWriter(fps=10)) + plt.close(fig) + print(f"Saved animation to {out}") + return final_dist + + +if __name__ == "__main__": + main() diff --git a/examples/unicycle/reach_goal/stochastic_cbf.py b/examples/unicycle/reach_goal/stochastic_cbf.py index a1844efa..b27b377c 100644 --- a/examples/unicycle/reach_goal/stochastic_cbf.py +++ b/examples/unicycle/reach_goal/stochastic_cbf.py @@ -23,7 +23,7 @@ plot = 1 if not os.getenv("CBFKIT_TEST_MODE") else 0 should_animate = 1 if not os.getenv("CBFKIT_TEST_MODE") else 0 -save = 0 +save = 1 if not os.getenv("CBFKIT_TEST_MODE") else 0 # Simulation parameters tf = 10.0 if not os.getenv("CBFKIT_TEST_MODE") else 0.5 @@ -129,5 +129,6 @@ def sigma(x): obstacles=obstacles, ellipsoids=ellipsoids, save_animation=save, - animation_filename=file_path + "stochastic_cbf_control.mp4", + animation_filename=file_path + "stochastic_cbf_control.gif", + backend="matplotlib", ) diff --git a/examples/van_der_pol/regulation/perfect_sensing.py b/examples/van_der_pol/regulation/perfect_sensing.py index 364e8dcd..e69357d9 100644 --- a/examples/van_der_pol/regulation/perfect_sensing.py +++ b/examples/van_der_pol/regulation/perfect_sensing.py @@ -28,7 +28,7 @@ tf = 5.0 if not os.getenv("CBFKIT_TEST_MODE") else 0.5 n_steps = int(tf / setup.dt) plot = 1 if not os.getenv("CBFKIT_TEST_MODE") else 0 -save = 0 +save = 1 # Controlled reverse-time Van der Pol dynamics dynamics = van_der_pol.reverse_van_der_pol_oscillator(epsilon=setup.epsilon, sigma=setup.Q) diff --git a/examples/van_der_pol/visualizations/path.py b/examples/van_der_pol/visualizations/path.py index 0d837997..ccdc3b6e 100644 --- a/examples/van_der_pol/visualizations/path.py +++ b/examples/van_der_pol/visualizations/path.py @@ -67,15 +67,22 @@ def animate( ): from cbfkit.utils.animator import CBFAnimator - animator = CBFAnimator(states, dt=dt, x_lim=x_lim, y_lim=y_lim, title=title) + animator = CBFAnimator( + states, dt=dt, x_lim=x_lim, y_lim=y_lim, title=title, backend="matplotlib" + ) animator.add_goal(desired_state[:2], radius=desired_state_radius) animator.add_trajectory( - x_idx=0, y_idx=1, data=estimates, - color="tab:orange", label="Estimated Trajectory", + x_idx=0, + y_idx=1, + data=estimates, + color="tab:orange", + label="Estimated Trajectory", ) animator.add_trajectory( - x_idx=0, y_idx=1, - color="tab:blue", label="Trajectory", + x_idx=0, + y_idx=1, + color="tab:blue", + label="Trajectory", ) if save_animation: diff --git a/media/showcase/mpc_double_integrator.gif b/media/showcase/mpc_double_integrator.gif index 5e9f6768..8badd47d 100644 Binary files a/media/showcase/mpc_double_integrator.gif and b/media/showcase/mpc_double_integrator.gif differ diff --git a/media/showcase/mppi_stl.gif b/media/showcase/mppi_stl.gif index 0e8270f2..ad0eeccf 100644 Binary files a/media/showcase/mppi_stl.gif and b/media/showcase/mppi_stl.gif differ diff --git a/media/showcase/multi_robot_2d.gif b/media/showcase/multi_robot_2d.gif index 753b5238..86eab9cb 100644 Binary files a/media/showcase/multi_robot_2d.gif and b/media/showcase/multi_robot_2d.gif differ diff --git a/media/showcase/quadrotor_6dof.gif b/media/showcase/quadrotor_6dof.gif index bcea4043..fa8546df 100644 Binary files a/media/showcase/quadrotor_6dof.gif and b/media/showcase/quadrotor_6dof.gif differ diff --git a/media/showcase/risk_aware_cvar.gif b/media/showcase/risk_aware_cvar.gif index a2ca76ab..bd1f240e 100644 Binary files a/media/showcase/risk_aware_cvar.gif and b/media/showcase/risk_aware_cvar.gif differ diff --git a/media/showcase/robust_cbf.gif b/media/showcase/robust_cbf.gif index d0c694d2..45c0a64f 100644 Binary files a/media/showcase/robust_cbf.gif and b/media/showcase/robust_cbf.gif differ diff --git a/media/showcase/stochastic_cbf.gif b/media/showcase/stochastic_cbf.gif index adf81e48..38ebd73c 100644 Binary files a/media/showcase/stochastic_cbf.gif and b/media/showcase/stochastic_cbf.gif differ diff --git a/media/showcase/van_der_pol_clf.gif b/media/showcase/van_der_pol_clf.gif index f8740511..174a1fcc 100644 Binary files a/media/showcase/van_der_pol_clf.gif and b/media/showcase/van_der_pol_clf.gif differ