sci-solver 0.1.0__cp313-cp313-win_amd64.whl

sci_solver/__init__.py

@@ -0,0 +1,9 @@
+"""SCI: Sequential Contact Insertion for continuous-time robot fleet planning."""
+from .api import Solution, solve
+from .config import Config, ProblemConfig, SolverConfig
+from .solvers import ContactSolver, RelativeTrajectory
+
+__all__ = [
+    "Config", "ProblemConfig", "SolverConfig", "ContactSolver",
+    "RelativeTrajectory", "Solution", "solve",
+]

sci_solver/api.py

@@ -0,0 +1,76 @@
+"""Convenience interface around the reference joint solver."""
+from dataclasses import dataclass
+
+import numpy as np
+
+from .config import Config, ProblemConfig, SolverConfig
+from .evaluation import continuous_acceleration_cost
+from .solvers.contact_solver import ContactSolver, RelativeTrajectory
+from .solvers.vector_matching import VectorContact
+
+
+@dataclass
+class Solution:
+    """A returned iterate and its diagnostics; inspect success before using it.
+
+    The trajectory remains available when solving fails, for diagnosis.
+    """
+
+    trajectory: RelativeTrajectory
+    contacts: tuple[VectorContact, ...]
+    metrics: dict
+    acceleration_cost: float
+
+    @property
+    def success(self) -> bool:
+        return bool(self.metrics["converged"])
+
+    def sample(self, times, derivative: int = 0) -> np.ndarray:
+        """Evaluate position or derivative 1, 2, or 3; shape (robots, times, dimension).
+
+        Times must be a finite one-dimensional array within the planning horizon.
+        At an interior contact the third derivative uses the segment on its right.
+        """
+        return self.trajectory.robots(times, derivative)
+
+
+def solve(
+    initial_positions,
+    final_positions,
+    *,
+    time_horizon: float,
+    min_distance: float,
+    initial_velocities=None,
+    final_velocities=None,
+    options: SolverConfig | None = None,
+) -> Solution:
+    """Plan for N >= 2 robots in d >= 2 dimensions with prescribed endpoints.
+
+    Positions and optional velocities have shape (N, d). Omitted velocities
+    are zero. Use SolverConfig(sci_detection_tol=...) to set the absolute
+    separation allowance explicitly; omitted options retain the reference
+    solver's defaults. This local solver can return an unsuccessful iterate.
+    """
+    initial_positions = np.asarray(initial_positions, dtype=float)
+    final_positions = np.asarray(final_positions, dtype=float)
+    if initial_positions.ndim != 2 or final_positions.shape != initial_positions.shape:
+        raise ValueError("Endpoint positions must have matching shape (N, d)")
+    n, d = initial_positions.shape
+    if time_horizon is None:
+        raise ValueError("time_horizon must be positive and finite")
+    config = Config(
+        problem=ProblemConfig(
+            n_robots=n, dimension=d, time_horizon=time_horizon, min_distance=min_distance,
+        ),
+        solver=SolverConfig() if options is None else options,
+    )
+    solver = ContactSolver(config)
+    solver.set_initial_states(initial_positions, initial_velocities)
+    solver.set_final_states(final_positions, final_velocities)
+    result = solver.generate_trajectories()
+    return Solution(
+        trajectory=solver.trajectory,
+        contacts=tuple(solver.contacts),
+        metrics=result["metrics"],
+        acceleration_cost=continuous_acceleration_cost(solver),
+    )

sci_solver/cli/__init__.py

@@ -0,0 +1 @@
+"""Command-line interface for saved endpoint problems."""

sci_solver/cli/vector_sci.py

