embodik 0.1.1__tar.gz → 0.3.0__tar.gz

{embodik-0.1.1 → embodik-0.3.0}/.gitignore

@@ -20,6 +20,10 @@ python_bindings/**/py.typed
 build/**/*.pyi
 build/**/py.typed
 
+# Ignore generated type stubs copied into the source tree (nanobind.stubgen)
+*.pyi
+*.typed
+
 # CMake build artifacts (root directory)
 CMakeCache.txt
 CMakeFiles/

embodik-0.3.0/CHANGELOG.md

@@ -0,0 +1,109 @@
+# Changelog
+
+All notable changes to this project will be documented in this file.
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
+and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
+
+## [0.3.0] - 2026-02-03
+
+### Added
+- **GPU Acceleration**: Batched velocity IK solving with massive parallelism (100-500x speedup)
+  - Achieves ~670,000 IK solves/second at batch size 10,000
+  - Ideal for RL training (4096+ parallel environments), motion planning, and dataset generation
+  - GPU batched solver via CusADi-compiled CUDA kernels
+- **FI-PeSNS Solver**: Fixed-Iteration Penalized eSNS algorithm optimized for GPU
+  - Singularity-Robust Inverse (SRINV) for numerical stability
+  - Analytical scaling for feasible task scales without iterative saturation
+  - Penalty gradient approach for constraint enforcement
+  - Fixed iterations for predictable compute time (ideal for real-time RL)
+- **GPU Collision Detection**: GPU-accelerated collision detection via NVIDIA Warp
+  - Batched collision queries for parallel processing
+  - Experimental support for GPU-accelerated self-collision avoidance
+- **Parallel Trajectory Tracking Demo**: Visualize 100 robot instances simultaneously tracking different trajectories
+  - Interactive demo with Viser visualization
+  - Demonstrates GPU parallelization capabilities
+  - Achieves ~50,000+ IK solves/second with GPU acceleration
+- **Teleoperation IK Example**: Interactive IK control using Seer wireless controller (Xvisio SDK)
+- **GPU Benchmarks and Demos**: Comprehensive benchmarking tools for GPU performance
+  - Batch IK performance benchmarks
+  - FI-PeSNS vs CPU accuracy benchmarks
+  - Collision detection benchmarks
+  - GPU solver demonstration panels
+- **CasADi Integration**: Export and compile velocity IK functions to CUDA kernels
+  - Symbolic function export for GPU compilation
+  - CusADi integration for CUDA kernel generation
+- **GPU Environment Support**: CUDA feature in pixi.toml with GPU-specific tasks
+  - `check-cuda`, `check-gpu`, `install-cusadi` tasks
+  - GPU demos and benchmarks via pixi tasks
+  - CUDA environment configuration
+
+### Changed
+- Enhanced examples with GPU acceleration support
+- Updated dependencies to include xvisio SDK support for teleoperation examples
+- Improved GPU documentation and setup instructions
+
+### Dependencies
+- Added `casadi>=3.6.0` and `torch>=2.0.0` to `[gpu]` optional dependencies
+- Added `warp-lang>=1.0.0` to `[gpu-collision]` optional dependencies
+- Added `xvisio>=0.3.1` to pixi dependencies for teleoperation examples
+
+## [0.2.0] - 2026-01-30
+
+### Added
+- Position IK now supports `excluded_joint_indices` to lock joints during solves
+- Position IK can enforce collision constraints during iterative solves (when configured)
+
+### Changed
+- Velocity solver now groups same-priority tasks to make ordering within a priority level symmetric
+- Collision constraint computation now uses cached allow-masks to skip excluded pairs efficiently
+- Collision recovery near `min_distance` uses deadband + adaptive push to reduce jitter and stalling
+- Collision constraint default `nearest_points_all_pairs` set to true
+- Collision debug evaluation is side-effect free (no solver state mutation)
+
+### Fixed
+- Position IK now applies collision constraints and excluded joint indices consistently with velocity IK
+- Collision constraint pair selection now uses hysteresis across frames for stability
+
+## [0.1.1] - 2025-01-09
+
+### Added
+- **PyPI Publishing**: Full wheel building and publishing workflow for TestPyPI and PyPI
+- **`embodik-sanitize-env` CLI**: Helper to sanitize `LD_LIBRARY_PATH` for clean pip installs
+- **`embodik-examples` CLI**: Tool to list and copy examples for pip-installed users
+- **Built-in robot presets**: `panda` and `iiwa` presets work without `robot_presets.yaml`
+- **sdist build support**: Source distribution builds now auto-detect PyPI `pin` wheel's Pinocchio
+
+### Changed
+- Simplified `_runtime_deps.py` - removed complex preloading logic, now just provides `import_pinocchio()` helper
+- Documentation updated to recommend `venv + pip + unset LD_LIBRARY_PATH` as primary user flow
+- ViserVisualizer now uses empty GeometryModel instead of None when collisions disabled
+
+### Fixed
+- Fixed `KeyError: 'pinocchio/frames/universe'` in ViserVisualizer when `load_collisions=False`
+- Fixed RPATH for `_embodik_impl.so` to correctly find `libembodik_core.so`
+- Fixed sdist builds failing to find Pinocchio by auto-detecting `pin` wheel's CMake config
+- Examples now work correctly for pip-installed users via `embodik-examples --copy`
+
+### Dependencies
+- Added `pin>=3.8.0` to build-system requirements for sdist builds
+- Added `pyyaml`, `robot_descriptions`, `viser`, `yourdfpy` to `[examples]` optional dependencies
+
+## [0.1.0] - 2025-12-12
+
+### Added
+- Initial release of embodiK
+- High-performance inverse kinematics solver with hierarchical task resolution
+- Python bindings using nanobind
+- Support for multiple task types (FrameTask, PostureTask, COMTask, JointTask, MultiJointTask)
+- Position and velocity IK solving
+- Optional visualization tools using Viser
+- Comprehensive test suite
+- Example scripts demonstrating various use cases
+
+### Technical Details
+- C++17 core library built on Pinocchio and Eigen3
+- Python 3.8+ support
+- Linux x86_64 support
+- CMake-based build system
+- scikit-build-core for Python packaging

