diff --git a/.gitignore b/.gitignore index 846586a..6417165 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,6 @@ dist/ .coverage htmlcov/ .DS_Store + +# native PyO3 build artifact (build with maturin) +ghostloop/_rt_safety*.so diff --git a/ghostloop/policies/safe_projection.py b/ghostloop/policies/safe_projection.py index 3bc5d2a..344c159 100644 --- a/ghostloop/policies/safe_projection.py +++ b/ghostloop/policies/safe_projection.py @@ -16,6 +16,15 @@ Intent with corrected args + a ``"projected_from": orig_target`` field for auditability. + When the optional native extension ``ghostloop._rt_safety`` is + built AND the workspace carries only Sphere obstacles, the + analytic geometry runs in the allocation-free Rust core instead + of pure Python. The result is numerically identical (verified to + 1e-9 per axis in tests/test_rt_safety_equivalence.py); the + audit/return shape is unchanged. Any workspace with a box + obstacle, or any environment where the native module is not + built, transparently uses the pure-Python path below. + ``project_to_sdf(intent, workspace, extras=())`` Numerical: gradient-descent on the SDF until distance >= 0. Slower than the analytic version; handles convex polytopes / @@ -39,6 +48,15 @@ ) +# Optional allocation-free Rust fast path for the sphere-only analytic +# projection. Guarded so ghostloop runs unchanged when the native extension +# has not been built (the pure-Python path below is always available). +try: + from .. import _rt_safety as _RT_SAFETY # type: ignore[attr-defined] +except Exception: # noqa: BLE001 - any import/ABI failure must fall back cleanly. + _RT_SAFETY = None + + Point3 = tuple[float, float, float] @@ -61,6 +79,52 @@ def _set_target(args: dict, p: Point3) -> dict: return args +def _project_point_native( + target: Point3, workspace: WorkspaceModel, +) -> Point3 | None: + """Run the sphere-only analytic projection in the native Rust core. + + Returns the projected point, or ``None`` if the native path does not + apply (module not built, or the workspace has a non-sphere obstacle). + The numerics match the pure-Python branch below bit-for-bit: the Rust + core uses f64 and the identical operation order (clamp to bounds, then + sequential radial push-out per sphere, degenerate ``dist < 1e-9`` sets + ``p.x = center.x + forbidden_r`` and continues). + """ + if _RT_SAFETY is None: + return None + spheres = [] + for ob in workspace.obstacles: + if not isinstance(ob, Sphere): + # Box (or any future non-sphere) obstacle: the native core does + # not handle these analytically; defer to pure Python. + return None + spheres.append( + ( + float(ob.center[0]), + float(ob.center[1]), + float(ob.center[2]), + float(ob.radius), + float(ob.inflation), + ) + ) + bmin = ( + float(workspace.bounds_min[0]), + float(workspace.bounds_min[1]), + float(workspace.bounds_min[2]), + ) + bmax = ( + float(workspace.bounds_max[0]), + float(workspace.bounds_max[1]), + float(workspace.bounds_max[2]), + ) + try: + out = _RT_SAFETY.project_to_workspace(target, bmin, bmax, spheres) + except Exception: # noqa: BLE001 - never let the fast path break a repair. + return None + return float(out[0]), float(out[1]), float(out[2]) + + def project_to_workspace( intent: Intent, workspace: WorkspaceModel, ) -> Intent: @@ -73,32 +137,43 @@ def project_to_workspace( AxisAlignedBox obstacles are NOT pushed out of analytically (the closest-point-on-box-exterior is geometry that's annoying to compute robustly without numpy). For boxes use ``project_to_sdf``. + + When ``ghostloop._rt_safety`` is built and the workspace has only + Sphere obstacles, the analytic geometry runs in the native Rust core; + the result and the ``projected_from`` audit field are identical to the + pure-Python computation. Otherwise the pure-Python path runs unchanged. """ target = _extract_target(intent.args) if target is None: return intent - p = list(target) - # Clamp to outer bounds. - for i in range(3): - p[i] = max(workspace.bounds_min[i], min(workspace.bounds_max[i], p[i])) - # Radial pushout from each sphere. - for ob in workspace.obstacles: - if not isinstance(ob, Sphere): - continue - cx, cy, cz = ob.center - dx, dy, dz = p[0] - cx, p[1] - cy, p[2] - cz - dist = math.sqrt(dx * dx + dy * dy + dz * dz) - forbidden_r = ob.radius + ob.inflation - if dist < forbidden_r: - if dist < 1e-9: - # Degenerate: pick an arbitrary direction. - p[0] = cx + forbidden_r + + native = _project_point_native(target, workspace) + if native is not None: + out_target = native + else: + p = list(target) + # Clamp to outer bounds. + for i in range(3): + p[i] = max(workspace.bounds_min[i], min(workspace.bounds_max[i], p[i])) + # Radial pushout from each sphere. + for ob in workspace.obstacles: + if not isinstance(ob, Sphere): continue - scale = forbidden_r / dist - p[0] = cx + dx * scale - p[1] = cy + dy * scale - p[2] = cz + dz * scale - out_target = (p[0], p[1], p[2]) + cx, cy, cz = ob.center + dx, dy, dz = p[0] - cx, p[1] - cy, p[2] - cz + dist = math.sqrt(dx * dx + dy * dy + dz * dz) + forbidden_r = ob.radius + ob.inflation + if dist < forbidden_r: + if dist < 1e-9: + # Degenerate: pick an arbitrary direction. + p[0] = cx + forbidden_r + continue + scale = forbidden_r / dist + p[0] = cx + dx * scale + p[1] = cy + dy * scale + p[2] = cz + dz * scale + out_target = (p[0], p[1], p[2]) + if out_target == target: return intent new_args = _set_target(intent.args, out_target) diff --git a/native/rt_safety/.gitignore b/native/rt_safety/.gitignore new file mode 100644 index 0000000..889d174 --- /dev/null +++ b/native/rt_safety/.gitignore @@ -0,0 +1,2 @@ +/target +/.m4venv diff --git a/native/rt_safety/Cargo.lock b/native/rt_safety/Cargo.lock new file mode 100644 index 0000000..bf794f4 --- /dev/null +++ b/native/rt_safety/Cargo.lock @@ -0,0 +1,180 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "autocfg" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "indoc" +version = "2.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "79cf5c93f93228cf8efb3ba362535fb11199ac548a09ce117c9b1adc3030d706" +dependencies = [ + "rustversion", +] + +[[package]] +name = "libc" +version = "0.2.186" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" + +[[package]] +name = "memoffset" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "488016bfae457b036d996092f6cb448677611ce4449e970ceaf42695203f218a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "portable-atomic" +version = "1.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c33a9471896f1c69cecef8d20cbe2f7accd12527ce60845ff44c153bb2a21b49" + +[[package]] +name = "proc-macro2" +version = "1.0.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "pyo3" +version = "0.22.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f402062616ab18202ae8319da13fa4279883a2b8a9d9f83f20dbade813ce1884" +dependencies = [ + "cfg-if", + "indoc", + "libc", + "memoffset", + "once_cell", + "portable-atomic", + "pyo3-build-config", + "pyo3-ffi", + "pyo3-macros", + "unindent", +] + +[[package]] +name = "pyo3-build-config" +version = "0.22.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b14b5775b5ff446dd1056212d778012cbe8a0fbffd368029fd9e25b514479c38" +dependencies = [ + "once_cell", + "target-lexicon", +] + +[[package]] +name = "pyo3-ffi" +version = "0.22.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ab5bcf04a2cdcbb50c7d6105de943f543f9ed92af55818fd17b660390fc8636" +dependencies = [ + "libc", + "pyo3-build-config", +] + +[[package]] +name = "pyo3-macros" +version = "0.22.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fd24d897903a9e6d80b968368a34e1525aeb719d568dba8b3d4bfa5dc67d453" +dependencies = [ + "proc-macro2", + "pyo3-macros-backend", + "quote", + "syn", +] + +[[package]] +name = "pyo3-macros-backend" +version = "0.22.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "36c011a03ba1e50152b4b394b479826cad97e7a21eb52df179cd91ac411cbfbe" +dependencies = [ + "heck", + "proc-macro2", + "pyo3-build-config", + "quote", + "syn", +] + +[[package]] +name = "quote" +version = "1.0.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rt_safety" +version = "0.1.0" +dependencies = [ + "pyo3", +] + +[[package]] +name = "rustversion" +version = "1.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" + +[[package]] +name = "syn" +version = "2.0.118" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b9ae57f904213ebb649ce6895b8a66c66f0203b9319718f69a5612a065b1422" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "target-lexicon" +version = "0.12.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[package]] +name = "unindent" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7264e107f553ccae879d21fbea1d6724ac785e8c3bfc762137959b5802826ef3" diff --git a/native/rt_safety/Cargo.toml b/native/rt_safety/Cargo.toml new file mode 100644 index 0000000..23fb78d --- /dev/null +++ b/native/rt_safety/Cargo.toml @@ -0,0 +1,23 @@ +[package] +name = "rt_safety" +version = "0.1.0" +edition = "2021" +authors = ["Joe Munene "] +description = "Allocation-free, real-time-grade port of ghostloop's safe-projection envelope, exposed to Python via PyO3." +license = "MIT" + +[lib] +# cdylib: the Python extension module (.so / .pyd). +# rlib: so the same logic can be unit-tested with `cargo test` and reused +# by other Rust crates (the M4 rt_control campaign core). +name = "rt_safety" +crate-type = ["cdylib", "rlib"] + +[dependencies] +pyo3 = { version = "0.22", features = ["extension-module"] } + +[profile.release] +opt-level = 3 +lto = true +codegen-units = 1 +panic = "abort" diff --git a/native/rt_safety/pyproject.toml b/native/rt_safety/pyproject.toml new file mode 100644 index 0000000..182bff1 --- /dev/null +++ b/native/rt_safety/pyproject.toml @@ -0,0 +1,21 @@ +# maturin build config for the rt_safety extension. +# +# This pyproject exists only so `maturin` knows how to compile the PyO3 +# cdylib (it injects the macOS `-undefined dynamic_lookup` link flag that a +# bare `cargo build` of an extension-module cdylib lacks). The compiled +# artifact is installed into the ghostloop package as `ghostloop._rt_safety`; +# the parent ghostloop project itself stays a plain setuptools build. +[build-system] +requires = ["maturin>=1.7,<2"] +build-backend = "maturin" + +[project] +name = "ghostloop-rt-safety" +version = "0.1.0" +description = "Allocation-free real-time port of ghostloop's safe-projection envelope." +requires-python = ">=3.10" + +[tool.maturin] +# Install the compiled module at ghostloop._rt_safety so the in-tree shim's +# `from ghostloop import _rt_safety` resolves. +module-name = "ghostloop._rt_safety" diff --git a/native/rt_safety/src/lib.rs b/native/rt_safety/src/lib.rs new file mode 100644 index 0000000..acf6197 --- /dev/null +++ b/native/rt_safety/src/lib.rs @@ -0,0 +1,273 @@ +//! rt_safety: an allocation-free, real-time-grade port of ghostloop's +//! safe-projection envelope. +//! +//! This crate re-implements, bit-for-bit, the sphere-only fast path of +//! ghostloop's pure-Python safety geometry: +//! +//! ghostloop/policies/workspace.py -> `in_envelope` +//! (WorkspaceModel.violates for sphere-only workspaces: +//! outer-bounds AABB test + Sphere.contains inflated-radius test). +//! +//! ghostloop/policies/safe_projection.py -> `project_to_workspace` +//! (project_to_workspace: clamp target to the bounds AABB, then for +//! each sphere in order, if the point is inside the inflated radius, +//! push it radially out to exactly `radius + inflation`). +//! +//! Fidelity guarantees that make the native path a drop-in for the Python: +//! - `f64` throughout, matching CPython float. +//! - the *same* arithmetic order as the Python (`dx*dx + dy*dy + dz*dz`, +//! then `sqrt`, then compare against `radius + inflation`, then +//! `scale = forbidden_r / dist`). No fused-multiply-add, no reordering. +//! - the clamp is `max(lo, min(hi, p))`, identical to Python's +//! `max(bounds_min, min(bounds_max, p))`. +//! - the degenerate `dist < 1e-9` branch sets `p[0] = center.x + forbidden_r` +//! and continues, leaving `p[1]` / `p[2]` untouched, exactly as Python. +//! - the bounds test uses strict `<` / `>` and the sphere test uses `<=`, +//! matching `violates` and `Sphere.contains` respectively. +//! +//! Allocation discipline: the hot path (`project`, `in_envelope`) takes the +//! spheres as a borrowed slice and operates on fixed `[f64; 3]` stack arrays. +//! No `Vec`, no boxing, no locks, no I/O. The PyO3 wrappers do the only +//! allocation, and only to marshal the Python `list` of spheres into a small +//! stack buffer (`MAX_SPHERES`) before calling the pure core. + +#![allow(clippy::needless_range_loop)] + +use pyo3::prelude::*; +use pyo3::types::PyList; + +/// Cartesian degrees of freedom. ghostloop's workspace is strictly 3D. +pub const DOF: usize = 3; + +/// Maximum spheres marshalled onto the stack per call. Real ghostloop +/// workspaces carry a handful of obstacles; this cap keeps the marshalling +/// buffer fixed-size and the obstacle loop's worst case bounded. Workspaces +/// with more spheres than this fall back to Python (the shim only dispatches +/// to native when the count fits). +pub const MAX_SPHERES: usize = 64; + +/// A spherical forbidden region. `forbidden_r` is the already-summed +/// `radius + inflation`, so the core never has to recombine them and the +/// arithmetic order is fixed at the marshalling boundary. +#[derive(Clone, Copy)] +pub struct Sphere { + pub center: [f64; DOF], + pub forbidden_r: f64, +} + +/// Faithful port of `WorkspaceModel.violates` for a sphere-only workspace. +/// +/// Returns `true` iff the point is *valid* (inside the outer bounds AND +/// outside every inflated sphere): i.e. `violates(p) is None` in Python. +/// +/// Bounds test mirrors `violates`: a point exactly on a face is in-bounds +/// (`p < lo` / `p > hi` are the only rejections). Sphere test mirrors +/// `Sphere.contains`: `dist <= radius + inflation` is *inside* (a violation), +/// so a point exactly on the inflated surface is treated as contained, which +/// is why the projector pushes to *exactly* `forbidden_r` and the validity +/// check must therefore use the same `<=` boundary the projector lands on. +/// +/// NOTE: box obstacles are intentionally unsupported here. The Python shim +/// only routes sphere-only workspaces to this function; anything with an +/// `AxisAlignedBox` obstacle stays on the pure-Python path. +#[inline] +pub fn in_envelope( + p: &[f64; DOF], + bounds_min: &[f64; DOF], + bounds_max: &[f64; DOF], + spheres: &[Sphere], +) -> bool { + for i in 0..DOF { + if p[i] < bounds_min[i] || p[i] > bounds_max[i] { + return false; + } + } + for s in spheres { + // Same accumulation order as Sphere.contains: + // sqrt(sum((p[i]-c[i])**2)). + let dx = p[0] - s.center[0]; + let dy = p[1] - s.center[1]; + let dz = p[2] - s.center[2]; + let d = (dx * dx + dy * dy + dz * dz).sqrt(); + if d <= s.forbidden_r { + return false; + } + } + true +} + +/// Faithful port of `project_to_workspace`'s analytic geometry. +/// +/// Operation order, replicated exactly from safe_projection.py: +/// 1. Clamp every axis into `[bounds_min, bounds_max]` via +/// `max(lo, min(hi, p))`. +/// 2. For each sphere *in input order*, recompute the displacement from the +/// *current* (possibly already-pushed) point. If `dist < forbidden_r`: +/// - degenerate `dist < 1e-9`: set `p[0] = center.x + forbidden_r`, +/// leave `p[1]`/`p[2]`, and continue to the next sphere; +/// - otherwise scale all three axes by `forbidden_r / dist` about the +/// centre. +/// There is no re-clamp and no second pass, matching the Python exactly. +#[inline] +pub fn project_to_workspace( + target: &[f64; DOF], + bounds_min: &[f64; DOF], + bounds_max: &[f64; DOF], + spheres: &[Sphere], +) -> [f64; DOF] { + let mut p = *target; + + // (1) Clamp to outer bounds: max(lo, min(hi, p)). + for i in 0..DOF { + p[i] = f64::max(bounds_min[i], f64::min(bounds_max[i], p[i])); + } + + // (2) Radial push-out from each sphere, in order, against the live point. + for s in spheres { + let cx = s.center[0]; + let cy = s.center[1]; + let cz = s.center[2]; + let dx = p[0] - cx; + let dy = p[1] - cy; + let dz = p[2] - cz; + let dist = (dx * dx + dy * dy + dz * dz).sqrt(); + let forbidden_r = s.forbidden_r; + if dist < forbidden_r { + if dist < 1e-9 { + // Degenerate: arbitrary direction along +x; leave y, z. + p[0] = cx + forbidden_r; + continue; + } + let scale = forbidden_r / dist; + p[0] = cx + dx * scale; + p[1] = cy + dy * scale; + p[2] = cz + dz * scale; + } + } + + p +} + +// -------------------------------------------------------------------------- +// PyO3 marshalling boundary. +// +// Each sphere is passed from Python as a 5-tuple +// (cx, cy, cz, radius, inflation); we fold radius+inflation into forbidden_r +// here, at the boundary, so the order of that single add is fixed and the +// hot core never sees the split. +// -------------------------------------------------------------------------- + +fn marshal_spheres(spheres: &Bound<'_, PyList>) -> PyResult<([Sphere; MAX_SPHERES], usize)> { + let n = spheres.len(); + if n > MAX_SPHERES { + return Err(pyo3::exceptions::PyValueError::new_err(format!( + "rt_safety: {} spheres exceeds MAX_SPHERES={}", + n, MAX_SPHERES + ))); + } + let mut buf = [Sphere { center: [0.0; DOF], forbidden_r: 0.0 }; MAX_SPHERES]; + for (i, item) in spheres.iter().enumerate() { + let cx: f64 = item.get_item(0)?.extract()?; + let cy: f64 = item.get_item(1)?.extract()?; + let cz: f64 = item.get_item(2)?.extract()?; + let radius: f64 = item.get_item(3)?.extract()?; + let inflation: f64 = item.get_item(4)?.extract()?; + buf[i] = Sphere { + center: [cx, cy, cz], + // Same combination as Python: forbidden_r = ob.radius + ob.inflation. + forbidden_r: radius + inflation, + }; + } + Ok((buf, n)) +} + +/// Python: `_rt_safety.in_envelope(p, bounds_min, bounds_max, spheres) -> bool`. +/// +/// `p`, `bounds_min`, `bounds_max` are 3-tuples of float; `spheres` is a list +/// of `(cx, cy, cz, radius, inflation)` tuples. Returns `True` iff `p` is a +/// valid workspace point (equivalent to `WorkspaceModel.violates(p) is None`). +#[pyfunction] +#[pyo3(name = "in_envelope")] +fn py_in_envelope( + p: (f64, f64, f64), + bounds_min: (f64, f64, f64), + bounds_max: (f64, f64, f64), + spheres: &Bound<'_, PyList>, +) -> PyResult { + let (buf, n) = marshal_spheres(spheres)?; + let pa = [p.0, p.1, p.2]; + let bmin = [bounds_min.0, bounds_min.1, bounds_min.2]; + let bmax = [bounds_max.0, bounds_max.1, bounds_max.2]; + Ok(in_envelope(&pa, &bmin, &bmax, &buf[..n])) +} + +/// Python: `_rt_safety.project_to_workspace(p, bounds_min, bounds_max, spheres) +/// -> (f64, f64, f64)`. Numerically identical to the analytic geometry in +/// `safe_projection.project_to_workspace`. +#[pyfunction] +#[pyo3(name = "project_to_workspace")] +fn py_project_to_workspace( + p: (f64, f64, f64), + bounds_min: (f64, f64, f64), + bounds_max: (f64, f64, f64), + spheres: &Bound<'_, PyList>, +) -> PyResult<(f64, f64, f64)> { + let (buf, n) = marshal_spheres(spheres)?; + let target = [p.0, p.1, p.2]; + let bmin = [bounds_min.0, bounds_min.1, bounds_min.2]; + let bmax = [bounds_max.0, bounds_max.1, bounds_max.2]; + let out = project_to_workspace(&target, &bmin, &bmax, &buf[..n]); + Ok((out[0], out[1], out[2])) +} + +/// The Python module: `ghostloop._rt_safety`. +#[pymodule] +fn _rt_safety(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_function(wrap_pyfunction!(py_in_envelope, m)?)?; + m.add_function(wrap_pyfunction!(py_project_to_workspace, m)?)?; + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn unit_box() -> ([f64; DOF], [f64; DOF]) { + ([-1.0, -1.0, 0.0], [1.0, 1.0, 1.0]) + } + + #[test] + fn clamps_out_of_bounds() { + let (lo, hi) = unit_box(); + let out = project_to_workspace(&[5.0, -9.0, 0.5], &lo, &hi, &[]); + assert_eq!(out, [1.0, -1.0, 0.5]); + } + + #[test] + fn pushes_out_of_sphere() { + let (lo, hi) = unit_box(); + let s = Sphere { center: [0.0, 0.0, 0.5], forbidden_r: 0.25 }; + // Inside the sphere, off-centre. + let out = project_to_workspace(&[0.05, 0.0, 0.5], &lo, &hi, &[s]); + let d = ((out[0]) * (out[0]) + (out[2] - 0.5) * (out[2] - 0.5)).sqrt(); + assert!((d - 0.25).abs() < 1e-12, "landed at {:?}", out); + assert!(in_envelope(&out, &lo, &hi, &[s])); + } + + #[test] + fn degenerate_center_pushes_along_x() { + let (lo, hi) = unit_box(); + let s = Sphere { center: [0.0, 0.0, 0.5], forbidden_r: 0.25 }; + let out = project_to_workspace(&[0.0, 0.0, 0.5], &lo, &hi, &[s]); + assert_eq!(out, [0.25, 0.0, 0.5]); + } + + #[test] + fn valid_point_is_in_envelope() { + let (lo, hi) = unit_box(); + let s = Sphere { center: [0.0, 0.0, 0.5], forbidden_r: 0.25 }; + assert!(in_envelope(&[0.8, 0.8, 0.8], &lo, &hi, &[s])); + assert!(!in_envelope(&[0.0, 0.0, 0.5], &lo, &hi, &[s])); + assert!(!in_envelope(&[2.0, 0.0, 0.5], &lo, &hi, &[s])); + } +} diff --git a/tests/test_rt_safety_equivalence.py b/tests/test_rt_safety_equivalence.py new file mode 100644 index 0000000..eb1bca3 --- /dev/null +++ b/tests/test_rt_safety_equivalence.py @@ -0,0 +1,165 @@ +"""Equivalence proof: native rt_safety vs pure-Python safe_projection. + +For a few hundred randomized, sphere-only workspaces and targets (each +case seeded deterministically by its index, not by the wall clock), this +test asserts: + + 1. The native Rust ``project_to_workspace`` and the pure-Python + ``project_to_workspace`` agree to within 1e-9 on every axis. This is + the substantive claim: the allocation-free fast path is numerically + identical to the reference implementation it accelerates. + + 2. The projected point is inside the safety envelope: + ``WorkspaceModel.violates(projected) is None``. + + A point pushed *exactly* onto an inflated sphere surface is, by the + existing pure-Python semantics (``Sphere.contains`` uses ``<=``), + reported as a violation. That exact-boundary outcome is a property of + the reference implementation, not of the native port, and it occurs + identically in both. We therefore assert strict validity for every + case whose projected point is not within 1e-9 of a surface, and for + the rare on-surface case we assert the point is on-or-outside every + forbidden radius (does not penetrate) and is in-bounds. Both + implementations land on the identical point, which is what assertion + (1) already proves. + +If the native module is not built, the test is skipped (the pure-Python +path remains the production behavior). +""" + +from __future__ import annotations + +import math +import random + +import pytest + +from ghostloop.core import Intent +from ghostloop.policies import safe_projection as sp +from ghostloop.policies.workspace import Sphere, WorkspaceModel + + +pytestmark = pytest.mark.skipif( + sp._RT_SAFETY is None, + reason="native ghostloop._rt_safety extension not built", +) + +N_CASES = 400 +BOUNDS_MIN = (-1.0, -1.0, 0.0) +BOUNDS_MAX = (1.0, 1.0, 1.0) + + +def _make_case(idx: int) -> tuple[WorkspaceModel, tuple[float, float, float]]: + """Build a deterministic sphere-only workspace + target from the index.""" + rng = random.Random(idx) + n_spheres = rng.randint(0, 4) + obstacles = [ + Sphere( + center=( + rng.uniform(-0.6, 0.6), + rng.uniform(-0.6, 0.6), + rng.uniform(0.2, 0.8), + ), + radius=rng.uniform(0.03, 0.25), + inflation=rng.uniform(0.0, 0.05), + ) + for _ in range(n_spheres) + ] + ws = WorkspaceModel( + bounds_min=BOUNDS_MIN, bounds_max=BOUNDS_MAX, obstacles=obstacles + ) + # Targets range well outside the bounds and through obstacle interiors so + # both the clamp and the radial push-out paths are exercised. + target = ( + rng.uniform(-1.6, 1.6), + rng.uniform(-1.6, 1.6), + rng.uniform(-0.6, 1.6), + ) + return ws, target + + +def _project(ws: WorkspaceModel, target, native: bool): + """Run project_to_workspace with the native fast path on or off.""" + saved = sp._RT_SAFETY + try: + if not native: + sp._RT_SAFETY = None + intent = Intent("move_to", {"x": target[0], "y": target[1], "z": target[2]}) + out = sp.project_to_workspace(intent, ws) + finally: + sp._RT_SAFETY = saved + if out is intent: + return target, out + return (out.args["x"], out.args["y"], out.args["z"]), out + + +def test_native_matches_python_within_1e9(): + assert sp._RT_SAFETY is not None + on_surface_cases = 0 + for idx in range(N_CASES): + ws, target = _make_case(idx) + + p_native, intent_native = _project(ws, target, native=True) + p_python, intent_python = _project(ws, target, native=False) + + # (1) Numerical identity, every axis. + for axis in range(3): + assert abs(p_native[axis] - p_python[axis]) <= 1e-9, ( + f"case {idx} axis {axis}: native={p_native[axis]!r} " + f"python={p_python[axis]!r}" + ) + + # The audit field and return shape must match too. + if "projected_from" in intent_native.args or "projected_from" in intent_python.args: + assert intent_native.args.get("projected_from") == \ + intent_python.args.get("projected_from"), f"case {idx} audit field" + + # (2) Safety envelope. + v = ws.violates(p_native) + if v is None: + continue + # Inherent exact-boundary case: the point sits on (not inside) a + # forbidden surface. Assert it does not penetrate and is in-bounds. + on_surface_cases += 1 + for axis in range(3): + assert BOUNDS_MIN[axis] - 1e-9 <= p_native[axis] <= BOUNDS_MAX[axis] + 1e-9, ( + f"case {idx}: projected point out of bounds: {p_native}" + ) + for ob in ws.obstacles: + dist = math.sqrt(sum((p_native[i] - ob.center[i]) ** 2 for i in range(3))) + forbidden_r = ob.radius + ob.inflation + assert dist >= forbidden_r - 1e-9, ( + f"case {idx}: projected point penetrates sphere " + f"(dist={dist!r} < forbidden_r={forbidden_r!r})" + ) + + # Sanity: the test actually exercised both paths, not a degenerate run. + assert on_surface_cases <= N_CASES // 20, ( + f"unexpectedly many on-surface boundary cases: {on_surface_cases}" + ) + + +def test_in_envelope_matches_violates(): + """The native in_envelope predicate agrees with WorkspaceModel.violates.""" + assert sp._RT_SAFETY is not None + for idx in range(N_CASES): + ws, _ = _make_case(idx) + spheres = [ + ( + float(ob.center[0]), float(ob.center[1]), float(ob.center[2]), + float(ob.radius), float(ob.inflation), + ) + for ob in ws.obstacles + ] + rng = random.Random(10_000 + idx) + for _ in range(5): + p = ( + rng.uniform(-1.2, 1.2), + rng.uniform(-1.2, 1.2), + rng.uniform(-0.2, 1.2), + ) + native_ok = sp._RT_SAFETY.in_envelope(p, BOUNDS_MIN, BOUNDS_MAX, spheres) + python_ok = ws.violates(p) is None + assert native_ok == python_ok, ( + f"case {idx} point {p}: native={native_ok} python={python_ok}" + )