@@ -0,0 +1,142 @@
+"""Run explicit-vector SCI on NPZ endpoint data, or a bounded 20-case smoke suite."""
+
+import argparse
+import json
+import os
+import platform
+import sys
+from importlib.metadata import version
+from itertools import combinations
+from pathlib import Path
+
+import numpy as np
+
+from sci_solver import Config, ContactSolver
+from sci_solver.config import ProblemConfig, SolverConfig
+from sci_solver.evaluation import continuous_acceleration_cost
+
+
+def smoke_endpoints(d, case):
+    """Small deterministic cases; the fleet extends the manuscript's seed 82."""
+    if case in ("fleet", "fleet5"):
+        n = 5 if case == "fleet5" else 3
+        rng = np.random.default_rng(0 if n == 5 else 82)
+
+        def draw():
+            while True:
+                points = rng.uniform(-4, 4, (n, 2))
+                if all(
+                    np.linalg.norm(points[p] - points[q]) > 1.5
+                    for p, q in combinations(range(n), 2)
+                ):
+                    return points
+
+        start, goal = draw(), draw()
+        rng = np.random.default_rng(17)
+        start = np.column_stack((start, rng.normal(scale=0.1, size=(n, d - 2))))
+        goal = np.column_stack((goal, rng.normal(scale=0.1, size=(n, d - 2))))
+        return start, goal
+    start = np.zeros((2, d))
+    if case == "free":
+        start[1, 0] = 3
+        return start, start + 1
+    start[:, :2] = [[-2, -0.2], [2, 0.2]]
+    if case == "head_on":
+        start[:, 1] = 0
+    goal = start.copy()
+    goal[:, 0] *= -1
+    return start, goal
+
+
+def smoke_report():
+    records = []
+    for d in (2, 3, 5, 10):
+        for case in ("free", "head_on", "offset", "fleet", "fleet5"):
+            start, goal = smoke_endpoints(d, case)
+            cfg = Config(
+                name=f"smoke_{case}_{d}",
+                problem=ProblemConfig(
+                    n_robots=len(start), dimension=d, time_horizon=6, timestep=0.1, min_distance=1
+                ),
+                solver=SolverConfig(
+                    sci_detection_tol=1e-6,
+                    use_analytical_jacobian=True,
+                ),
+            )
+            solver = ContactSolver(cfg)
+            solver.set_initial_states(start)
+            solver.set_final_states(goal)
+            metrics = solver.generate_trajectories()["metrics"]
+            record = dict(case=case, dimension=d, n_robots=len(start), vector=metrics)
+            record["vector_acceleration_cost"] = continuous_acceleration_cost(solver)
+            records.append(record)
+    return dict(
+        environment=dict(
+            python=sys.version,
+            platform=platform.platform(),
+            packages={name: version(name) for name in ("numpy", "scipy")},
+            thread_limits={
+                name: os.environ.get(name)
+                for name in ("OPENBLAS_NUM_THREADS", "OMP_NUM_THREADS", "VECLIB_MAXIMUM_THREADS")
+            },
+        ),
+        records=records,
+    )
+
+
+
+def main(argv=None):
+    parser = argparse.ArgumentParser(description=__doc__)
+    source = parser.add_mutually_exclusive_group(required=True)
+    source.add_argument("--smoke", action="store_true", help="Only 20 small cases, N <= 5")
+    source.add_argument(
+        "--states",
+        type=Path,
+        help="NPZ with initial_positions/final_positions and optional velocities",
+    )
+    parser.add_argument("--config", type=Path, help="JSON Config, required with --states")
+    parser.add_argument("--output", type=Path, help="JSON metrics path; otherwise print JSON")
+    parser.add_argument(
+        "--trajectories", type=Path, help="Optional sampled NPZ output for a single solve"
+    )
+    args = parser.parse_args(argv)
+    if args.smoke:
+        if args.config or args.trajectories:
+            parser.error("--config/--trajectories apply only to --states")
+        report = smoke_report()
+        success = all(record["vector"]["converged"] for record in report["records"])
+    else:
+        if args.config is None:
+            parser.error("--states requires --config")
+        solver = ContactSolver(Config.from_json(args.config))
+        with np.load(args.states, allow_pickle=False) as data:
+            solver.set_initial_states(data["initial_positions"], data.get("initial_velocities"))
+            solver.set_final_states(data["final_positions"], data.get("final_velocities"))
+        result = solver.generate_trajectories()
+        report = result["metrics"]
+        success = report["converged"]
+        if args.trajectories:
+            args.trajectories.parent.mkdir(parents=True, exist_ok=True)
+            np.savez(
+                args.trajectories,
+                times=result["times"],
+                **result["trajectories"],
+                contact_states=solver.solution.reshape(len(solver.contacts), solver.system.b),
+                contact_pairs=np.array([c.pair for c in solver.contacts], dtype=int).reshape(-1, 2),
+            )
+    encoded = json.dumps(
+        report,
+        indent=2,
+        allow_nan=False,
+        default=lambda value: value.item() if isinstance(value, np.generic) else str(value),
+    )
+    if args.output:
+        args.output.parent.mkdir(parents=True, exist_ok=True)
+        args.output.write_text(encoded + "\n")
+    else:
+        print(encoded)
+    return 0 if success else 1
+
+
+if __name__ == "__main__":
+    sys.exit(main())

