openscvx 0.3.2.dev173__tar.gz → 0.3.2.dev195__tar.gz

{openscvx-0.3.2.dev173/openscvx.egg-info → openscvx-0.3.2.dev195}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: openscvx
-Version: 0.3.2.dev173
+Version: 0.3.2.dev195
 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.3.2.dev173 → openscvx-0.3.2.dev195}/examples/drone/dr_vp.py

@@ -116,14 +116,13 @@ for center in gate_centers:
 ### End Gate Parameters ###
 
 n_subs = 10
-init_poses = []
 np.random.seed(0)
-for i in range(n_subs):
-    init_pose = np.array([100.0, -60.0, 20.0])
-    init_pose[:2] = init_pose[:2] + np.random.random(2) * 20.0
-    init_poses.append(init_pose)
-
-init_poses = init_poses
+init_poses = np.array(
+    [
+        [100.0 + np.random.random() * 20.0, -60.0 + np.random.random() * 20.0, 20.0]
+        for _ in range(n_subs)
+    ]
+)  # Shape: (n_subs, 3)
 
 
 # Define list of all states (needed for Problem and constraints)
@@ -142,9 +141,16 @@ constraints = []
 for state in states:
     constraints.extend([ox.ctcs(state <= state.max), ox.ctcs(state.min <= state)])
 
-# Add visibility constraints using symbolic expressions
-for pose in init_poses:
-    constraints.append(ox.ctcs(g_vp(pose, position, attitude) <= 0.0))
+# Add visibility constraints using Vmap for parallel evaluation
+# Single CTCS constraint with vectorized evaluation over all target poses
+visibility_constraint = ox.ctcs(
+    ox.Vmap(
+        lambda pose: g_vp(pose, position, attitude),
+        batch=init_poses,
+    )
+    <= 0.0
+)
+constraints.append(visibility_constraint)
 
 # Add gate constraints using symbolic expressions
 for node, cen in zip(gate_nodes, A_gate_cen):
@@ -207,12 +213,9 @@ for _ in range(n_gates + 1):
 # Modify attitude to point sensor at targets
 R_sb = R_sb  # Sensor to body frame
 b = R_sb @ np.array([0, 1, 0])
+mean_target = np.mean(init_poses, axis=0)  # Average target position
 for k in range(n):
-    kp = []
-    for pose in init_poses:
-        kp.append(pose)
-    kp = np.mean(kp, axis=0)
-    a = kp - position_bar[k]
+    a = mean_target - position_bar[k]
     # Determine the direction cosine matrix that aligns the z-axis of the sensor frame with the
     # relative position vector
     q_xyz = np.cross(b, a)

{openscvx-0.3.2.dev173 → openscvx-0.3.2.dev195}/examples/drone/dr_vp_nodal.py

@@ -116,14 +116,13 @@ for center in gate_centers:
 ### End Gate Parameters ###
 
 n_subs = 10
-init_poses = []
 np.random.seed(5)
-for i in range(n_subs):
-    init_pose = np.array([100.0, -70.0, 20.0])
-    init_pose[:2] = init_pose[:2] + np.random.random(2) * 20.0
-    init_poses.append(init_pose)
-
-init_poses = init_poses
+init_poses = np.array(
+    [
+        [100.0 + np.random.random() * 20.0, -70.0 + np.random.random() * 20.0, 20.0]
+        for _ in range(n_subs)
+    ]
+)  # Shape: (n_subs, 3)
 
 
 # Define list of all states (needed for Problem and constraints)
@@ -142,9 +141,16 @@ constraints = []
 for state in states:
     constraints.extend([ox.ctcs(state <= state.max), ox.ctcs(state.min <= state)])
 
-# Add visibility constraints using symbolic expressions
-for pose in init_poses:
-    constraints.append((g_vp(pose, position, attitude) <= 0.0))
+# Add visibility constraints using Vmap for parallel evaluation
+# Single nodal constraint with vectorized evaluation over all target poses
+visibility_constraint = (
+    ox.Vmap(
+        lambda pose: g_vp(pose, position, attitude),
+        batch=init_poses,
+    )
+    <= 0.0
+)
+constraints.append(visibility_constraint)
 
 # Add gate constraints using symbolic expressions
 for node, cen in zip(gate_nodes, A_gate_cen):
