openscvx 0.5.3.dev29__tar.gz → 0.5.3.dev30__tar.gz

{openscvx-0.5.3.dev29/openscvx.egg-info → openscvx-0.5.3.dev30}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: openscvx
-Version: 0.5.3.dev29
+Version: 0.5.3.dev30
 Summary: A general Python-based successive convexification implementation which uses a JAX backend.
 Author-email: Chris Hayner and Griffin Norris <haynec@uw.edu>
 License: Apache Software License

openscvx-0.5.3.dev30/examples/double_integrator/_plotting.py

@@ -0,0 +1,265 @@
+"""Plotting helpers for double-integrator examples."""
+
+from __future__ import annotations
+
+import numpy as np
+import plotly.graph_objects as go
+from scipy.interpolate import CubicSpline
+
+from openscvx.algorithms import OptimizationResults
+
+
+def _ball_center_at(
+    t: float,
+    t_knots: np.ndarray,
+    x_knots: np.ndarray,
+    y_knots: np.ndarray,
+    *,
+    method: str = "cubic",
+) -> np.ndarray:
+    if method == "pchip":
+        from scipy.interpolate import PchipInterpolator
+
+        cx = PchipInterpolator(t_knots, x_knots)(t)
+        cy = PchipInterpolator(t_knots, y_knots)(t)
+    else:
+        cx = CubicSpline(t_knots, x_knots)(t)
+        cy = CubicSpline(t_knots, y_knots)(t)
+    return np.array([cx, cy])
+
+
+def _circle_polyline(
+    center: np.ndarray, radius: float, n_pts: int = 80
+) -> tuple[np.ndarray, np.ndarray]:
+    theta = np.linspace(0.0, 2.0 * np.pi, n_pts)
+    return center[0] + radius * np.cos(theta), center[1] + radius * np.sin(theta)
+
+
+def _frame_args(duration_ms: int) -> dict:
+    return {
+        "frame": {"duration": duration_ms, "redraw": True},
+        "mode": "immediate",
+        "fromcurrent": True,
+        "transition": {"duration": 0},
+    }
+
+
+def plot_moving_safe_zones(results: OptimizationResults, *, animate: bool = True) -> go.Figure:
+    """Plot the solved trajectory with moving safe-zone balls.
+
+    When ``animate`` is True (default), the figure includes a Play/Pause control
+    and slider that advance the vehicle along the trajectory while the safe-zone
+    balls move along their spline paths.
+    """
+    traj = results.trajectory
+    pos = traj["position"]
+    t = traj["time"].reshape(-1)
+    radius = float(results.plotting_data["ball_radius"])
+    t_knots = np.asarray(results.plotting_data["t_knots"], dtype=float)
+    ball_paths = results.plotting_data["ball_paths"]
+    interp_method = results.plotting_data.get("ball_interp_method", "cubic")
+    start = np.asarray(results.plotting_data["start"], dtype=float)
+    goal = np.asarray(results.plotting_data["goal"], dtype=float)
+
+    n_balls = len(ball_paths)
+    palette = [
+        "#2ca02c",
+        "#1f77b4",
+        "#ff7f0e",
+        "#9467bd",
+        "#d62728",
+        "#8c564b",
+        "#e377c2",
+        "#7f7f7f",
+    ]
+    colors = [palette[i % len(palette)] for i in range(n_balls)]
+    names = [f"Safe zone {i}" for i in range(n_balls)]
+    n_animated_traces = 3 + 2 * n_balls
+
+    # Subsample dense propagated trajectories for a manageable animation length
+    n_pts = len(t)
+    stride = max(1, n_pts // 60)
+    frame_indices = list(range(0, n_pts, stride))
+    if frame_indices[-1] != n_pts - 1:
+        frame_indices.append(n_pts - 1)
+
+    def _ball_circle_trace(idx: int, center: np.ndarray) -> go.Scatter:
+        cx, cy = _circle_polyline(center, radius)
+        return go.Scatter(
+            x=cx,
+            y=cy,
+            mode="lines",
+            line={"color": colors[idx], "width": 2.5},
+            fill="toself",
+            fillcolor=_rgba(colors[idx], 0.12),
+            name=names[idx],
+            hovertemplate=f"{names[idx]}<br>(%{{x:.2f}}, %{{y:.2f}})<extra></extra>",
+        )
+
+    # Frame 0 geometry
+    centers_0 = [
+        _ball_center_at(float(t[0]), t_knots, np.asarray(xk), np.asarray(yk), method=interp_method)
+        for xk, yk in ball_paths
+    ]
+
+    fig = go.Figure(
+        data=[
+            go.Scatter(
+                x=pos[:1, 0],
+                y=pos[:1, 1],
+                mode="lines",
+                line={"color": "#111111", "width": 3},
+                name="Trajectory",
+            ),
+            go.Scatter(
+                x=[pos[0, 0]],
+                y=[pos[0, 1]],
+                mode="markers",
+                marker={"size": 14, "color": "#111111", "line": {"color": "white", "width": 2}},
+                name="Vehicle",
+            ),
+            go.Scatter(
+                x=[start[0], goal[0]],
+                y=[start[1], goal[1]],
+                mode="markers+text",
+                marker={"size": 12, "symbol": ["circle", "star"], "color": ["#9467bd", "#d62728"]},
+                text=["start", "goal"],
+                textposition="top center",
+                name="Endpoints",
+            ),
+            *[_ball_circle_trace(i, centers_0[i]) for i in range(n_balls)],
+            *[
+                go.Scatter(
+                    x=np.asarray(x_knots),
+                    y=np.asarray(y_knots),
+                    mode="lines",
+                    line={"color": colors[idx], "width": 1.5, "dash": "dash"},
+                    showlegend=False,
+                    hoverinfo="skip",
+                    name=f"{names[idx]} path",
+                )
+                for idx, (x_knots, y_knots) in enumerate(ball_paths)
+            ],
+        ]
+    )
+
+    if animate:
+        frames = []
+        for frame_no, i in enumerate(frame_indices):
+            centers_i = [
+                _ball_center_at(
+                    float(t[i]), t_knots, np.asarray(xk), np.asarray(yk), method=interp_method
+                )
+                for xk, yk in ball_paths
+            ]
+            frame_data = [
+                go.Scatter(x=pos[: i + 1, 0], y=pos[: i + 1, 1]),
+                go.Scatter(x=[pos[i, 0]], y=[pos[i, 1]]),
+                go.Scatter(x=[start[0], goal[0]], y=[start[1], goal[1]]),
+                *[
+                    go.Scatter(
+                        x=_circle_polyline(centers_i[j], radius)[0],
+                        y=_circle_polyline(centers_i[j], radius)[1],
+                    )
+                    for j in range(n_balls)
+                ],
+                *[
+                    go.Scatter(x=np.asarray(x_knots), y=np.asarray(y_knots))
+                    for x_knots, y_knots in ball_paths
+                ],
+            ]
+            frames.append(
+                go.Frame(name=str(frame_no), data=frame_data, traces=list(range(n_animated_traces)))
+            )
+
+        fig.frames = frames
+
+        frame_ms = 80
+        fig.update_layout(
+            updatemenus=[
+                {
+                    "type": "buttons",
+                    "direction": "left",
+                    "x": 0.05,
+                    "y": -0.08,
+                    "buttons": [
+                        {
+                            "label": "Play",
+                            "method": "animate",
+                            "args": [None, _frame_args(frame_ms)],
+                        },
+                        {
+                            "label": "Pause",
+                            "method": "animate",
+                            "args": [[None], _frame_args(0)],
+                        },
+                    ],
+                }
+            ],
+            sliders=[
+                {
+                    "x": 0.12,
+                    "y": -0.12,
+                    "len": 0.82,
+                    "pad": {"b": 10, "t": 40},
+                    "currentvalue": {"prefix": "t = ", "suffix": " s", "visible": True},
+                    "steps": [
+                        {
+                            "label": f"{float(t[i]):.2f}",
+                            "method": "animate",
+                            "args": [[str(frame_no)], _frame_args(0)],
+                        }
+                        for frame_no, i in enumerate(frame_indices)
+                    ],
+                }
+            ],
+        )
+    else:
+        # Static snapshot: full trajectory plus balls at a few sample times
+        fig.data[0].x = pos[:, 0]
+        fig.data[0].y = pos[:, 1]
+        fig.data[0].mode = "lines+markers"
+        fig.data[0].marker = {
+            "size": 5,
+            "color": t,
+            "colorscale": "Viridis",
+            "showscale": True,
+            "colorbar": {"title": "time [s]"},
+        }
+        fig.data[1].visible = False
+
+        sample_times = np.linspace(t[0], t[-1], 5)[1:-1]
+        for ti in sample_times:
+            for idx, (x_knots, y_knots) in enumerate(ball_paths):
+                center = _ball_center_at(
+                    ti, t_knots, np.asarray(x_knots), np.asarray(y_knots), method=interp_method
+                )
+                cx, cy = _circle_polyline(center, radius)
+                fig.add_trace(
+                    go.Scatter(
+                        x=cx,
+                        y=cy,
+                        mode="lines",
+                        line={"color": colors[idx], "width": 1, "dash": "dot"},
+                        opacity=0.35,
+                        showlegend=False,
+                        hoverinfo="skip",
+                    )
+                )
+
+    fig.update_layout(
+        title="Moving Safe Zones — 2D Double Integrator",
+        xaxis_title="x [m]",
+        yaxis_title="y [m]",
+        yaxis={"scaleanchor": "x", "scaleratio": 1},
+        template="plotly_white",
+        legend={"x": 0.01, "y": 0.99},
+        margin={"b": 120 if animate else 80},
+    )
+    return fig
+
+
+def _rgba(hex_color: str, alpha: float) -> str:
+    hex_color = hex_color.lstrip("#")
+    r, g, b = (int(hex_color[i : i + 2], 16) for i in (0, 2, 4))
+    return f"rgba({r},{g},{b},{alpha})"

openscvx-0.5.3.dev30/examples/double_integrator/moving_safe_zones.py

@@ -0,0 +1,190 @@
+"""2D double integrator with STL moving safe zones.
+
+The vehicle must stay inside at least one of several moving circular safe zones
+while traveling from a start point to a goal. Each zone center follows a
+time-varying path (PCHIP spline in physical time), and the specification is
+
+    Always( Or(in_ball_0, ..., in_ball_N) )
+
+over the full horizon, enforced with ``ox.stl`` operators.
+
+The balls are designed as a time-segmented relay: each ball covers a different
+time window along a feasible hand-off path, but no single ball spans the full
+journey. Only ball 0 covers the start and only ball 5 covers the goal, so the
+vehicle must switch zones to reach the finish.
+"""
+
+import os
+import sys
+
+import numpy as np
+
+current_dir = os.path.dirname(os.path.abspath(__file__))
+project_root = os.path.dirname(os.path.dirname(current_dir))
+if project_root not in sys.path:
+    sys.path.append(project_root)
+
+import openscvx as ox
+from examples.double_integrator._plotting import plot_moving_safe_zones
+from openscvx import Problem
+
+# Discretization
+N = 30
+TOTAL_TIME = 10.0
+
+START = np.array([-4.0, 0.0])
+GOAL = np.array([4.0, 0.0])
+
+BALL_RADIUS = 0.6
+
+# Knot times aligned with relay hand-offs (non-uniform so ball paths can
+# diverge between segments).
+T_KNOTS = np.array([0.0, 2.0, 3.5, 5.0, 7.5, 10.0])
+
+# Feasible relay anchors (each lies inside exactly one ball at that time).
+RELAY_KEYFRAMES = [
+    START,
+    np.array([-2.0, 0.9]),
+    np.array([-0.8, 0.2]),
+    np.array([0.5, -1.0]),
+    np.array([2.5, -0.7]),
+    GOAL,
+]
+RELAY_NODES = [0, 6, 10, 15, 22, N - 1]
+
+# Six relay balls — each covers part of the journey; no ball spans start→goal.
+BALL_PATHS = [
+    # Ball 0: early high lane (only ball at t=0)
+    (
+        np.array([-4.0, -2.0, -0.5, 1.0, 2.2, 2.8]),
+        np.array([0.0, 0.9, 1.1, 1.3, 1.4, 1.5]),
+    ),
+    # Ball 1: high→mid transition (~2–5 s)
+    (
+        np.array([-3.5, -2.2, -0.8, 0.2, 1.3, 2.2]),
+        np.array([-0.4, 0.55, 0.2, -0.15, -0.35, -0.55]),
+    ),
+    # Ball 2: alternate mid-high (never at start/goal)
+    (
+        np.array([-2.8, -1.4, 0.3, 1.6, 2.8, 3.4]),
+        np.array([1.25, 1.15, 1.0, 0.9, 0.8, 0.7]),
+    ),
+    # Ball 3: low trench (~3.5–8 s)
+    (
+        np.array([-2.5, -1.4, 0.0, 0.5, 2.5, 3.1]),
+        np.array([-1.1, -1.0, -0.95, -1.0, -0.7, -0.65]),
+    ),
+    # Ball 4: late high option (never at start/goal)
+    (
+        np.array([-1.5, -0.2, 1.2, 2.3, 3.1, 3.5]),
+        np.array([1.15, 1.05, 0.95, 0.75, 0.55, 0.5]),
+    ),
+    # Ball 5: finisher (only ball at t=10)
+    (
+        np.array([-1.2, 0.3, 1.4, 2.4, 3.4, 4.0]),
+        np.array([-0.55, -0.45, -0.35, -0.25, -0.1, 0.0]),
+    ),
+]
+
+# States
+position = ox.State("position", shape=(2,))
+position.min = np.array([-6.0, -2.5])
+position.max = np.array([6.0, 2.5])
+position.initial = START
+position.final = GOAL
+
+velocity = ox.State("velocity", shape=(2,))
+velocity.min = np.array([-8.0, -8.0])
+velocity.max = np.array([8.0, 8.0])
+velocity.initial = np.array([0.0, 0.0])
+velocity.final = [ox.Free(0.0), ox.Free(0.0)]
+
+# Fixed final time so ball centers stay synchronized with physical time.
+time = ox.Time(
+    initial=0.0,
+    final=TOTAL_TIME,
+    min=0.0,
+    max=TOTAL_TIME,
+    uniform_time_grid=True,
+)
+
+force = ox.Control("force", shape=(2,))
+A_MAX = 12.0
+force.max = np.array([A_MAX, A_MAX])
+force.min = np.array([-A_MAX, -A_MAX])
+force.guess = np.zeros((N, 2))
+
+states = [position, velocity, time]
+controls = [force]
+m = 1.0
+
+ball_radius = ox.Parameter("ball_radius", shape=(), value=BALL_RADIUS)
+
+in_ball_predicates = []
+for ball_x, ball_y in BALL_PATHS:
+    center = ox.Concat(
+        ox.Cinterp(time[0], T_KNOTS, ball_x, method="pchip"),
+        ox.Cinterp(time[0], T_KNOTS, ball_y, method="pchip"),
+    )
+    delta = position - center
+    in_ball_predicates.append(ox.Sum(delta * delta) <= ball_radius * ball_radius)
+
+in_some_ball = ox.stl.Or(*in_ball_predicates)
+
+constraints = []
+for state in states:
+    constraints.extend([ox.ctcs(state <= state.max), ox.ctcs(state.min <= state)])
+
+constraints.append(ox.stl.Always(in_some_ball, (0, N - 1)).over())
+
+dynamics = {
+    "position": velocity,
+    "velocity": (1.0 / m) * force,
+    "time": 1.0,
+}
+
+position.guess = ox.init.linspace(keyframes=RELAY_KEYFRAMES, nodes=RELAY_NODES)
+velocity.guess = np.gradient(position.guess, TOTAL_TIME / (N - 1), axis=0)
+force.guess = m * np.gradient(velocity.guess, TOTAL_TIME / (N - 1), axis=0)
+
+problem = Problem(
+    dynamics=dynamics,
+    states=states,
+    controls=controls,
+    time=time,
+    constraints=constraints,
+    N=N,
+    algorithm={
+        "autotuner": ox.ConstantProximalWeight(),
+        "lam_prox": 5e-3,
+        "lam_vc": 2e1,
+        "lam_cost": 1e-3,
+    },
+    float_dtype="float64",
+)
+
+plotting_data = {
+    "ball_radius": BALL_RADIUS,
+    "t_knots": T_KNOTS,
+    "ball_paths": BALL_PATHS,
+    "ball_interp_method": "pchip",
+    "start": START,
+    "goal": GOAL,
+}
+
+if __name__ == "__main__":
+    n_balls = len(BALL_PATHS)
+    print("2D Double Integrator — Moving Safe Zones (STL Or + Always)")
+    print("=" * 60)
+    print(f"Start: {START}, Goal: {GOAL}")
+    print(f"Balls: {n_balls}, radius: {BALL_RADIUS}")
+    print("Design: time-segmented relay (mandatory ball hand-offs)")
+    print(f"Spec: Always( Or(in_ball_0, ..., in_ball_{n_balls - 1}) ) over the horizon")
+    print("=" * 60)
+
+    problem.initialize()
+    results = problem.solve()
+    results = problem.post_process()
+    results.update(plotting_data)
+
+    plot_moving_safe_zones(results).show()

{openscvx-0.5.3.dev29 → openscvx-0.5.3.dev30}/openscvx/_version.py

@@ -18,7 +18,7 @@ version_tuple: tuple[int | str, ...]
 commit_id: str | None
 __commit_id__: str | None
 
-__version__ = version = '0.5.3.dev29'
-__version_tuple__ = version_tuple = (0, 5, 3, 'dev29')
+__version__ = version = '0.5.3.dev30'
+__version_tuple__ = version_tuple = (0, 5, 3, 'dev30')
 
-__commit_id__ = commit_id = 'gbdd7166d1'
+__commit_id__ = commit_id = 'gcbb86d554'

{openscvx-0.5.3.dev29 → openscvx-0.5.3.dev30/openscvx.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: openscvx
-Version: 0.5.3.dev29
+Version: 0.5.3.dev30
 Summary: A general Python-based successive convexification implementation which uses a JAX backend.
 Author-email: Chris Hayner and Griffin Norris <haynec@uw.edu>
 License: Apache Software License

{openscvx-0.5.3.dev29 → openscvx-0.5.3.dev30}/openscvx.egg-info/SOURCES.txt

@@ -85,6 +85,8 @@ examples/car/dubins_car_obstacle_conditional.py
 examples/car/dubins_car_obstacle_stl.py
 examples/car/dubins_car_stl_or.py
 examples/car/dubins_car_waypoint_stl.py
+examples/double_integrator/_plotting.py
+examples/double_integrator/moving_safe_zones.py
 examples/drone/2d_obstacle_avoidance_batched_ic.py
 examples/drone/cinema_vp.py
 examples/drone/cinema_vp_nodal.py