openscvx 0.3.2.dev279__tar.gz → 0.3.2.dev295__tar.gz

{openscvx-0.3.2.dev279/openscvx.egg-info → openscvx-0.3.2.dev295}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: openscvx
-Version: 0.3.2.dev279
+Version: 0.3.2.dev295
 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.dev295/examples/car/dubins_car_conditional.py

@@ -0,0 +1,144 @@
+"""Dubins car path planning with conditional velocity constraint using Cond operator.
+
+This example demonstrates the use of the Cond operator for conditional logic
+in constraints. The car has different maximum velocity limits based on proximity
+to the obstacle:
+
+- When far from obstacle: higher maximum velocity (10.0)
+- When near obstacle: lower maximum velocity (5.0) for safer navigation
+
+This showcases how to use ox.Cond for JAX-traceable conditional branching
+in constraint expressions.
+"""
+
+import os
+import sys
+
+import jax.numpy as jnp
+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 import plot_dubins_car, plot_velocity_vs_distance
+from openscvx import Problem
+
+n = 8
+total_time = 2.0  # Total simulation time
+
+# Define state components
+position = ox.State("position", shape=(2,))  # 2D position [x, y]
+position.min = np.array([-5.0, -5.0])
+position.max = np.array([5.0, 5.0])
+position.initial = np.array([0, -2])
+position.final = np.array([0, 2])
+
+theta = ox.State("theta", shape=(1,))  # Heading angle
+theta.min = np.array([-2 * jnp.pi])
+theta.max = np.array([2 * jnp.pi])
+theta.initial = np.array([0])
+theta.final = [ox.Free(0)]
+
+# Define control components
+speed = ox.Control("speed", shape=(1,))  # Forward speed
+speed.min = np.array([0])
+speed.max = np.array([10])
+speed.guess = np.ones((n, 1)) * 10.0
+
+angular_rate = ox.Control("angular_rate", shape=(1,))  # Angular velocity
+angular_rate.min = np.array([-5])
+angular_rate.max = np.array([5])
+angular_rate.guess = np.zeros((n, 1))
+
+# Define list of all states and controls
+states = [position, theta]
+controls = [speed, angular_rate]
+
+# Define Parameters with initial values for obstacle radius and center
+obs_center = ox.Parameter("obs_center", shape=(2,), value=np.array([-0.01, 0.0]))
+obs_radius = ox.Parameter("obs_radius", shape=(), value=1.0)
+
+# Safety distance threshold for conditional velocity constraint
+safety_threshold = 1.1  # Distance from obstacle where we reduce max velocity
+
+# Generate box constraints for all states
+constraints = []
+for state in states:
+    constraints.extend([ox.ctcs(state <= state.max), ox.ctcs(state.min <= state)])
+
+# Add obstacle avoidance constraint
+constraints.append(ox.ctcs(obs_radius <= ox.linalg.Norm(position - obs_center)))
+
+# Add the conditional velocity constraint
+distance_to_obstacle = ox.linalg.Norm(position - obs_center)
+constraints.append(
+    ox.ctcs(
+        speed <= ox.Cond(distance_to_obstacle <= safety_threshold, 5.0, 10.0),
+        idx=1,
+        penalty="smooth_relu",
+    )
+)
+
+
+# Define normal dynamics (no conditional logic here)
+dynamics = {
+    "position": ox.Concat(
+        speed * ox.Sin(theta),  # x_dot
+        speed * ox.Cos(theta),  # y_dot
+    ),
+    "theta": angular_rate,
+}
+
+
+# Build the problem (parameters auto-collected from Parameter objects)
+time = ox.Time(
+    initial=0.0,
+    final=ox.Minimize(total_time),
+    min=0.0,
+    max=20,
+)
+
+problem = Problem(
+    dynamics=dynamics,
+    states=states,
+    controls=controls,
+    time=time,
+    constraints=constraints,
+    N=n,
+    licq_max=1e-6,
+)
+
+# Set solver parameters
+problem.settings.scp.w_tr = 1e0
+problem.settings.scp.w_tr_adapt = 1.1
+problem.settings.scp.w_tr_max_scaling_factor = 1e2
+problem.settings.scp.lam_cost = 1e0
+problem.settings.scp.lam_vc = 1e3
+problem.settings.scp.uniform_time_grid = True
+
+
+plotting_dict = {
+    "obs_radius": problem.parameters["obs_radius"],
+    "obs_center": problem.parameters["obs_center"],
+    "safety_threshold": safety_threshold,
+}
+
+if __name__ == "__main__":
+    print("Dubins Car with Conditional Velocity Constraint (using Cond operator)")
+    print("=" * 70)
+    print("Max velocity is 5.0 when within 2.0 units of obstacle, else 10.0")
+    print("=" * 70)
+
+    problem.initialize()
+    results = problem.solve()
+    results = problem.post_process()
+    results.update(plotting_dict)
+
+    # Plot trajectory
+    plot_dubins_car(results, problem.settings).show()
+
+    # Plot velocity vs distance to obstacle
+    plot_velocity_vs_distance(results, problem.settings).show()

{openscvx-0.3.2.dev279 → openscvx-0.3.2.dev295}/examples/plotting.py

@@ -322,6 +322,86 @@ def plot_dubins_car(results: OptimizationResults, params: Config):
     return fig
 
 