sci_solver/config.py

@@ -0,0 +1,138 @@
+"""Solver configuration, independent of experiment and plotting dependencies."""
+from dataclasses import asdict, dataclass, field
+import json
+from pathlib import Path
+import numpy as np
+
+@dataclass
+class EnvironmentConfig:
+    pos_x_min: float | None = None
+    pos_x_max: float | None = None
+    pos_y_min: float | None = None
+    pos_y_max: float | None = None
+    # Full-dimensional alternatives to the legacy x/y fields; do not mix them.
+    position_lower: list[float] | None = None
+    position_upper: list[float] | None = None
+
+    def bounds(self, dimension: int) -> tuple[np.ndarray, np.ndarray]:
+        if dimension < 2:
+            raise ValueError("Position bounds require dimension >= 2")
+        legacy = (self.pos_x_min, self.pos_y_min, self.pos_x_max, self.pos_y_max)
+        if (self.position_lower is not None or self.position_upper is not None) and any(
+            value is not None for value in legacy
+        ):
+            raise ValueError("Do not mix position_lower/upper with legacy x/y bounds")
+        lower, upper = np.full(dimension, -np.inf), np.full(dimension, np.inf)
+        for axis, value in enumerate(legacy[:2]):
+            if value is not None:
+                lower[axis] = value
+        for axis, value in enumerate(legacy[2:]):
+            if value is not None:
+                upper[axis] = value
+        for values, target in ((self.position_lower, lower), (self.position_upper, upper)):
+            if values is not None:
+                array = np.asarray(values, dtype=float)
+                if array.shape != (dimension,):
+                    raise ValueError("Position bounds must have one entry per spatial dimension")
+                target[:] = array
+        if np.any(np.isnan(lower)) or np.any(np.isnan(upper)) or np.any(lower > upper):
+            raise ValueError("Invalid position bounds")
+        return lower, upper
+
+    @property
+    def pos_min(self) -> np.ndarray:
+        dimension = len(
+            self.position_lower
+            if self.position_lower is not None
+            else self.position_upper if self.position_upper is not None else [0, 0]
+        )
+        return self.bounds(dimension)[0]
+
+    @property
+    def pos_max(self) -> np.ndarray:
+        dimension = len(
+            self.position_lower
+            if self.position_lower is not None
+            else self.position_upper if self.position_upper is not None else [0, 0]
+        )
+        return self.bounds(dimension)[1]
+
+@dataclass
+class DynamicsConfig:
+    vel_min: float | None = None
+    vel_max: float | None = None
+    acc_min: float | None = None
+    acc_max: float | None = None
+    jerk_min: float | None = None
+    jerk_max: float | None = None
+
+@dataclass
+class ProblemConfig:
+    n_robots: int
+    min_distance: float
+    time_horizon: float | None = None
+    dimension: int = 2
+    timestep: float = 0.1
+    environment: EnvironmentConfig = field(default_factory=EnvironmentConfig)
+    dynamics: DynamicsConfig = field(default_factory=DynamicsConfig)
+
+    def __post_init__(self):
+        if isinstance(self.n_robots, bool) or not isinstance(self.n_robots, int) or self.n_robots < 2:
+            raise ValueError("n_robots must be an integer >= 2")
+        if isinstance(self.dimension, bool) or not isinstance(self.dimension, int) or self.dimension < 2:
+            raise ValueError("dimension must be an integer >= 2")
+        if not np.isfinite(self.timestep) or self.timestep <= 0:
+            raise ValueError("timestep must be positive and finite")
+        if self.time_horizon is not None and (not np.isfinite(self.time_horizon) or self.time_horizon <= 0):
+            raise ValueError("time_horizon must be positive and finite")
+
+    @property
+    def n_timesteps(self) -> int | None:
+        return None if self.time_horizon is None else int(self.time_horizon / self.timestep)
+
+
+@dataclass
+class SolverConfig:
+    sci_detection_tol: float = 0.0
+    sci_min_contact_gap: float = 0.0
+    sci_residual_tol: float = 1e-7
+    sci_mass_tol: float = 1e-9
+    sci_root_max_iterations: int = 100
+    sci_reaction_recovery: bool = True
+    timeout: float = 300.0
+    use_analytical_jacobian: bool = True
+
+    def __post_init__(self):
+        if (isinstance(self.sci_root_max_iterations, bool)
+                or not isinstance(self.sci_root_max_iterations, int)
+                or self.sci_root_max_iterations < 0):
+            raise ValueError("sci_root_max_iterations must be a nonnegative integer")
+
+
+@dataclass
+class Config:
+    problem: ProblemConfig
+    solver: SolverConfig = field(default_factory=SolverConfig)
+    name: str = "sci"
+
+    def to_dict(self) -> dict:
+        return asdict(self)
+
+    @classmethod
+    def from_dict(cls, values: dict) -> "Config":
+        values = dict(values)
+        problem = dict(values.pop("problem"))
+        for key, schema in (("environment", EnvironmentConfig), ("dynamics", DynamicsConfig)):
+            if key in problem:
+                problem[key] = schema(**problem[key])
+        return cls(problem=ProblemConfig(**problem),
+                   solver=SolverConfig(**values.pop("solver", {})), **values)
+
+    @classmethod
+    def from_json(cls, path: str | Path) -> "Config":
+        return cls.from_dict(json.loads(Path(path).read_text()))
+
+    def to_json(self, path: str | Path) -> None:
+        path = Path(path)
+        path.parent.mkdir(parents=True, exist_ok=True)
+        path.write_text(json.dumps(self.to_dict(), indent=2, allow_nan=False) + "\n")

