embodik 0.3.0__tar.gz → 0.4.1__tar.gz

{embodik-0.3.0 → embodik-0.4.1}/CHANGELOG.md

@@ -5,9 +5,77 @@ 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.4.0] - 2026-02-05
+
+### Breaking Changes
+- **Removed Python `pin` package from runtime dependencies**: EmbodiK now uses native C++ bindings
+  exclusively for all Pinocchio functionality. The `pip pinocchio` (`pin`) package is no longer
+  required at runtime, only at build time.
+  - This resolves numpy dependency conflicts when using EmbodiK with packages like `hmnd_robot`
+    that have different numpy version requirements
+  - All rotation utilities (log3, exp3, quaternion conversions) now use native bindings
+  - Collision distance computation now uses native `RobotModel.compute_min_collision_distance()`
+  - Visualization defaults to direct Viser (no Pinocchio ViserVisualizer dependency)
+
+### Added
+- **Native math utilities via nanobind**: New C++ bindings for rotation/pose math
+  - `embodik.log3(R)`: Compute axis-angle from rotation matrix (replaces `pin.log3`)
+  - `embodik.exp3(omega)`: Compute rotation matrix from axis-angle (replaces `pin.exp3`)
+  - `embodik.matrix_to_quaternion_wxyz(R)`: Convert rotation matrix to (w,x,y,z) quaternion
+  - `embodik.quaternion_wxyz_to_matrix(w,x,y,z)`: Convert quaternion to rotation matrix
+- **Native collision distance API**: RobotModel now exposes collision distance methods
+  - `RobotModel.compute_min_collision_distance()`: Get minimum distance across all collision pairs
+  - `RobotModel.compute_collision_distances()`: Get distances for all collision pairs
+- **Optional Pinocchio visualization**: `pip install embodik[visualization-pinocchio]` for
+  Pinocchio's ViserVisualizer (requires `pin>=3.8.0`)
+
+### Changed
+- Visualization now defaults to direct Viser implementation (no `pin` needed)
+- `utils.py` functions (`get_pose_error_vector`, `compute_pose_error`, `Rt`) use native bindings
+- `visualization.py` quaternion functions use native bindings
+- GPU collision fallback uses native `RobotModel.compute_min_collision_distance()`
+- Removed `_runtime_deps.py` (lazy Pinocchio import no longer needed)
+
+### Migration Guide
+If you were using Pinocchio Python bindings directly through EmbodiK:
+
+```python
+# Old (v0.3.x) - required pip pinocchio
+import pinocchio as pin
+R_error = pose_goal.rotation @ pose_current.rotation.T
+error = pin.log3(R_error)
+q = pin.Quaternion(R)
+
+# New (v0.4.0) - no pip pinocchio needed
+import embodik as eik
+R_error = pose_goal.rotation @ pose_current.rotation.T
+error = eik.log3(R_error)
+w, x, y, z = eik.matrix_to_quaternion_wxyz(R)
+```
+
+For collision distance computation:
+
+```python
+# Old (v0.3.x) - required pip pinocchio
+import pinocchio as pin
+pin.updateGeometryPlacements(model, data, collision_model, collision_data)
+for i in range(len(collision_model.collisionPairs)):
+    pin.computeDistance(collision_model, collision_data, i)
+    dist = collision_data.distanceResults[i].min_distance
+
+# New (v0.4.0) - native API
+robot_model.update_configuration(q)
+dist = robot_model.compute_min_collision_distance()
+```
+
 ## [0.3.0] - 2026-02-03
 
 ### Added
+- **PPH-SNS Solver**: Alternative GPU-optimized solver (Parallel Penalized Hierarchical SNS)
+  - Soft top-k violation selection using softmax weights
+  - Limited rank-1 projector updates (1–2 violators per iteration)
+  - Achieves ~632,000 IK solves/second at batch size 10,000
+  - Benchmark comparison scripts: `benchmark-solver-comparison`, `benchmark-solver-batched`
 - **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

{embodik-0.3.0 → embodik-0.4.1}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.2
 Name: embodik
-Version: 0.3.0
+Version: 0.4.1
 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>
@@ -23,13 +23,16 @@ 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: visualization-pinocchio
+Requires-Dist: pin>=3.8.0; extra == "visualization-pinocchio"
+Requires-Dist: viser>=0.1.0; extra == "visualization-pinocchio"
+Requires-Dist: trimesh>=3.0.0; extra == "visualization-pinocchio"
 Provides-Extra: examples
 Requires-Dist: pyyaml>=6.0; extra == "examples"
 Requires-Dist: robot_descriptions>=1.0.0; extra == "examples"