{embodik-0.1.1 → embodik-0.3.0}/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.16)
-project(embodik VERSION 0.1.1)
+project(embodik VERSION 0.2.0)
 
 # Python is used to auto-discover Pinocchio (PyPI `pin`) CMake config during sdist builds.
 find_package(Python REQUIRED COMPONENTS Interpreter)

embodik-0.3.0/PKG-INFO

@@ -0,0 +1,548 @@
+Metadata-Version: 2.2
+Name: embodik
+Version: 0.3.0
+Summary: High-performance inverse kinematics solver optimized for cross-embodiment VLA/AI applications
+Keywords: robotics,inverse-kinematics,optimization,motion-planning
+Author-Email: Andy Park <andypark.purdue@gmail.com>
+License: MIT
+Classifier: Development Status :: 3 - Alpha
+Classifier: Intended Audience :: Science/Research
+Classifier: Intended Audience :: Developers
+Classifier: License :: OSI Approved :: MIT License
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.10
+Classifier: Programming Language :: Python :: 3.11
+Classifier: Programming Language :: Python :: 3.12
+Classifier: Programming Language :: C++
+Classifier: Topic :: Scientific/Engineering :: Artificial Intelligence
+Classifier: Topic :: Scientific/Engineering :: Mathematics
+Classifier: Topic :: Software Development :: Libraries :: Python Modules
+Project-URL: Homepage, https://github.com/embodik/embodik
+Project-URL: Repository, https://github.com/embodik/embodik
+Project-URL: Documentation, https://embodik.readthedocs.io
+Project-URL: Bug Reports, https://github.com/embodik/embodik/issues
+Requires-Python: >=3.10
+Requires-Dist: numpy>=1.26.0
+Requires-Dist: pin>=3.8.0
+Provides-Extra: visualization
+Requires-Dist: viser>=0.1.0; extra == "visualization"
+Requires-Dist: trimesh>=3.0.0; extra == "visualization"
+Provides-Extra: visualization-legacy
+Requires-Dist: viser>=0.1.0; extra == "visualization-legacy"
+Requires-Dist: yourdfpy>=0.0.52; extra == "visualization-legacy"
+Provides-Extra: examples
+Requires-Dist: pyyaml>=6.0; extra == "examples"
+Requires-Dist: robot_descriptions>=1.0.0; extra == "examples"
+Requires-Dist: viser>=0.1.0; extra == "examples"
+Requires-Dist: yourdfpy>=0.0.52; extra == "examples"
+Provides-Extra: gpu
+Requires-Dist: casadi>=3.6.0; extra == "gpu"
+Requires-Dist: torch>=2.0.0; extra == "gpu"
+Provides-Extra: gpu-collision
+Requires-Dist: warp-lang>=1.0.0; extra == "gpu-collision"
+Provides-Extra: dev
+Requires-Dist: pytest>=7.0; extra == "dev"
+Requires-Dist: pytest-cov>=4.0; extra == "dev"
+Requires-Dist: black>=24.0; extra == "dev"
+Requires-Dist: isort>=5.13.0; extra == "dev"
+Description-Content-Type: text/markdown
+
+# EmbodiK: Cross-Embodiment Inverse Kinematics with Nanobind
+
+EmbodiK is a high-performance inverse kinematics (IK) library for cross-embodiment VLA/AI applications.
+
+- The core is implemented in C++, with Python bindings created using Nanobind.
+- EmbodiK delivers robust and high-performance IK behaviors, particularly optimized for humanoid robots and AI/VLA integrations.
+- The name "EmbodiK" highlights its focus on supporting various kinematic structures across different embodiment types.
+- The library handles diverse constraint types, supporting both single-task and multi-task velocity IK solvers.
+- Advanced inverse methods provide singularity-robustness.
+- Features include self-collision avoidance and interactive 3D visualization tools.
+
+**Author:** Andy Park <andypark.purdue@gmail.com>
+
+## Features
+
+- **High Performance**: C++ core with optimized Eigen linear algebra
+- **Python Integration**: Seamless numpy array support via Nanobind
+- **Multiple Solvers**: Single-step and full multi-task velocity IK
+- **Singularity Robust**: Advanced inverse methods for stable solutions
+- **Constraint Support**: Joint limits and operational space constraints
+- **Collision Avoidance**: Self-collision detection and avoidance
+- **Visualization**: Interactive 3D visualization with Viser
+- **Robot Models**: Built-in support for common robots (Panda, IIWA)
+- **GPU Acceleration**: Batched velocity IK via CusADi for massive parallelism (100-500x speedup)
+
+## Installation
+
+### Option A: Fresh Environment (No existing Pinocchio)
+
+If you don't have Pinocchio/Boost installed locally, installation is straightforward:
+
+```bash
+python3 -m venv .venv
+source .venv/bin/activate
+pip install -U pip
+
+# Install build dependencies and Pinocchio
+pip install pin scikit-build-core nanobind cmake ninja
+
+# Set CMAKE_PREFIX_PATH and install
+export CMAKE_PREFIX_PATH=$(python -c "import pinocchio, pathlib; print(pathlib.Path(pinocchio.__file__).resolve().parents[4])")
+pip install --no-build-isolation embodik
+
+# Verify
+python -c "import embodik; print(embodik.__version__, embodik.RobotModel)"
+```
+
+### Option B: Robotics Environment (Existing Pinocchio/ROS)
+
+If you have local Pinocchio/Boost builds (e.g., from source or ROS), you **must** clear conflicting paths first:
+
+```bash
+python3 -m venv .venv
+source .venv/bin/activate
+pip install -U pip
+
+# IMPORTANT: Clear local Pinocchio paths to avoid library conflicts
+unset LD_LIBRARY_PATH CMAKE_PREFIX_PATH pinocchio_DIR
+
+# Install build dependencies and Pinocchio from PyPI
+pip install pin scikit-build-core nanobind cmake ninja
+
+# Set CMAKE_PREFIX_PATH to the PyPI pin package
+export CMAKE_PREFIX_PATH=$(python -c "import pinocchio, pathlib; print(pathlib.Path(pinocchio.__file__).resolve().parents[4])")
+
+# Install embodik
+pip install --no-build-isolation embodik
+
+# Verify
+python -c "import embodik; print(embodik.__version__, embodik.RobotModel)"
+```
+
+### Running Examples
+
+```bash
+pip install "embodik[examples]"
+embodik-examples --copy
+cd embodik_examples
+python 01_basic_ik_simple.py --robot panda
+```
+
+### Troubleshooting
+
+| Error | Cause | Fix |
+|-------|-------|-----|
+| `ImportError: libboost_*.so...` | `LD_LIBRARY_PATH` points to local Pinocchio | `unset LD_LIBRARY_PATH` |
+| `CMake cannot find pinocchio` | Build can't find Pinocchio config | Set `CMAKE_PREFIX_PATH` (see above) |
+| `Cannot import scikit_build_core` | Missing build deps with `--no-build-isolation` | `pip install scikit-build-core nanobind cmake ninja` |
+
+### For Developers
+
+See [docs/installation.md](docs/installation.md) for development setup with Pixi.
+
+See [PUBLISHING.md](PUBLISHING.md) for wheel building and PyPI publishing.
+
+## Quick Start
+
+```python
+import embodik
+import numpy as np
+
+# Load robot model from URDF
+robot = embodik.RobotModel("path/to/robot.urdf", floating_base=False)
+
+# Create kinematics solver
+solver = embodik.KinematicsSolver(robot)
+
+# Add a frame task for end-effector control
+frame_task = solver.add_frame_task("ee_task", "end_effector")
+frame_task.priority = 0
+frame_task.weight = 1.0
+
+# Set target velocity (6D: 3 linear + 3 angular)
+target_velocity = np.array([0.1, 0.0, 0.0, 0.0, 0.0, 0.0])
+frame_task.set_target_velocity(target_velocity)
+
+# Solve velocity IK
+q = np.zeros(robot.nq)
+result = solver.solve_velocity(q, apply_limits=True)
+
+if result.status == embodik.SolverStatus.SUCCESS:
+    print(f"Joint velocities: {result.joint_velocities}")
+```
+
+## API Overview
+
+### High-Level API (Recommended)
+
+EmbodiK provides a high-level API built on top of Pinocchio for easy robot modeling and IK solving:
+
+```python
+import embodik
+import numpy as np
+
+# Create robot model
+robot = embodik.RobotModel("robot.urdf", floating_base=False)
+
+# Create solver
+solver = embodik.KinematicsSolver(robot)
+
+# Add tasks
+frame_task = solver.add_frame_task("task1", "end_effector")
+posture_task = solver.add_posture_task("posture")
+
+# Configure tasks
+frame_task.priority = 0
+frame_task.weight = 1.0
+posture_task.priority = 1
+posture_task.weight = 0.1
+
+# Solve
+q = np.zeros(robot.nq)
+result = solver.solve_velocity(q, apply_limits=True)
+```
+
+### Low-Level API
+
+For advanced users, EmbodiK also provides low-level multi-task velocity IK functions:
+
+```python
+import embodik as eik
+import numpy as np
+
+# Multiple tasks with constraints
+goals = [np.array([0.1, -0.2]), np.array([0.3])]
+jacobians = [
+    np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]),
+    np.array([[0.0, 0.0, 1.0]])
+]
+
+# Constraint matrix and limits
+C = np.eye(3)
+lower = np.array([-1e6, -1e6, -1e6])
+upper = np.array([1e6, 1e6, 1e6])
+
+params = {
+    "epsilon": 1e-6,
+    "regularization_factor": 1e-1,
+}
+
+result = eik.solve_velocity_ik_multi_task_np(
+    goals, jacobians, C, lower, upper, params
+)
+```
+
+## Examples
+
+The repository includes several example scripts:
+
+| Script | Description |
+|--------|-------------|
+| `01_basic_ik_simple.py` | Basic IK solving with interactive visualization |
+| `02_collision_aware_IK.py` | Collision-aware IK with self-collision avoidance + GPU benchmark panel |
+| `04_gpu_batch_ik.py` | GPU-accelerated batched velocity IK benchmark |
+| `05_gpu_collision_batch.py` | GPU-accelerated batch collision detection |
+| `06_gpu_solver_demo.py` | Comprehensive GPU solver demonstration and benchmark |
+| `07_parallel_trajectory_tracking.py` | **100 robots** tracking different trajectories in parallel (GPU demo) |
+| `robot_model_example.py` | Robot model usage and configuration |
+| `visualization_example.py` | Interactive 3D visualization examples |
+| `scripts/benchmark_fi_pesns.py` | FI-PeSNS vs CPU accuracy and performance benchmark |
+
+### Running Examples
+
+**For pip-installed users:**
+```bash
+# Install with example dependencies
+pip install embodik[examples]
+
+# Copy examples to a local directory
+embodik-examples --copy
+
+# Run examples
+cd embodik_examples
+python 01_basic_ik_simple.py --robot panda
+python 02_collision_aware_IK.py --robot panda
+```
+
+**For developers (from repository):**
+```bash
+# Install example dependencies
+pixi run install
+
+# Run basic IK example
+pixi run python examples/01_basic_ik_simple.py
+
+# Run collision-aware IK example
+pixi run python examples/02_collision_aware_IK.py --robot panda
+
+# Run GPU examples (requires cuda environment)
+pixi run -e cuda demo-gpu          # GPU solver benchmark
+pixi run -e cuda demo-ik-gpu       # Interactive IK with GPU panel
+pixi run -e cuda benchmark-gpu     # Batch IK benchmark
+pixi run -e cuda benchmark-collision  # Collision detection benchmark
+```
+
+See the [Examples Documentation](docs/examples/index.md) for detailed guides.
+
+## GPU Acceleration
+
+EmbodiK supports GPU-accelerated batched velocity IK solving for massive parallelism, ideal for:
+
+- **RL Training**: 4096+ parallel environments in Isaac Gym/Orbit
+- **Motion Planning**: Batch trajectory validation
+- **Dataset Generation**: Offline batch processing
+
+### Performance
+
+| Batch Size | CPU Sequential | GPU Batched | Speedup | Per-Sample | Constraint Sat |
+|------------|----------------|-------------|---------|------------|----------------|
+| 100        | 3.3 ms         | 1.6 ms      | **2x**  | 16 µs      | 100%           |
+| 1,000      | 29 ms          | 3.1 ms      | **9x**  | 3 µs       | 100%           |
+| 10,000     | 300 ms         | 15 ms       | **20x** | 1.5 µs     | 100%           |
+
+*Benchmarks on NVIDIA RTX A2000 8GB. FI-PeSNS solver with k_max=12, 7-DOF robot, 6D task.*
+
+**Key Results:**
+- **~670,000 IK solves/second** at batch size 10,000
+- **100% constraint satisfaction** with zero violations
+- Speedup scales with batch size due to GPU parallelism
+
+### Quick Start (GPU)
+
+```python
+from embodik import solve_velocity_batched
+
+# Batch of IK problems (e.g., 1000 parallel environments)
+result = solve_velocity_batched(
+    targets_batch,      # List of (task_dim,) arrays
+    jacobians_batch,    # List of (task_dim, n_dof) arrays
+    constraints_batch,  # List of (n_dof, n_dof) arrays
+    lower_bounds_batch,
+    upper_bounds_batch,
+    use_gpu=True,
+    casadi_path="path/to/fn_velocity_solve.casadi"
+)
+
+velocities = result.velocities  # (batch_size, n_dof)
+```
+
+### Setup
+
+1. **Install CUDA environment:**
+   ```bash
+   cd embodik
+   pixi install -e cuda
+   pixi run -e cuda install        # Install embodik in cuda env
+   pixi run -e cuda check-cuda     # Verify PyTorch CUDA
+   ```
+
+2. **Install CusADi (one-time):**
+   ```bash
+   pixi run -e cuda install-cusadi   # Clones to ~/.local/cusadi and installs
+   pixi run -e cuda check-gpu        # Verify all GPU components
+   # Output: CasADi: True, CusADi: True, CUDA: True
+   ```
+
+3. **Export and compile CasADi function:**
+   ```bash
+   # Export symbolic function
+   pixi run -e cuda export-casadi
+
+   # Compile to CUDA kernel
+   mv fn_velocity_solve.casadi ~/.local/cusadi/src/casadi_functions/
+   cd ~/.local/cusadi
+   python run_codegen.py --fn=fn_velocity_solve
+   ```
+
+4. **Run GPU demos:**
+   ```bash
+   pixi run -e cuda demo-gpu           # Comprehensive benchmark
+   pixi run -e cuda demo-ik-gpu        # Interactive IK with GPU panel
+   pixi run -e cuda benchmark-gpu      # Batch IK benchmark
+   pixi run -e cuda benchmark-collision  # Collision benchmark
+   ```
+
+### Available GPU Tasks
+
+| Task | Description |
+|------|-------------|
+| `pixi run -e cuda check-cuda` | Verify PyTorch CUDA availability |
+| `pixi run -e cuda check-gpu` | Verify CasADi + CusADi + CUDA |
+| `pixi run -e cuda install-cusadi` | Install CusADi from GitHub |
+| `pixi run -e cuda export-casadi` | Export FI-PeSNS velocity solve function |
+| `pixi run -e cuda demo-gpu` | Run GPU solver demo/benchmark |
+| `pixi run -e cuda demo-ik-gpu` | Interactive IK with GPU benchmark panel |
+| `pixi run -e cuda benchmark-gpu` | Batch IK performance benchmark |
+| `pixi run -e cuda benchmark-gpu-batched` | GPU batched IK benchmark (100/1000/10000) |
+| `pixi run -e cuda benchmark-fi-pesns` | FI-PeSNS vs CPU accuracy benchmark |
+| `pixi run -e cuda benchmark-collision` | Collision detection benchmark |
+| `pixi run -e cuda demo-parallel-tracking` | 100 robots tracking trajectories in parallel |
+| `pixi run -e cuda test-gpu` | Run GPU-specific tests |
+
+### FI-PeSNS: Fixed-Iteration Penalized eSNS
+
+EmbodiK includes **FI-PeSNS**, a GPU-optimized variant of the eSNS algorithm that trades exact constraint saturation for simpler, parallelizable penalty-based enforcement:
+
+**Key Features:**
+- **SRINV**: Singularity-Robust Inverse for numerical stability
+- **Analytical Scaling**: Computes feasible task scales without iterative saturation
+- **Penalty Gradient**: Nudges solution toward feasibility each iteration
+- **Fixed Iterations**: Predictable compute time, ideal for real-time RL
+
+**Algorithm:**
+```
+for i in range(k_max):
+    P = I  # Reset projector
+    for each task:
+        J_pinv = srinv(J @ P)
+        delta = J_pinv @ (target - J @ dq)
+        scale = get_feasible_scale(...)
+        dq += scale * delta
+        P -= J_pinv @ J @ P
+
+    # Penalty nudge toward feasibility
+    violation = max(0, max(lower - C@dq, C@dq - upper))
+    dq += eta * mu * C.T @ grad_violation
+    mu *= gamma  # Ramp penalty
+```
+
+**Benchmark (7-DOF Panda, 6D task):**
+
+| Mode | Batch | Time | Per-Sample | Max Violation | Constraint Sat |
+|------|-------|------|------------|---------------|----------------|
+| CPU Sequential | 100 | 3.3 ms | 33 µs | 0.0 | 100% |
+| CPU Sequential | 1,000 | 29 ms | 29 µs | 0.0 | 100% |
+| **GPU Batched** | 100 | 1.6 ms | 16 µs | 0.0 | 100% |
+| **GPU Batched** | 1,000 | 3.1 ms | 3 µs | 0.0 | 100% |
+| **GPU Batched** | 10,000 | 15 ms | 1.5 µs | 0.0 | 100% |
+
+*GPU benchmarks on NVIDIA RTX A2000 8GB with CusADi-compiled CUDA kernels.*
+
+### Parallel Trajectory Tracking Demo
+
+Visualize GPU parallelization with 100 robot instances simultaneously tracking different trajectories:
+
+```bash
+# Run the interactive demo (requires viser)
+pixi run -e cuda demo-parallel-tracking
+
+# Run benchmark only (no visualization)
+pixi run -e cuda demo-parallel-tracking-benchmark
+```
+
+Each robot tracks a unique trajectory (circles, figure-8s, spirals, hearts) while the GPU solver computes all 100 IK solutions in parallel. With GPU acceleration, this achieves **~50,000+ IK solves/second**.
+
+**Usage:**
+```python
+from embodik.gpu.casadi_fi_pesns import build_fi_pesns_single_task
+
+# Build solver
+fn = build_fi_pesns_single_task(
+    n_dof=7, task_dim=6, n_constraints=7,
+    k_max=10,    # Fixed iterations
+    mu0=1e-2,    # Initial penalty
+    gamma=2.0,   # Penalty growth
+    eta=0.1,     # Gradient step
+)
+
+# Solve
+velocity, scales = fn(target, jacobian.flatten(), C, lower, upper)
+```
+
+**Export for CusADi:**
+```bash
+# Export FI-PeSNS for CusADi compilation
+pixi run -e cuda python -m embodik.gpu.export_casadi_velocity_solve \
+    --robot panda --k_max 10 \
+    --out fn_velocity_solve.casadi
+
+# Compile to CUDA kernel
+mv fn_velocity_solve.casadi ~/.local/cusadi/src/casadi_functions/
+cd ~/.local/cusadi && python run_codegen.py --fn=fn_velocity_solve
+```
+
+### GPU Collision Detection (Experimental)
+
+EmbodiK also supports GPU-accelerated collision detection via NVIDIA Warp:
+
+```python
+from embodik.gpu.warp_collision import compute_collision_distances_batched
+
+# Batch collision queries
+result = compute_collision_distances_batched(
+    robot_model,
+    q_batch,  # (batch_size, n_dof) configurations
+    use_gpu=True
+)
+distances = result.distances  # (batch_size,) minimum distances
+```
+
+See [docs/installation.md](docs/installation.md) for detailed GPU setup instructions.
+
+## Testing
+
+```bash
+# Run all tests
+pixi run test
+
+# Run tests with verbose output
+pixi run test-verbose
+
+# Run tests with coverage
+pixi run test-cov
+```
+
+## Architecture
+
+```
+embodik/
+├── cpp_core/              # C++ core implementation
+│   ├── include/embodik/  # Header files
+│   └── src/              # Implementation files
+├── python_bindings/       # Nanobind C++ bindings
+│   └── src/              # Binding code
+├── python/embodik/        # Python package
+│   ├── utils.py          # Utility functions
+│   └── visualization.py  # Visualization support
+├── examples/              # Example scripts
+│   ├── 01_basic_ik_simple.py
+│   ├── 02_collision_aware_IK.py
+│   └── robot_models/     # Robot URDF files
+├── docs/                  # Documentation (MkDocs)
+└── test/                  # Test suite
+```
+
+## Documentation
+
+Full documentation is available at: **https://embodik.github.io/embodik/**
+
+- [Installation Guide](docs/installation.md) - Detailed installation instructions
+- [Quickstart](docs/quickstart.md) - Get started in 5 minutes
+- [API Reference](docs/api/index.md) - Complete API documentation
+- [Examples](docs/examples/index.md) - Example code and tutorials
+- [Development Guide](docs/development.md) - Contributing and development
+
+## Contributing
+
+We welcome contributions! Please see [CONTRIBUTING.md](CONTRIBUTING.md) for guidelines.
+
+Key principles:
+1. Follow the existing code style
+2. Add tests for new functionality
+3. Ensure numerical accuracy and stability
+4. Update documentation for API changes
+
+## License
+
+This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details.
+
+**Copyright (c) 2025 Andy Park <andypark.purdue@gmail.com>**
+
+The MIT License is a permissive license that allows for:
+- Commercial use
+- Modification
+- Distribution
+- Private use
+
+While providing liability protection for the authors. This makes it ideal for open-source projects that want to encourage widespread adoption and contribution.
+