@@ -207,12 +213,9 @@ for _ in range(n_gates + 1):
 # Modify attitude to point sensor at targets
 R_sb = R_sb  # Sensor to body frame
 b = R_sb @ np.array([0, 1, 0])
+mean_target = np.mean(init_poses, axis=0)  # Average target position
 for k in range(n):
-    kp = []
-    for pose in init_poses:
-        kp.append(pose)
-    kp = np.mean(kp, axis=0)
-    a = kp - position_bar[k]
+    a = mean_target - position_bar[k]
     # Determine the direction cosine matrix that aligns the z-axis of the sensor frame with the
     # relative position vector
     q_xyz = np.cross(b, a)

{openscvx-0.3.2.dev173 → openscvx-0.3.2.dev195}/examples/drone/dr_vp_polytope.py

@@ -111,10 +111,7 @@ polytope_point = np.array(
         [107.47, -50.00, 22.85],
     ]
 )
-init_poses = []
-for point in polytope_point:
-    init_poses.append(point)
-init_poses = init_poses
+init_poses = polytope_point  # Shape: (20, 3)
 ### Gate Parameters ###
 n_gates = 10
 gate_centers = [
@@ -162,9 +159,16 @@ constraints = []
 for state in states:
     constraints.extend([ox.ctcs(state <= state.max), ox.ctcs(state.min <= state)])
 
-# Add visibility constraints for polytope points using symbolic expressions
-for pose in init_poses:
-    constraints.append(ox.ctcs(g_vp(pose, position, attitude) <= 0.0))
+# Add visibility constraints using Vmap for parallel evaluation
+# Single CTCS constraint with vectorized evaluation over all polytope points
+visibility_constraint = ox.ctcs(
+    ox.Vmap(
+        lambda pose: g_vp(pose, position, attitude),
+        batch=init_poses,
+    )
+    <= 0.0
+)
+constraints.append(visibility_constraint)
 
 # Add gate constraints using symbolic expressions
 for node, cen in zip(gate_nodes, A_gate_cen):
@@ -227,12 +231,9 @@ for _ in range(n_gates + 1):
 # Modify attitude to point sensor at targets
 R_sb = R_sb  # Sensor to body frame
 b = R_sb @ np.array([0, 1, 0])
+mean_target = np.mean(init_poses, axis=0)  # Average target position
 for k in range(n):
-    kp = []
-    for pose in init_poses:
-        kp.append(pose)
-    kp = np.mean(kp, axis=0)
-    a = kp - position_bar[k]
+    a = mean_target - position_bar[k]
     # Determine the direction cosine matrix that aligns the z-axis of the sensor frame with the
     # relative position vector
     q_xyz = np.cross(b, a)

openscvx-0.3.2.dev195/examples/drone/obstacle_avoidance_vmap.py

@@ -0,0 +1,238 @@
+"""Double integrator obstacle avoidance with Vmap for parallel constraint evaluation.
+
+This example demonstrates using ox.Vmap directly in constraints for data-parallel
+obstacle avoidance with numerous spherical obstacles.
+
+The approach:
+
+1. Stack all obstacle centers into a single array
+2. Use Vmap directly in the constraint to compute distances in parallel
+3. The vector-valued constraint is enforced element-wise
+
+Compare with:
+  - obstacle_avoidance.py (manual loop over 3 obstacles)
+"""
+
+import os
+import sys
+
+import numpy as np
+
+# Add grandparent directory to path to import examples.plotting
+current_dir = os.path.dirname(os.path.abspath(__file__))
+grandparent_dir = os.path.dirname(os.path.dirname(current_dir))
+sys.path.append(grandparent_dir)
+
+import openscvx as ox
+from examples.plotting_viser import (
+    create_animated_plotting_server,
+    create_scp_animated_plotting_server,
+)
+from openscvx import Problem
+
+n = 6  # Number of nodes
+total_time = 10.0  # Total time for the simulation
+
+# =============================================================================
+# State and Control Definitions
+# =============================================================================
+
+# 3D position
+position = ox.State("position", shape=(3,))
+position.max = np.array([15.0, 15.0, 15.0])
+position.min = np.array([-15.0, -15.0, 0.0])
+position.initial = np.array([-10.0, -10.0, 2.0])
+position.final = np.array([10.0, 10.0, 2.0])
+
+# 3D velocity
+velocity = ox.State("velocity", shape=(3,))
+velocity.max = np.array([10.0, 10.0, 10.0])
+velocity.min = np.array([-10.0, -10.0, -10.0])
+velocity.initial = np.array([0.0, 0.0, 0.0])
+velocity.final = [("free", 0.0), ("free", 0.0), ("free", 0.0)]
+
+# Control force
+force = ox.Control("force", shape=(3,))
+f_max = 20.0
+force.max = np.array([f_max, f_max, f_max])
+force.min = np.array([-f_max, -f_max, -f_max])
+
+# Physical parameters
+m = 1.0  # Mass
+g = -9.81  # Gravity
+
+# =============================================================================
+# Obstacle Configuration (3D grid)
+# =============================================================================
+
+obstacle_radius_min, obstacle_radius_max = 1.0, 2.5
+
+# Generate obstacle positions in a 3D grid pattern between start and goal
+np.random.seed(42)
+obstacle_centers = []
+
+# Tweak number of obstacles by changing the grid sizes
+n_rows = 4
+n_cols = 4
+n_lays = 4
+
+# Create a 3D field of obstacles: rows (x) x columns (y) x layers (z)
+for i in range(n_rows):
+    for j in range(n_cols):
+        for k in range(n_lays):
+            # Base grid position
+            x = -6.0 + i * 6.0
+            y = -7.5 + j * 5.0
+            z = 1.0 + k * 2.5  # Layers at z = 1.0, 3.5, 6.0
+            # Add some randomness
+            x += np.random.uniform(-1.0, 1.0)
+            y += np.random.uniform(-1.0, 1.0)
+            z += np.random.uniform(-0.5, 0.5)
+            obstacle_centers.append([x, y, z])
+
+n_obstacles = len(obstacle_centers)  # 36 obstacles
+
+obstacle_centers = np.array(obstacle_centers)  # Shape: (n_obstacles, 3)
+obstacle_radii = np.random.uniform(obstacle_radius_min, obstacle_radius_max, size=n_obstacles)
+
+print(f"Created {n_obstacles} obstacles")
+print(f"Obstacle centers shape: {obstacle_centers.shape}")
+
+# =============================================================================
+# Dynamics (simple double integrator)
+# =============================================================================
+
+dynamics = {
+    "position": velocity,
+    "velocity": (1.0 / m) * force + np.array([0.0, 0.0, g]),
+}
+
+# =============================================================================
+# Constraints
+# =============================================================================
+
+states = [position, velocity]
+controls = [force]
+
+constraints = []
+
+# Box constraints on states
+for state in states:
+    constraints.extend(
+        [
+            ox.ctcs(state <= state.max),
+            ox.ctcs(state.min <= state),
+        ]
+    )
+
+# Box constraints on controls
+constraints.extend(
+    [
+        force <= force.max,
+        force.min <= force,
+    ]
+)
+
+# =============================================================================
+# Obstacle Avoidance
+# =============================================================================
+# Two approaches are shown below for benchmarking. Comment/uncomment to toggle.
+
+# --- APPROACH 1: Vmap (single constraint, vectorized) ---
+# - Single CTCS constraint with vectorized evaluation
+# - Vmap computes distance to all obstacles in parallel
+
+obstacle_avoidance = ox.ctcs(
+    obstacle_radii
+    <= ox.Vmap(
+        lambda obs_center: ox.linalg.Norm(position - obs_center),
+        batch=obstacle_centers,
+    )
+)
+constraints.append(obstacle_avoidance)
+
+# --- APPROACH 2: Individual constraints (loop, no vmap) ---
+# - Creates n_obstacles separate CTCS constraints
+# - Each constraint is lowered and traced independently
+
+# for i in range(n_obstacles):
+#     obs_center = obstacle_centers[i]
+#     radius = obstacle_radii[i]
+#     obstacle_constraint = ox.ctcs(
+#         radius <= ox.linalg.Norm(position - obs_center)
+#     )
+#     constraints.append(obstacle_constraint)
+
+# =============================================================================
+# Initial Guesses
+# =============================================================================
+
+straight_line = np.linspace(position.initial, position.final, n)
+position.guess = straight_line
+velocity.guess = np.zeros((n, 3))
+force.guess = np.tile([0.0, 0.0, -m * g], (n, 1))  # Hover thrust
+
+# =============================================================================
+# Problem Setup
+# =============================================================================
+
+time = ox.Time(
+    initial=0.0,
+    final=("minimize", total_time),
+    min=0.0,
+    max=total_time,
+)
+
+problem = Problem(
+    dynamics=dynamics,
+    states=states,
+    controls=controls,
+    time=time,
+    constraints=constraints,
+    N=n,
+)
+
+# SCP settings
+problem.settings.prp.dt = 0.01
+problem.settings.scp.w_tr = 5e0
+problem.settings.scp.lam_cost = 1e0
+problem.settings.scp.lam_vc = 1e2
+problem.settings.scp.ep_tr = 1e-3
+problem.settings.scp.ep_vb = 1e-4
+problem.settings.scp.ep_vc = 1e-8
+problem.settings.scp.cost_drop = 5
+problem.settings.scp.cost_relax = 0.7
+problem.settings.scp.w_tr_adapt = 1.5
+
+# =============================================================================
+# Solve and Visualize
+# =============================================================================
+
+if __name__ == "__main__":
+    problem.initialize()
+    results = problem.solve()
+    results = problem.post_process()
+
+    # Store obstacle info for visualization
+    results.update(
+        {
+            "obstacles_centers": [c for c in obstacle_centers],
+            "obstacles_radii": [[1 / r, 1 / r, 1 / r] for r in obstacle_radii],
+            "obstacles_axes": [np.eye(3) for _ in range(n_obstacles)],
+        }
+    )
+
+    # Create viser visualization servers
+    traj_server = create_animated_plotting_server(
+        results,
+        thrust_key="force",
+        viewcone_scale=5.0,
+    )
+    scp_server = create_scp_animated_plotting_server(
+        results,
+        attitude_stride=3,
+        frame_duration_ms=200,
+    )
+
+    # Keep servers running
+    traj_server.sleep_forever()

{openscvx-0.3.2.dev173 → openscvx-0.3.2.dev195}/examples/plotting_viser.py

@@ -58,6 +58,7 @@ def create_animated_plotting_server(
     target_radius: float = 1.0,
     show_control_plot: str | None = None,
     show_control_norm_plot: str | None = None,
+    show_grid: bool = True,
 ) -> viser.ViserServer:
     """Create an animated trajectory visualization server.
 
@@ -101,6 +102,7 @@ def create_animated_plotting_server(
             showing each control component vs time with animated markers
         show_control_norm_plot: If provided with a control name, displays norm plot
             showing ‖control‖₂ vs time with animated marker
+        show_grid: Whether to show the grid (default True)
 
     Returns:
         ViserServer instance (animation runs in background thread)
@@ -135,7 +137,7 @@ def create_animated_plotting_server(
     colors = compute_velocity_colors(vel)
 
     # Create server
-    server = create_server(pos)
+    server = create_server(pos, show_grid=show_grid)
 
     # Add static elements
     if "vertices" in results:
@@ -257,6 +259,7 @@ def create_scp_animated_plotting_server(
     frame_duration_ms: int = 500,
     scene_scale: float = 1.0,
     cmap_name: str = "viridis",
+    show_grid: bool = True,
 ) -> viser.ViserServer:
     """Create an animated visualization of SCP iteration convergence.
 
@@ -289,6 +292,7 @@ def create_scp_animated_plotting_server(
         scene_scale: Divide all positions by this factor. Use >1 for large-scale
             trajectories (e.g., 100.0 for km-scale problems).
         cmap_name: Matplotlib colormap name for iteration coloring (default: "viridis")
+        show_grid: Whether to show the grid (default True)
 
     Returns:
         ViserServer instance (animation runs in background thread)
@@ -322,7 +326,7 @@ def create_scp_animated_plotting_server(
         attitudes = [X[:, attitude_slice] for X in X_history]
 
     # Create server using final iteration's positions for grid sizing
-    server = create_server(positions[-1])
+    server = create_server(positions[-1], show_grid=show_grid)
 
     # Add static elements (gates, obstacles) if present
     if "vertices" in results:

openscvx-0.3.2.dev195/examples/spacecraft/proxops_cw.py

@@ -0,0 +1,188 @@
+"""Proximity Operations (ProxOps) using Clohessy-Wiltshire dynamics.
+
+This example demonstrates optimal trajectory generation for spacecraft
+proximity operations and docking using the Clohessy-Wiltshire (CW) equations
+for relative motion in a circular orbit.
+See [Clohessy-Wiltshire equations](https://en.wikipedia.org/wiki/Clohessy%E2%80%93Wiltshire_equations)
+for further details.
+The problem includes:
+
+- 3D relative position and velocity dynamics (CW equations)
+- Fuel-optimal control (minimize delta-v)
+- Thrust magnitude constraints
+- Approach cone constraint for safe docking corridor
+- Final docking at target (origin)
+
+!!! tip "The CW frame convention:"
+    - x: radial direction (outward from Earth)
+    - y: along-track direction (velocity direction)
+    - z: cross-track direction (normal to orbit plane)
+"""
+
+import os
+import sys
+
+import numpy as np
+
+# Add grandparent directory to path to import examples.plotting
+current_dir = os.path.dirname(os.path.abspath(__file__))
+grandparent_dir = os.path.dirname(os.path.dirname(current_dir))
+sys.path.append(grandparent_dir)
+
+import openscvx as ox
+from examples.plotting_viser import (
+    create_animated_plotting_server,
+    create_scp_animated_plotting_server,
+)
+from openscvx import Problem
+from openscvx.plotting import plot_scp_iterations
+from openscvx.plotting.viser import add_glideslope_cone
+
+# Problem parameters
+n_nodes = 5  # Number of discretization nodes
+total_time = 180.0  # Total maneuver time in seconds
+
+# Orbital parameters (ISS-like orbit at ~400 km altitude)
+mu = 3.986004418e14  # Earth gravitational parameter [m^3/s^2]
+a = 6.778e6  # Semi-major axis [m] (Earth radius + 400 km)
+n = np.sqrt(mu / a**3)  # Mean motion [rad/s] (~0.00113 rad/s)
+
+# Define state components
+# Position in CW frame [x, y, z] in meters
+position = ox.State("position", shape=(3,))
+position.max = np.array([100.0, 100.0, 100.0])
+position.min = np.array([-100.0, -100.0, -100.0])
+position.initial = np.array([0.0, -100.0, 0.0])  # Start 100m behind target (V-bar position)
+position.final = np.array([0.0, 0.0, 0.0])  # Dock at origin
+position.guess = np.linspace(position.initial, position.final, n_nodes)
+
+# Velocity in CW frame [vx, vy, vz] in m/s
+velocity = ox.State("velocity", shape=(3,))
+v_max = 2.0  # Maximum relative velocity [m/s]
+velocity.max = np.array([v_max, v_max, v_max])
+velocity.min = np.array([-v_max, -v_max, -v_max])
+velocity.initial = [0.0, 2.0, 0.0]
+velocity.final = np.array([0.0, 0.0, 0.0])  # Zero relative velocity at docking
+velocity.guess = np.zeros((n_nodes, 3))
+
+# Define control: acceleration from thrusters [ax, ay, az] in m/s^2
+accel = ox.Control("accel", shape=(3,))
+a_max = 0.1  # Maximum acceleration [m/s^2] (typical for reaction control thrusters)
+accel.max = np.array([a_max, a_max, a_max])
+accel.min = np.array([-a_max, -a_max, -a_max])
+accel.guess = np.zeros((n_nodes, 3))
+
+# Define list of all states and controls
+states = [position, velocity]
+controls = [accel]
+
+# Generate constraints
+constraints = []
+
+# Box constraints for states
+for state in states:
+    constraints.extend(
+        [
+            ox.ctcs(state <= state.max),
+            ox.ctcs(state.min <= state),
+        ]
+    )
+
+# R-bar approach cone constraint (from below, -x direction)
+# Enforces sqrt(y^2 + z^2) <= tan(theta) * (-x)
+# This requires the spacecraft to approach from negative x (below target)
+# and stay within a cone centered on the -x axis
+cone_half_angle = 20 * np.pi / 180  # 20 degree half-angle
+constraints.append(
+    ox.ctcs(ox.linalg.Norm(position[1:]) <= np.tan(cone_half_angle) * (-position[0])).over(
+        (n_nodes - 3, n_nodes - 1)
+    )
+)
+# Enforce entrance to the cone at safe distance
+constraints.append((-position[0] >= 20.0).at([n_nodes - 3]))
+
+# Clohessy-Wiltshire dynamics
+dynamics = {
+    "position": velocity,
+    "velocity": ox.Concat(
+        3 * n**2 * position[0] + 2 * n * velocity[1] + accel[0],
+        -2 * n * velocity[0] + accel[1],
+        -(n**2) * position[2] + accel[2],
+    ),
+}
+
+# Time configuration (free final time to minimize fuel)
+time = ox.Time(
+    initial=0.0,
+    final=("free", total_time),
+    min=0.0,
+    max=total_time,
+)
+
+# Build the problem
+problem = Problem(
+    dynamics=dynamics,
+    states=states,
+    controls=controls,
+    time=time,
+    constraints=constraints,
+    N=n_nodes,
+)
+
+# Solver settings (FOH is default, no need to set explicitly)
+problem.settings.dis.dis_type = "ZOH"
+problem.settings.scp.k_max = 100
+problem.settings.scp.w_tr = 1e0
+problem.settings.scp.w_tr_adapt = 1.2
+problem.settings.scp.lam_cost = 1e-1
+problem.settings.scp.lam_vc = 1e1
+problem.settings.scp.lam_vb = 1e0
+
+# Plotting metadata
+plotting_dict = {
+    "mean_motion": n,
+}
+
+if __name__ == "__main__":
+    problem.initialize()
+    results = problem.solve()
+    results = problem.post_process()
+    results.update(plotting_dict)
+
+    # Plot results
+    plot_scp_iterations(results).show()
+
+    # Create animation
+    traj_server = create_animated_plotting_server(results, thrust_key="accel", show_grid=False)
+
+    # Add R-bar approach cone (opens in -x direction)
+    add_glideslope_cone(
+        traj_server,
+        apex=(0, 0, 0),
+        height=20.0,  # Cone extends 100m in -x direction
+        glideslope_angle_deg=cone_half_angle * 180 / np.pi,
+        axis=(-1, 0, 0),  # R-bar: negative radial direction
+        color=(200, 80, 80),
+        opacity=0.5,
+    )
+
+    # Create SCP iteration visualization
+    scp_server = create_scp_animated_plotting_server(
+        results,
+        frame_duration_ms=200,
+        scene_scale=1.0,
+        show_grid=False,
+    )
+
+    # Add R-bar approach cone to SCP visualization
+    add_glideslope_cone(
+        scp_server,
+        apex=(0, 0, 0),
+        height=20.0,
+        glideslope_angle_deg=cone_half_angle * 180 / np.pi,
+        axis=(-1, 0, 0),
+        color=(200, 80, 80),
+        opacity=0.5,
+    )
+
+    scp_server.sleep_forever()

{openscvx-0.3.2.dev173 → openscvx-0.3.2.dev195}/openscvx/__init__.py

@@ -51,6 +51,7 @@ from openscvx.symbolic.expr import (
     Sum,
     Tan,
     Variable,
+    Vmap,
     Vstack,
     ctcs,
 )
@@ -113,6 +114,8 @@ __all__ = [
     "NodalConstraint",
     "CTCS",
     "ctcs",
+    # Data parallelism
+    "Vmap",
     # Submodules
     "stl",
     "spatial",

{openscvx-0.3.2.dev173 → openscvx-0.3.2.dev195}/openscvx/_version.py

@@ -28,7 +28,7 @@ version_tuple: VERSION_TUPLE
 commit_id: COMMIT_ID
 __commit_id__: COMMIT_ID
 
-__version__ = version = '0.3.2.dev173'
-__version_tuple__ = version_tuple = (0, 3, 2, 'dev173')
+__version__ = version = '0.3.2.dev195'
+__version_tuple__ = version_tuple = (0, 3, 2, 'dev195')
 
-__commit_id__ = commit_id = 'g99c4b988b'
+__commit_id__ = commit_id = 'ged5d19a8c'