@@ -67,6 +70,7 @@ EmbodiK is a high-performance inverse kinematics (IK) library for cross-embodime
 - **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
+- **Limit Recovery**: Configurable joint limit recovery gain when outside bounds
 - **Collision Avoidance**: Self-collision detection and avoidance
 - **Visualization**: Interactive 3D visualization with Viser
 - **Robot Models**: Built-in support for common robots (Panda, IIWA)
@@ -74,6 +78,10 @@ EmbodiK is a high-performance inverse kinematics (IK) library for cross-embodime
 
 ## Installation
 
+> **Note (v0.4.0+)**: EmbodiK no longer requires the Python `pin` package at runtime.
+> All Pinocchio functionality is exposed through native C++ bindings. This resolves
+> numpy dependency conflicts when using EmbodiK alongside packages like `hmnd_robot`.
+
 ### Option A: Fresh Environment (No existing Pinocchio)
 
 If you don't have Pinocchio/Boost installed locally, installation is straightforward:
@@ -83,14 +91,14 @@ python3 -m venv .venv
 source .venv/bin/activate
 pip install -U pip
 
-# Install build dependencies and Pinocchio
+# Install build dependencies (pin is needed for build only, not runtime)
 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
+# Verify (no pin import needed!)
 python -c "import embodik; print(embodik.__version__, embodik.RobotModel)"
 ```
 
@@ -106,7 +114,7 @@ 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
+# Install build dependencies (pin is needed for build only, not runtime)
 pip install pin scikit-build-core nanobind cmake ninja
 
 # Set CMAKE_PREFIX_PATH to the PyPI pin package
@@ -115,7 +123,7 @@ export CMAKE_PREFIX_PATH=$(python -c "import pinocchio, pathlib; print(pathlib.P
 # Install embodik
 pip install --no-build-isolation embodik
 
-# Verify
+# Verify (no pin import needed!)
 python -c "import embodik; print(embodik.__version__, embodik.RobotModel)"
 ```
 
@@ -173,6 +181,37 @@ if result.status == embodik.SolverStatus.SUCCESS:
 
 ## API Overview
 
+### Native Math Utilities
+
+EmbodiK provides native bindings for rotation and pose math (no Python `pin` package needed):
+
+```python
+import embodik as eik
+import numpy as np
+
+# Rotation matrix to axis-angle (replaces pin.log3)
+R = np.eye(3)
+omega = eik.log3(R)  # Returns [0, 0, 0]
+
+# Axis-angle to rotation matrix (replaces pin.exp3)
+omega = np.array([0, 0, np.pi/4])
+R = eik.exp3(omega)
+
+# Rotation matrix to quaternion (wxyz format)
+w, x, y, z = eik.matrix_to_quaternion_wxyz(R)
+
+# Quaternion to rotation matrix
+R = eik.quaternion_wxyz_to_matrix(w, x, y, z)
+
+# Create SE3 transform
+T = eik.Rt(R=R, t=np.array([1, 0, 0]))
+
+# Collision distance (no pin needed)
+robot = eik.RobotModel("robot.urdf")
+robot.update_configuration(q)
+min_distance = robot.compute_min_collision_distance()
+```
+
 ### High-Level API (Recommended)
 
 EmbodiK provides a high-level API built on top of Pinocchio for easy robot modeling and IK solving:
@@ -247,6 +286,8 @@ The repository includes several example scripts:
 | `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 |
+| `scripts/benchmark_pph_sns_comparison.py` | FI-PeSNS vs PPH-SNS solver comparison (CPU + GPU) |
+| `scripts/benchmark_pph_sns_batched.py` | Batched GPU benchmark for both solvers |
 
 ### Running Examples
 
@@ -286,6 +327,8 @@ See the [Examples Documentation](docs/examples/index.md) for detailed guides.
 
 ## GPU Acceleration
 
+> **Note:** GPU solvers (FI-PeSNS, PPH-SNS) are **experimental** and require further validation. Use with caution in production systems.
+
 EmbodiK supports GPU-accelerated batched velocity IK solving for massive parallelism, ideal for:
 
 - **RL Training**: 4096+ parallel environments in Isaac Gym/Orbit
@@ -370,6 +413,9 @@ velocities = result.velocities  # (batch_size, n_dof)
 | `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 export-pph-sns` | Export PPH-SNS velocity solve function |
+| `pixi run -e cuda benchmark-solver-comparison` | Compare FI-PeSNS vs PPH-SNS (CPU + GPU) |
+| `pixi run -e cuda benchmark-solver-batched` | Batched GPU benchmark for both solvers |
 | `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 |