sci_solver/evaluation.py

@@ -0,0 +1,22 @@
+"""Measurements of the continuous trajectory."""
+import numpy as np
+
+def continuous_acceleration_cost(solver, *, knots=None):
+    """Exact cubic-path integral; explicit knots support saved baseline adapters."""
+    if knots is None:
+        if hasattr(solver, "trajectory"):
+            knots = solver.trajectory.times
+        else:
+            knots = sorted(
+                {
+                    t
+                    for path in solver._trajectories.values()
+                    for _, a, b in path.segments
+                    for t in (a, b)
+                }
+            )
+    cost = 0.0
+    for a, b in zip(knots[:-1], knots[1:], strict=True):
+        times = (a + b) / 2 + (b - a) / (2 * np.sqrt(3)) * np.array([-1.0, 1.0])
+        cost += (b - a) / 2 * np.sum(np.asarray(solver.sample(times=times)[3]) ** 2)
+    return float(cost)

sci_solver/solvers/__init__.py

@@ -0,0 +1,4 @@
+"""Numerical implementation of sequential contact insertion."""
+from .contact_solver import ContactSolver, RelativeTrajectory
+
+__all__ = ["ContactSolver", "RelativeTrajectory"]

sci_solver/solvers/_cubic_minima.c

@@ -0,0 +1,177 @@
+/* Batched continuous separation kernel: same derivative partition root isolation.
+ * No fast-math, no discarded small leading terms, no sampling acceptance. */
+#include <math.h>
+#include <float.h>
+#include <stdlib.h>
+#ifdef _MSC_VER
+#include <malloc.h>
+#endif
+
+static double value(const double *p, int degree, double x) {
+    double y=p[degree];
+    for(int i=degree-1;i>=0;--i)y=y*x+p[i];
+    return y;
+}
+static int compare(const void *a,const void *b) {
+    double x=*(const double*)a,y=*(const double*)b;
+    return (x>y)-(x<y);
+}
+static int unique(double *a,int n) {
+    qsort(a,n,sizeof(double),compare);
+    int k=0;
+    for(int i=0;i<n;++i)if(k==0 || a[i]!=a[k-1])a[k++]=a[i];
+    return k;
+}
+static double bracket(const double *p,int degree,double a,double b,double fa) {
+    double x=(a+b)*0.5;
+    for(int k=0;k<100;++k) {
+        double fx=value(p,degree,x);
+        if(fx==0)return x;
+        if(signbit(fx)==signbit(fa)){a=x;fa=fx;}else b=x;
+        if(b-a<=1e-14+4*DBL_EPSILON*fabs(x))return (a+b)*0.5;
+        double derivative=degree*p[degree];
+        for(int j=degree-1;j>=1;--j)derivative=derivative*x+j*p[j];
+        double trial=x-fx/derivative;
+        /* Safeguard Newton by insisting on progress inside the bracket. */
+        /* Every other step bisects: at least 50 halvings within the cap,
+         * even for a very flat root. There is no silent loose termination. */
+        x=(k%2==0&&isfinite(trial)&&trial>a+0.1*(b-a)&&trial<b-0.1*(b-a))?trial:(a+b)*0.5;
+    }
+    return x;
+}
+static int roots(const double *input,int degree,double *out) {
+    while(degree>0 && input[degree]==0)--degree;
+    if(degree==0)return 0;
+    double p[6],largest=0;
+    for(int i=0;i<=degree;++i)largest=fmax(largest,fabs(input[i]));
+    for(int i=0;i<=degree;++i)p[i]=input[i]/largest;
+    if(degree==1){double x=-p[0]/p[1];if(x>=0&&x<=1){out[0]=x;return 1;}return 0;}
+    double derivative[5],critical[32],partition[34];
+    for(int i=1;i<=degree;++i)derivative[i-1]=i*p[i];
+    int count=roots(derivative,degree-1,critical),n=0;
+    partition[0]=0;partition[1]=1;
+    for(int i=0;i<count;++i){partition[i+2]=critical[i];out[n++]=critical[i];}
+    int parts=unique(partition,count+2);
+    for(int i=0;i<parts-1;++i){
+        double a=partition[i],b=partition[i+1],fa=value(p,degree,a),fb=value(p,degree,b);
+        if(fa==0)out[n++]=a;
+        if(fb==0)out[n++]=b;
+        if(fa!=0&&fb!=0&&signbit(fa)!=signbit(fb))out[n++]=bracket(p,degree,a,b,fa);
+    }
+    return unique(out,n);
+}
+static double distance(const double *c,int d,double t) {
+    double sum=0;
+    for(int j=0;j<d;++j){double x=c[j]+t*(c[d+j]+t*(c[2*d+j]+t*c[3*d+j]));sum+=x*x;}
+    return sqrt(sum);
+}
+static double lower_bound(const double *c,int d,double *guard) {
+#ifdef _MSC_VER
+    /* MSVC has no C variable-length arrays; retain the same stack workspace. */
+    double *controls=_alloca((size_t)4*d*sizeof(double));
+    double *direction=_alloca((size_t)d*sizeof(double));
+    double *delta=_alloca((size_t)d*sizeof(double));
+    double sq=0,dot=0,magnitude=0;
+#else
+    double controls[4*d],direction[d],delta[d],sq=0,dot=0,magnitude=0;
+#endif
+    for(int j=0;j<d;++j){
+        controls[j]=c[j];controls[d+j]=c[j]+c[d+j]/3;
+        controls[2*d+j]=c[j]+2*c[d+j]/3+c[2*d+j]/3;
+        controls[3*d+j]=c[j]+c[d+j]+c[2*d+j]+c[3*d+j];
+        delta[j]=controls[3*d+j]-c[j];sq+=delta[j]*delta[j];dot+=c[j]*delta[j];
+        for(int k=0;k<4;++k)magnitude+=fabs(c[k*d+j]);
+    }
+    double u=sq>0?fmax(0,fmin(1,-dot/sq)):0,norm=0,box=0;
+    for(int j=0;j<d;++j){
+        double lo=controls[j],hi=lo;
+        for(int k=1;k<4;++k){lo=fmin(lo,controls[k*d+j]);hi=fmax(hi,controls[k*d+j]);}
+        double near=fmax(0,fmax(lo,-hi));box+=near*near;
+        direction[j]=c[j]+u*delta[j];norm+=direction[j]*direction[j];
+    }
+    norm=sqrt(norm);double support=INFINITY;
+    if(norm>0){for(int k=0;k<4;++k){double s=0;for(int j=0;j<d;++j)s+=controls[k*d+j]*(direction[j]/norm);support=fmin(support,s);}}
+    else support=0;
+    *guard=1024*DBL_EPSILON*fmax(1,magnitude);
+    return fmax(0,fmax(sqrt(box),support)-*guard);
+}
+/* Outputs per-polynomial minima. Pruned entries stay +inf. */
+int batch_minima(const double *coefficients,int count,int d,int pruning,double *times,double *distances,int *solved) {
+    double upper=INFINITY,best=INFINITY;
+    *solved=0;
+    if(pruning)for(int i=0;i<count;++i){
+        const double *c=coefficients+i*4*d;double magnitude=0;
+        for(int j=0;j<d;++j)for(int k=0;k<4;++k)magnitude+=fabs(c[k*d+j]);
+        double guard=1024*DBL_EPSILON*fmax(1,magnitude);
+        upper=fmin(upper,fmin(distance(c,d,0),fmin(distance(c,d,.5),distance(c,d,1)))+guard);
+    }
+    for(int i=0;i<count;++i){
+        const double *c=coefficients+i*4*d;double guard=0;
+        distances[i]=INFINITY;times[i]=0;
+        if(pruning && lower_bound(c,d,&guard)>fmin(upper,best)+guard)continue;
+        ++*solved;
+        double p[6]={0},candidates[64];
+        for(int j=0;j<d;++j)for(int k=0;k<4;++k)for(int l=0;l<3;++l)p[k+l]+=c[k*d+j]*(l+1)*c[(l+1)*d+j];
+        for(int k=0;k<6;++k)if(!isfinite(p[k]))return -1;
+        int nr=roots(p,5,candidates);
+        candidates[nr++]=0;candidates[nr++]=1;nr=unique(candidates,nr);
+        for(int k=0;k<nr;++k){double q=distance(c,d,candidates[k]);if(q<distances[i]){times[i]=candidates[k];distances[i]=q;}}
+        best=fmin(best,distances[i]);
+    }
+    return 0;
+}
+
+/* Python buffer API keeps the build independent of NumPy headers. */
+#define PY_SSIZE_T_CLEAN
+#include <Python.h>
+#include <string.h>
+
+static int double_buffer(PyObject *object, Py_buffer *buffer, int writable) {
+    int flags = PyBUF_FORMAT | PyBUF_C_CONTIGUOUS;
+    if (writable) flags |= PyBUF_WRITABLE;
+    if (PyObject_GetBuffer(object, buffer, flags) < 0) return -1;
+    if (buffer->itemsize != sizeof(double) || strcmp(buffer->format, "d") != 0) {
+        PyErr_SetString(PyExc_ValueError, "Expected contiguous native float64 buffers");
+        return -1;
+    }
+    return 0;
+}
+
+static PyObject *python_minima(PyObject *self, PyObject *args) {
+    PyObject *coefficients, *times, *distances;
+    Py_buffer c = {0}, t = {0}, r = {0};
+    int count, dimension, pruning, solved, status;
+    if (!PyArg_ParseTuple(args, "OiipOO", &coefficients, &count, &dimension,
+                          &pruning, &times, &distances)) return NULL;
+    if (count < 0 || dimension < 1 ||
+        (Py_ssize_t)count > PY_SSIZE_T_MAX / (Py_ssize_t)sizeof(double) / 4 / dimension) {
+        PyErr_SetString(PyExc_ValueError, "Invalid batch dimensions");
+        return NULL;
+    }
+    if (double_buffer(coefficients, &c, 0) < 0 || double_buffer(times, &t, 1) < 0 ||
+        double_buffer(distances, &r, 1) < 0) goto error;
+    if (c.len != (Py_ssize_t)count * 4 * dimension * (Py_ssize_t)sizeof(double) ||
+        t.len != (Py_ssize_t)count * (Py_ssize_t)sizeof(double) || r.len != t.len) {
+        PyErr_SetString(PyExc_ValueError, "Buffer sizes do not match batch dimensions");
+        goto error;
+    }
+    status = batch_minima(c.buf, count, dimension, pruning, t.buf, r.buf, &solved);
+    PyBuffer_Release(&c); PyBuffer_Release(&t); PyBuffer_Release(&r);
+    if (status) {
+        PyErr_SetString(PyExc_ValueError, "Nonfinite distance derivative polynomial");
+        return NULL;
+    }
+    return PyLong_FromLong(solved);
+error:
+    if (c.obj) PyBuffer_Release(&c);
+    if (t.obj) PyBuffer_Release(&t);
+    if (r.obj) PyBuffer_Release(&r);
+    return NULL;
+}
+
+static PyMethodDef methods[] = {
+    {"minima", python_minima, METH_VARARGS, "Compute batched cubic distance minima."},
+    {NULL, NULL, 0, NULL}
+};
+static struct PyModuleDef module = {PyModuleDef_HEAD_INIT, "_cubic_minima", NULL, -1, methods};
+PyMODINIT_FUNC PyInit__cubic_minima(void) { return PyModule_Create(&module); }