+def plot_velocity_vs_distance(results: OptimizationResults, params: Config):
+    """Plot velocity against distance to obstacle.
+
+    This plot demonstrates how the conditional velocity constraint works,
+    showing how velocity changes based on proximity to the obstacle.
+    """
+    fig = go.Figure()
+
+    # Extract position and velocity
+    position = results.trajectory["position"]
+    velocity = results.trajectory.get("speed")
+
+    if velocity is None:
+        # If speed is not available, try to get it from controls
+        velocity = results.controls.get("speed")
+
+    if velocity is None:
+        raise ValueError("Velocity data not found in results")
+
+    # Flatten velocity to 1D array
+    velocity = np.asarray(velocity).flatten()
+
+    # Get obstacle center and radius
+    obs_center = results.plotting_data["obs_center"]
+    _ = results.plotting_data["obs_radius"]
+
+    # Calculate distance to obstacle center for each point
+    # Distance = ||position - obs_center||
+    distance_from_center = np.linalg.norm(position - obs_center, axis=1)
+
+    # Plot velocity vs distance
+    fig.add_trace(
+        go.Scatter(
+            x=distance_from_center,
+            y=velocity,
+            mode="lines+markers",
+            line={"color": "blue", "width": 2},
+            marker={"size": 5},
+            name="Velocity",
+        )
+    )
+
+    # Add vertical line at safety threshold if available
+    if "safety_threshold" in results.plotting_data:
+        safety_threshold = results.plotting_data["safety_threshold"]
+        fig.add_vline(
+            x=safety_threshold,
+            line_dash="dash",
+            line_color="red",
+            annotation_text=f"Safety threshold ({safety_threshold:.2f})",
+            annotation_position="top",
+        )
+
+    # Add horizontal lines for max velocities
+    fig.add_hline(
+        y=5.0,
+        line_dash="dot",
+        line_color="orange",
+        annotation_text="Max velocity (near): 5.0",
+        annotation_position="right",
+    )
+    fig.add_hline(
+        y=10.0,
+        line_dash="dot",
+        line_color="green",
+        annotation_text="Max velocity (far): 10.0",
+        annotation_position="right",
+    )
+
+    fig.update_layout(
+        title="Velocity vs Distance to Obstacle",
+        xaxis_title="Distance from Obstacle Center",
+        yaxis_title="Velocity",
+        template="plotly_dark",
+        title_x=0.5,
+    )
+
+    return fig
+
+
 def plot_dubins_car_disjoint(results: OptimizationResults, params: Config):
     # Plot the trajectory of the Dubins car, but show wp1 and wp2 as circles with centers and radii
     fig = go.Figure()

{openscvx-0.3.2.dev279 → openscvx-0.3.2.dev295}/openscvx/__init__.py

@@ -16,9 +16,12 @@ from openscvx.symbolic.expr import (
     CTCS,
     Abs,
     Add,
+    All,
+    Any,
     Bilerp,
     Block,
     Concat,
+    Cond,
     Constant,
     Constraint,
     Control,
@@ -114,6 +117,10 @@ __all__ = [
     "Max",
     "Linterp",
     "Bilerp",
+    # Logical/control flow operations
+    "All",
+    "Any",
+    "Cond",
     # Constraints
     "Constraint",
     "Equality",

{openscvx-0.3.2.dev279 → openscvx-0.3.2.dev295}/openscvx/_version.py

@@ -28,7 +28,7 @@ version_tuple: VERSION_TUPLE
 commit_id: COMMIT_ID
 __commit_id__: COMMIT_ID
 
-__version__ = version = '0.3.2.dev279'
-__version_tuple__ = version_tuple = (0, 3, 2, 'dev279')
+__version__ = version = '0.3.2.dev295'
+__version_tuple__ = version_tuple = (0, 3, 2, 'dev295')
 
-__commit_id__ = commit_id = 'g815902cab'
+__commit_id__ = commit_id = 'g9a2fa0dbd'

{openscvx-0.3.2.dev279 → openscvx-0.3.2.dev295}/openscvx/symbolic/expr/__init__.py

@@ -115,6 +115,9 @@ from .lie import (
 # Linear algebra operations
 from .linalg import Diag, Inv, Norm, Sum, Transpose
 
+# Logical/control flow operations
+from .logic import All, Any, Cond
+
 # Mathematical functions
 from .math import (
     Abs,
@@ -198,6 +201,10 @@ __all__ = [
     "Max",
     "Linterp",
     "Bilerp",
+    # Logical/control flow operations
+    "All",
+    "Any",
+    "Cond",
     # Linear algebra operations
     "Transpose",
     "Stack",

{openscvx-0.3.2.dev279 → openscvx-0.3.2.dev295}/openscvx/symbolic/expr/constraint.py

@@ -89,21 +89,25 @@ class Constraint(Expr):
         return new_constraint
 
     def check_shape(self) -> Tuple[int, ...]:
-        """Check that constraint operands are broadcastable. Returns scalar shape."""
+        """Check that constraint operands are broadcastable and return the shape.
+
+        Returns the broadcasted shape of lhs and rhs, which represents the shape
+        of the constraint residual (lhs - rhs). Vector constraints are interpreted
+        element-wise.
+
+        Returns:
+            tuple: The broadcasted shape of lhs and rhs
+        """
         L_shape = self.lhs.check_shape()
         R_shape = self.rhs.check_shape()
 
         # Figure out their broadcasted shape (or error if incompatible)
         try:
-            np.broadcast_shapes(L_shape, R_shape)
+            return np.broadcast_shapes(L_shape, R_shape)
         except ValueError as e:
             constraint_type = type(self).__name__
             raise ValueError(f"{constraint_type} not broadcastable: {L_shape} vs {R_shape}") from e
 
-        # Allow vector constraints - they're interpreted element-wise
-        # Return () as constraints always produce a scalar
-        return ()
-
     def at(self, nodes: Union[list, tuple]):
         """Apply this constraint only at specific discrete nodes.