@@ -379,9 +425,33 @@ velocities = result.velocities  # (batch_size, n_dof)
 | `pixi run -e cuda demo-parallel-tracking` | 100 robots tracking trajectories in parallel |
 | `pixi run -e cuda test-gpu` | Run GPU-specific tests |
 
+### GPU Solvers: FI-PeSNS and PPH-SNS
+
+EmbodiK provides two GPU-optimized velocity IK solvers, both suitable for CusADi compilation:
+
+| Solver | Description | Best For |
+|--------|-------------|----------|
+| **FI-PeSNS** | Fixed-Iteration Penalized eSNS | Default choice, proven accuracy |
+| **PPH-SNS** | Parallel Penalized Hierarchical SNS | Alternative with soft top-k violation selection |
+
+Both achieve **100% constraint satisfaction** with zero violations. FI-PeSNS is typically ~7% faster at large batch sizes; PPH-SNS offers a different formulation with limited rank-1 projector updates.
+
+**Benchmark (10,000 instances, 7-DOF Panda):**
+
+| Solver | Time | Throughput |
+|--------|------|------------|
+| FI-PeSNS | 14.8 ms | **675,000 solves/sec** |
+| PPH-SNS | 15.8 ms | **632,000 solves/sec** |
+
+```bash
+# Compare both solvers
+pixi run -e cuda benchmark-solver-comparison
+pixi run -e cuda benchmark-solver-batched
+```
+
 ### 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:
+**FI-PeSNS** is the primary GPU solver—a variant of eSNS that trades exact constraint saturation for simpler, parallelizable penalty-based enforcement:
 
 **Key Features:**
 - **SRINV**: Singularity-Robust Inverse for numerical stability
@@ -418,6 +488,33 @@ for i in range(k_max):
 
 *GPU benchmarks on NVIDIA RTX A2000 8GB with CusADi-compiled CUDA kernels.*
 
+### PPH-SNS: Parallel Penalized Hierarchical SNS
+
+**PPH-SNS** is an alternative GPU-native design with:
+
+- **Soft top-k violation selection** using softmax weights
+- **Limited rank-1 projector updates** (1–2 violators per iteration)
+- **Aggressive penalty ramping** (γ=3.0)
+- **Fixed-depth unrolling** for CusADi compilation
+
+```bash
+# Export PPH-SNS (writes to ~/.local/cusadi/src/casadi_functions/)
+pixi run -e cuda export-pph-sns
+
+# Compile to CUDA kernel
+cd ~/.local/cusadi && python run_codegen.py --fn=fn_pph_sns_velocity_solve
+```
+
+```python
+from embodik.gpu.casadi_pph_sns import build_pph_sns_single_task
+
+fn = build_pph_sns_single_task(
+    n_dof=7, task_dim=6, n_constraints=7,
+    k_max=14, m_max=2,  # Outer iterations, max saturations per iteration
+)
+velocity, scales = fn(target, jacobian.flatten(), C, lower, upper)
+```
+
 ### Parallel Trajectory Tracking Demo
 
 Visualize GPU parallelization with 100 robot instances simultaneously tracking different trajectories:
@@ -518,6 +615,7 @@ 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
+- [GPU Solvers](docs/gpu_solvers.md) - FI-PeSNS and PPH-SNS GPU-accelerated solvers
 - [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

{embodik-0.3.0 → embodik-0.4.1}/README.md

@@ -18,6 +18,7 @@ EmbodiK is a high-performance inverse kinematics (IK) library for cross-embodime
 - **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
+- **Limit Recovery**: Configurable joint limit recovery gain when outside bounds
 - **Collision Avoidance**: Self-collision detection and avoidance
 - **Visualization**: Interactive 3D visualization with Viser
 - **Robot Models**: Built-in support for common robots (Panda, IIWA)
@@ -25,6 +26,10 @@ EmbodiK is a high-performance inverse kinematics (IK) library for cross-embodime
 
 ## Installation
 
+> **Note (v0.4.0+)**: EmbodiK no longer requires the Python `pin` package at runtime.
+> All Pinocchio functionality is exposed through native C++ bindings. This resolves
+> numpy dependency conflicts when using EmbodiK alongside packages like `hmnd_robot`.
+
 ### Option A: Fresh Environment (No existing Pinocchio)
 
 If you don't have Pinocchio/Boost installed locally, installation is straightforward:
@@ -34,14 +39,14 @@ python3 -m venv .venv
 source .venv/bin/activate
 pip install -U pip
 
-# Install build dependencies and Pinocchio
+# Install build dependencies (pin is needed for build only, not runtime)
 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
+# Verify (no pin import needed!)
 python -c "import embodik; print(embodik.__version__, embodik.RobotModel)"
 ```
 
@@ -57,7 +62,7 @@ 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
+# Install build dependencies (pin is needed for build only, not runtime)
 pip install pin scikit-build-core nanobind cmake ninja
 
 # Set CMAKE_PREFIX_PATH to the PyPI pin package
@@ -66,7 +71,7 @@ export CMAKE_PREFIX_PATH=$(python -c "import pinocchio, pathlib; print(pathlib.P
 # Install embodik
 pip install --no-build-isolation embodik
 
-# Verify
+# Verify (no pin import needed!)
 python -c "import embodik; print(embodik.__version__, embodik.RobotModel)"
 ```
 
@@ -124,6 +129,37 @@ if result.status == embodik.SolverStatus.SUCCESS:
 
 ## API Overview
 
+### Native Math Utilities
+
+EmbodiK provides native bindings for rotation and pose math (no Python `pin` package needed):
+
+```python
+import embodik as eik
+import numpy as np
+
+# Rotation matrix to axis-angle (replaces pin.log3)
+R = np.eye(3)
+omega = eik.log3(R)  # Returns [0, 0, 0]
+
+# Axis-angle to rotation matrix (replaces pin.exp3)
+omega = np.array([0, 0, np.pi/4])
+R = eik.exp3(omega)
+
+# Rotation matrix to quaternion (wxyz format)
+w, x, y, z = eik.matrix_to_quaternion_wxyz(R)
+
+# Quaternion to rotation matrix
+R = eik.quaternion_wxyz_to_matrix(w, x, y, z)
+
+# Create SE3 transform
+T = eik.Rt(R=R, t=np.array([1, 0, 0]))
+
+# Collision distance (no pin needed)
+robot = eik.RobotModel("robot.urdf")
+robot.update_configuration(q)
+min_distance = robot.compute_min_collision_distance()
+```
+
 ### High-Level API (Recommended)
 
 EmbodiK provides a high-level API built on top of Pinocchio for easy robot modeling and IK solving:
@@ -198,6 +234,8 @@ The repository includes several example scripts:
 | `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 |
+| `scripts/benchmark_pph_sns_comparison.py` | FI-PeSNS vs PPH-SNS solver comparison (CPU + GPU) |
+| `scripts/benchmark_pph_sns_batched.py` | Batched GPU benchmark for both solvers |
 
 ### Running Examples
 
@@ -237,6 +275,8 @@ See the [Examples Documentation](docs/examples/index.md) for detailed guides.
 
 ## GPU Acceleration
 
+> **Note:** GPU solvers (FI-PeSNS, PPH-SNS) are **experimental** and require further validation. Use with caution in production systems.
+
 EmbodiK supports GPU-accelerated batched velocity IK solving for massive parallelism, ideal for:
 
 - **RL Training**: 4096+ parallel environments in Isaac Gym/Orbit
@@ -321,6 +361,9 @@ velocities = result.velocities  # (batch_size, n_dof)
 | `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 export-pph-sns` | Export PPH-SNS velocity solve function |
+| `pixi run -e cuda benchmark-solver-comparison` | Compare FI-PeSNS vs PPH-SNS (CPU + GPU) |
+| `pixi run -e cuda benchmark-solver-batched` | Batched GPU benchmark for both solvers |
 | `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 |
@@ -330,9 +373,33 @@ velocities = result.velocities  # (batch_size, n_dof)
 | `pixi run -e cuda demo-parallel-tracking` | 100 robots tracking trajectories in parallel |
 | `pixi run -e cuda test-gpu` | Run GPU-specific tests |
 
+### GPU Solvers: FI-PeSNS and PPH-SNS
+
+EmbodiK provides two GPU-optimized velocity IK solvers, both suitable for CusADi compilation:
+
+| Solver | Description | Best For |
+|--------|-------------|----------|
+| **FI-PeSNS** | Fixed-Iteration Penalized eSNS | Default choice, proven accuracy |
+| **PPH-SNS** | Parallel Penalized Hierarchical SNS | Alternative with soft top-k violation selection |
+
+Both achieve **100% constraint satisfaction** with zero violations. FI-PeSNS is typically ~7% faster at large batch sizes; PPH-SNS offers a different formulation with limited rank-1 projector updates.
+
+**Benchmark (10,000 instances, 7-DOF Panda):**
+
+| Solver | Time | Throughput |
+|--------|------|------------|
+| FI-PeSNS | 14.8 ms | **675,000 solves/sec** |
+| PPH-SNS | 15.8 ms | **632,000 solves/sec** |
+
+```bash
+# Compare both solvers
+pixi run -e cuda benchmark-solver-comparison
+pixi run -e cuda benchmark-solver-batched
+```
+
 ### 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:
+**FI-PeSNS** is the primary GPU solver—a variant of eSNS that trades exact constraint saturation for simpler, parallelizable penalty-based enforcement:
 
 **Key Features:**
 - **SRINV**: Singularity-Robust Inverse for numerical stability
@@ -369,6 +436,33 @@ for i in range(k_max):
 
 *GPU benchmarks on NVIDIA RTX A2000 8GB with CusADi-compiled CUDA kernels.*
 
+### PPH-SNS: Parallel Penalized Hierarchical SNS
+
+**PPH-SNS** is an alternative GPU-native design with:
+
+- **Soft top-k violation selection** using softmax weights
+- **Limited rank-1 projector updates** (1–2 violators per iteration)
+- **Aggressive penalty ramping** (γ=3.0)
+- **Fixed-depth unrolling** for CusADi compilation
+
+```bash
+# Export PPH-SNS (writes to ~/.local/cusadi/src/casadi_functions/)
+pixi run -e cuda export-pph-sns
+
+# Compile to CUDA kernel
+cd ~/.local/cusadi && python run_codegen.py --fn=fn_pph_sns_velocity_solve
+```
+
+```python
+from embodik.gpu.casadi_pph_sns import build_pph_sns_single_task
+
+fn = build_pph_sns_single_task(
+    n_dof=7, task_dim=6, n_constraints=7,
+    k_max=14, m_max=2,  # Outer iterations, max saturations per iteration
+)
+velocity, scales = fn(target, jacobian.flatten(), C, lower, upper)
+```
+
 ### Parallel Trajectory Tracking Demo
 
 Visualize GPU parallelization with 100 robot instances simultaneously tracking different trajectories:
@@ -469,6 +563,7 @@ 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
+- [GPU Solvers](docs/gpu_solvers.md) - FI-PeSNS and PPH-SNS GPU-accelerated solvers
 - [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

{embodik-0.3.0 → embodik-0.4.1}/cpp_core/include/embodik/kinematics_solver.hpp

@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include <algorithm>
 #include <cstdint>
 #include <embodik/robot_model.hpp>
 #include <embodik/tasks.hpp>
@@ -189,6 +190,15 @@ public:
    */
   void set_damping(double damping) { damping_ = damping; }
 
+  /**
+   * @brief Set recovery gain for joint limit violations.
+   *
+   * Values are clamped to [0, 1]. Higher values recover faster.
+   */
+  void set_limit_recovery_gain(double gain) {
+    limit_recovery_gain_ = std::clamp(gain, 0.0, 1.0);
+  }
+
   /**
    * @brief Enable verbose debugging for position IK iterations.
    * @param enable True to print/log per-iteration errors and store traces.
@@ -280,6 +290,24 @@ public:
   std::vector<std::pair<std::string, std::string>>
   get_active_collision_pairs() const;
 
+  /**
+   * @brief Calculate velocity box constraints based on position, velocity, and
+   * acceleration limits.
+   *
+   * Velocity limits are computed as:
+   * min(position_margin/dt, velocity_limit, sqrt(2*accel*margin))
+   *
+   * @param position_margin_lower Distance from current position to lower limit
+   * @param position_margin_upper Distance from current position to upper limit
+   * @param velocity_limit Maximum allowed velocity
+   * @param acceleration_limit Maximum allowed acceleration
+   * @param dt Time step
+   * @return Pair of (lower_velocity_limit, upper_velocity_limit)
+   */
+  std::pair<double, double> calculate_velocity_box_constraint(
+      double position_margin_lower, double position_margin_upper,
+      double velocity_limit, double acceleration_limit, double dt) const;
+
 private:
   std::shared_ptr<RobotModel> robot_;
   std::vector<std::shared_ptr<Task>> tasks_;
@@ -294,6 +322,7 @@ private:
   double norm_threshold_ = 1e10;
   int max_zero_scale_iterations_ = 2;
   bool position_ik_debug_ = false;
+  double limit_recovery_gain_ = 0.5;
 
   // Constraint options
   bool use_velocity_limits_ = true;
@@ -349,24 +378,6 @@ private:
   bool collision_pair_allowed(const std::string &a, const std::string &b) const;
   std::optional<CollisionConstraintResult> compute_collision_constraint();
 
-  /**
-   * @brief Calculate velocity box constraints based on position, velocity, and
-   * acceleration limits
-   *
-   * Velocity limits are computed as:
-   * min(position_margin/dt, velocity_limit, sqrt(2*accel*margin))
-   *
-   * @param position_margin_lower Distance from current position to lower limit
-   * @param position_margin_upper Distance from current position to upper limit
-   * @param velocity_limit Maximum allowed velocity
-   * @param acceleration_limit Maximum allowed acceleration
-   * @param dt Time step
-   * @return Pair of (lower_velocity_limit, upper_velocity_limit)
-   */
-  std::pair<double, double> calculate_velocity_box_constraint(
-      double position_margin_lower, double position_margin_upper,
-      double velocity_limit, double acceleration_limit, double dt) const;
-
 public:
   // ========== Position IK Methods ==========
 

{embodik-0.3.0 → embodik-0.4.1}/cpp_core/include/embodik/robot_model.hpp

@@ -259,6 +259,26 @@ public:
     return collision_model_ != nullptr && collision_data_ != nullptr;
   }
 
+  /**
+   * @brief Compute minimum collision distance at current configuration.
+   *
+   * Updates geometry placements and computes distances for all collision pairs,
+   * returning the minimum distance found.
+   *
+   * @return Minimum collision distance, or infinity if no collision geometry
+   */
+  double compute_min_collision_distance() const;
+
+  /**
+   * @brief Compute collision distances for all pairs at current configuration.
+   *
+   * Updates geometry placements and computes distances for all collision pairs.
+   *
+   * @return Vector of distances for each collision pair, in order of
+   * collision_model.collisionPairs
+   */
+  std::vector<double> compute_collision_distances() const;
+
   // Access to URDF path for visualization
   const std::string &urdf_path() const { return urdf_path_; }
 

{embodik-0.3.0 → embodik-0.4.1}/cpp_core/src/kinematics_solver.cpp

@@ -691,8 +691,17 @@ KinematicsSolver::compute_collision_constraint() {
 std::pair<double, double> KinematicsSolver::calculate_velocity_box_constraint(
     double position_margin_lower, double position_margin_upper,
     double velocity_limit, double acceleration_limit, double dt) const {
+  constexpr double kMarginEpsilon = 1e-4;
+  const double raw_margin_lower = position_margin_lower;
+  const double raw_margin_upper = position_margin_upper;
+  const bool outside_lower = raw_margin_lower < -kMarginEpsilon;
+  const bool outside_upper = raw_margin_upper < -kMarginEpsilon;
+
+  if (outside_lower && outside_upper) {
+    return std::make_pair(-velocity_limit, velocity_limit);
+  }
 
-  // Clamp margins to be non-negative
+  // Clamp margins to be non-negative for nominal bounds.
   position_margin_lower = std::max(0.0, position_margin_lower);
   position_margin_upper = std::max(0.0, position_margin_upper);
 
@@ -712,6 +721,28 @@ std::pair<double, double> KinematicsSolver::calculate_velocity_box_constraint(
   double upper_limit =
       std::min({vel_from_pos_upper, velocity_limit, vel_from_accel_upper});
 
+  if (outside_lower) {
+    const double violation = -raw_margin_lower;
+    const double recovery_min = (limit_recovery_gain_ * violation) / dt;
+    const double recovery_lower = std::min(recovery_min, velocity_limit);
+    if (recovery_lower > lower_limit) {
+      lower_limit = recovery_lower;
+    }
+  } else if (outside_upper) {
+    const double violation = -raw_margin_upper;
+    const double recovery_max = -(limit_recovery_gain_ * violation) / dt;
+    const double recovery_upper = std::max(recovery_max, -velocity_limit);
+    if (recovery_upper < upper_limit) {
+      upper_limit = recovery_upper;
+    }
+  }
+
+  if (lower_limit > upper_limit) {
+    const double midpoint = 0.5 * (lower_limit + upper_limit);
+    lower_limit = midpoint;
+    upper_limit = midpoint;
+  }
+
   return std::make_pair(lower_limit, upper_limit);
 }