sci_solver/solvers/base.py

@@ -0,0 +1,95 @@
+"""Base solver class for trajectory optimization."""
+
+from abc import ABC, abstractmethod
+
+import numpy as np
+
+from ..config import Config
+
+
+class Solver(ABC):
+    # Opt in to the general-dimensional base endpoint setters. ContactSolver
+    # has its own setters. Legacy and prioritized solvers retain planar guards.
+    supports_nd_states = False
+
+    def __init__(self, config: Config):
+        self.config = config
+
+        self.N = config.problem.n_robots
+        self.d = config.problem.dimension
+        if not isinstance(self.d, int) or self.d < 2:
+            raise ValueError("problem.dimension must be an integer >= 2")
+        self.h = config.problem.timestep
+        self.R = config.problem.min_distance
+
+        self.T = config.problem.time_horizon
+        self.K = config.problem.n_timesteps if self.T is not None else None
+
+        self.T_MULTIPLIER = 1.0
+
+        # Environment limits (None = unconstrained)
+        self.pos_min, self.pos_max = config.problem.environment.bounds(self.d)
+
+        # Dynamics limits (None = unconstrained)
+        self.vel_min = config.problem.dynamics.vel_min
+        self.vel_max = config.problem.dynamics.vel_max
+        self.acc_min = config.problem.dynamics.acc_min
+        self.acc_max = config.problem.dynamics.acc_max
+        self.jerk_min = config.problem.dynamics.jerk_min
+        self.jerk_max = config.problem.dynamics.jerk_max
+
+        self.initial_positions = None
+        self.initial_velocities = None
+        self.final_positions = None
+        self.final_velocities = None
+        self.trajectories = None
+
+    def set_initial_states(self, positions, velocities=None):
+        self.initial_positions, self.initial_velocities = self._endpoint_states(
+            positions, velocities
+        )
+
+    def set_final_states(self, positions, velocities=None):
+        self.final_positions, self.final_velocities = self._endpoint_states(positions, velocities)
+
+        if self.T is None:
+            if self.initial_positions is None:
+                raise ValueError("Call set_initial_states() before set_final_states()")
+            self.T = self._auto_time_horizon()
+            self.K = int(round(self.T / self.h))
+
+    def _endpoint_states(self, positions, velocities):
+        if self.d != 2 and not self.supports_nd_states:
+            raise ValueError(f"{type(self).__name__} supports only dimension=2")
+        positions = np.asarray(positions, dtype=float)
+        velocities = (
+            np.zeros_like(positions) if velocities is None else np.asarray(velocities, dtype=float)
+        )
+        shapes = ((self.N, self.d), (self.N * self.d,))
+        if positions.shape not in shapes or velocities.shape not in shapes:
+            raise ValueError(
+                f"Expected endpoint positions/velocities of shape ({self.N}, {self.d}) or flattened"
+            )
+        if not np.all(np.isfinite(positions)) or not np.all(np.isfinite(velocities)):
+            raise ValueError("Endpoint states must be finite")
+        return positions.ravel().copy(), velocities.ravel().copy()
+
+    def _auto_time_horizon(self) -> float:
+        initial = self.initial_positions.reshape(-1, self.d)
+        final = self.final_positions.reshape(-1, self.d)
+
+        distances = np.linalg.norm(final - initial, axis=1)
+        max_distance = distances.max()
+
+        v_max = self.vel_max if self.vel_max is not None else 2.0
+        T = (1.5 * max_distance) / v_max
+        T *= self.T_MULTIPLIER
+
+        K = int(np.ceil(T / self.h))
+        T = K * self.h
+
+        return float(T)
+
+    @abstractmethod
+    def generate_trajectories(self, **kwargs):
+        pass