Zeerekahmad/projection fix and stack

.cpplint.cfg

@@ -11,6 +11,9 @@ filter=-build/c++17
 # Per our style guide, we want to allow the use of non-const reference passing for output parameters
 filter=-runtime/references
 
+# Catch2 idiom is `CHECK(a == b)` with expression decomposition; CHECK_EQ is a gtest macro and not applicable here
+filter=-readability/check
+
 # Disable all formatting checks, this is handled by clang-format
 filter=-readability/braces
 filter=-whitespace/braces

.github/workflows/build.yml

@@ -27,11 +27,54 @@ jobs:
       ROS_DISTRO: ${{ matrix.ros }}
       PIP_BREAK_SYSTEM_PACKAGES: 1
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v5
       - uses: ros-tooling/action-ros-ci@v0.4
         with:
           target-ros2-distro: ${{ matrix.ros }}
-      - uses: actions/upload-artifact@v4
+      - uses: actions/upload-artifact@v6
         with:
           name: colcon-logs-${{ matrix.ros }}
           path: ros_ws/log
+
+  # The pytest suites only run under colcon above, so cover the wheel path the explorer uses.
+  wheel_and_explorer:
+    runs-on: ubuntu-latest
+    name: Wheel and explorer
+    steps:
+      - uses: actions/checkout@v5
+      - uses: astral-sh/setup-uv@v7
+      - name: Install package with dev extra
+        run: uv venv && uv pip install -e ".[dev]"
+      - name: Python tests
+        run: uv run pytest test/test_python_bindings.py test/test_explorer.py -v
+      - name: Explorer imports cleanly
+        run: uv run python -c "import polymath_kinematics.explorer"
+      - name: Console script resolves and launches
+        # A healthy headless start proves the entry point doesn't run the app body on import.
+        run: |
+          uv run kinematic-explorer --server.headless=true --server.port=8501 &
+          for _ in $(seq 1 60); do
+            if curl -sf http://localhost:8501/_stcore/health; then exit 0; fi
+            sleep 1
+          done
+          echo "explorer failed to become healthy" >&2
+          exit 1
+
+  standalone_cmake:
+    runs-on: ubuntu-latest
+    name: Standalone CMake + ctest
+    steps:
+      - uses: actions/checkout@v5
+      - name: Install build dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y cmake g++ catch2 pybind11-dev python3-dev
+      - name: Configure
+        # CMAKE_DISABLE_FIND_PACKAGE_ament_cmake forces the non-ament branch.
+        run: >
+          cmake -S . -B build -DBUILD_TESTING=ON
+          -DCMAKE_DISABLE_FIND_PACKAGE_ament_cmake=ON
+      - name: Build
+        run: cmake --build build -j
+      - name: Test
+        run: ctest --test-dir build --output-on-failure

.github/workflows/pre-commit.yml

@@ -10,8 +10,8 @@ jobs:
   pre-commit:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
-      - uses: actions/setup-python@v5
+      - uses: actions/checkout@v5
+      - uses: actions/setup-python@v6
         with:
           python-version: "3.10"
       - run: sudo apt-get update && sudo apt-get install libxml2-utils

.gitignore

@@ -1,4 +1,15 @@
-# Colcon
+# Colcon (when built from the workspace root, build/ lives there)
 /build/
 /install/
 /log/
+
+# Python / uv
+.venv/
+__pycache__/
+*.py[cod]
+*.egg-info/
+.pytest_cache/
+.ruff_cache/
+
+# Build artifacts from CMake direct invocation
+*.so

CHANGELOG.md

@@ -1,5 +1,73 @@
 # Changelog
 
+## v0.2.0
+
+### Forward projection
+
+- `BicycleProjector` — ramps the steering angle toward a target at a bounded rate, clamped to
+  `[min, max]`, and integrates pose with Euler. Pose reference is the rear axle.
+- `ArticulatedProjector` — same shape for the articulation angle γ and rate γ̇. Pose reference
+  (`base_link`) is the articulation joint; motion is integrated at the rear axle and converted
+  back to the joint each step.
+- `DifferentialDriveProjector` — ramps the body command (v, ω) toward a target under separate
+  linear and angular acceleration limits. Pose reference is the body centre.
+- All three expose `step()` for one time step and `project()` for a full horizon, returning a
+  timestamped state sequence whose element 0 is the initial state.
+
+### Footprints
+
+- `Pose2D`, `Point2D`, and `Footprint` (a counter-clockwise, unclosed polygon of any vertex count)
+  in a shared `pose2d.hpp`, along with `normalizeAngle`, `transformFootprint`,
+  `offsetAlongHeading`, and `rectangleFootprint` for the boxy common case.
+- Each projector optionally takes an **arbitrary body-frame polygon** and emits it transformed into
+  the world frame per sample; the kinematic models stay dimension-free. An empty polygon means
+  "unset" and yields an empty footprint rather than throwing. The articulated projector takes one
+  polygon per body.
+
+### Axle reference
+
+- `AxleReference::FRONT` / `REAR` selects the axle that `BicycleProjector` and
+  `ArticulatedProjector` measure poses and footprints from. Motion is still integrated where each
+  model is naturally defined (the rear axle); the reference conversion is applied on input and
+  output, so no integration accuracy is traded for the choice.
+- For the articulated model, `theta` follows the referenced axle's body — $\theta_r$ for REAR,
+  $\theta_f = \theta_r + \gamma$ for FRONT — and each body's polygon is anchored at **its own**
+  axle, the only anchoring that stays rigid as the joint articulates.
+- `ArticulatedProjectedState` gained `joint_pose`, so the articulation joint (`base_link`) is
+  reported on every sample whichever axle is referenced.
+- A differential drive has one axle, so `DifferentialDriveProjector` takes no reference; its
+  polygon is measured from the body centre.
+
+### Kinematics
+
+- `ArticulatedModel` now accepts an explicit articulation turning velocity (γ̇) on
+  `bodyVelocityToVehicleState` and `articulationToAxleVelocities`. The previous two-argument
+  forms remain as overloads that assume steady articulation (γ̇ = 0), so existing callers are
+  unaffected. γ̇ was previously a hardcoded zero constant.
+
+### Packaging
+
+- Wheel build via scikit-build-core, alongside the existing ROS2/ament path. One
+  `CMakeLists.txt` serves both; `find_package(ament_cmake QUIET)` selects the branch. The wheel
+  links the C++ library statically so it ships a single `.so`.
+- Python bindings for all three projectors, their projected-state structs, `Pose2D`, and
+  `Point2D`.
+
+### Kinematic Explorer
+
+- New Streamlit app (`kinematic-explorer`) for visualising trajectory lattices, kinematic
+  relationships, and vehicle footprints across all three models. Every trajectory it draws
+  comes from the C++ projectors, so the explorer and production code share one
+  forward-simulation path.
+- Geometry sliders (including body overhangs), a front/rear axle reference selector,
+  single-projected-trajectory ramp controls, and CSV / JSON / PNG / SVG / PDF download.
+
+### Removed
+
+- `Twist2D`, `normalize_angle`, and `transform_pose` from the Python package — leftovers from
+  when `Pose2D` was a Python shim, now superseded by the bound C++ `Pose2D` and
+  `normalizeAngle`.
+
 ## v0.1.0
 
 Initial release.

CMakeLists.txt

@@ -12,15 +12,14 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.15)
 project(polymath_kinematics)
 
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic -Werror)
-  add_link_options(-Wl,-no-undefined)
 endif()
 
 # Ubuntu detection (override with -DBUILD_JAMMY=ON/OFF)
@@ -43,58 +42,63 @@ if(NOT DEFINED BUILD_JAMMY)
   endif()
 endif()
 
-# Find packages
-find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_python REQUIRED)
+# Find packages. ament_cmake is QUIET so the wheel build (no ROS2) falls
+# through to a plain CMake configuration.
+find_package(ament_cmake QUIET)
 find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module)
-find_package(pybind11_vendor REQUIRED)
-find_package(pybind11 REQUIRED)
+find_package(pybind11 CONFIG REQUIRED)
 
-# C++ library with all kinematic models
-add_library(polymath_kinematics SHARED
+# C++ library with all kinematic models. SHARED under ROS2 (ament exports a
+# linkable .so to downstream packages); STATIC under wheel builds so the
+# pybind11 module is self-contained and the wheel ships a single .so.
+if(ament_cmake_FOUND)
+  set(_polymath_kinematics_lib_type SHARED)
+else()
+  set(_polymath_kinematics_lib_type STATIC)
+endif()
+add_library(polymath_kinematics ${_polymath_kinematics_lib_type}
   src/articulated_model.cpp
-  src/differential_drive_model.cpp
+  src/articulated_projector.cpp
   src/bicycle_model.cpp
+  src/bicycle_projector.cpp
+  src/differential_drive_model.cpp
+  src/differential_drive_projector.cpp
 )
 target_include_directories(polymath_kinematics PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
+set_target_properties(polymath_kinematics PROPERTIES POSITION_INDEPENDENT_CODE ON)
 
 # Python bindings
-set(PYBIND_TARGET polymath_kinematics_cpp)
-pybind11_add_module(${PYBIND_TARGET}
-  src/kinematics_pybind.cpp
-)
-set_target_properties(${PYBIND_TARGET} PROPERTIES LINK_OPTIONS "")
-target_link_libraries(${PYBIND_TARGET} PRIVATE polymath_kinematics)
-_ament_cmake_python_register_environment_hook()
-install(TARGETS ${PYBIND_TARGET} DESTINATION "${PYTHON_INSTALL_DIR}")
+pybind11_add_module(polymath_kinematics_cpp src/kinematics_pybind.cpp)
+target_link_libraries(polymath_kinematics_cpp PRIVATE polymath_kinematics)
 
-# Install C++ library
-install(
-  TARGETS polymath_kinematics
-  EXPORT ${PROJECT_NAME}_TARGETS
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-install(
-  EXPORT ${PROJECT_NAME}_TARGETS
-  NAMESPACE ${PROJECT_NAME}::
-  DESTINATION share/${PROJECT_NAME}/cmake
-)
-install(
-  DIRECTORY include/
-  DESTINATION include/
-)
+if(ament_cmake_FOUND)
+  find_package(ament_cmake_python REQUIRED)
 
-# Python module
-ament_python_install_package(${PROJECT_NAME})
+  install(
+    TARGETS polymath_kinematics
+    EXPORT ${PROJECT_NAME}_TARGETS
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
+  install(
+    EXPORT ${PROJECT_NAME}_TARGETS
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION share/${PROJECT_NAME}/cmake
+  )
+  install(DIRECTORY include/ DESTINATION include/)
+
+  ament_python_install_package(${PROJECT_NAME})
+  install(TARGETS polymath_kinematics_cpp DESTINATION "${PYTHON_INSTALL_DIR}")
+else()
+  # Wheel build: scikit-build-core installs the extension at the wheel root.
+  install(TARGETS polymath_kinematics_cpp DESTINATION .)
+endif()
 
-# Testing
 if(BUILD_TESTING)
-  find_package(ament_cmake_pytest REQUIRED)
   include(CTest)
 
   if(BUILD_JAMMY)
@@ -109,21 +113,35 @@ if(BUILD_TESTING)
     target_link_libraries(${TEST_NAME} PRIVATE polymath_kinematics Catch2::Catch2WithMain)
     target_include_directories(${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
     if(COMMAND catch_discover_tests)
-      catch_discover_tests(${TEST_NAME})
+      # PRE_TEST enumerates at ctest time. The default POST_BUILD runs each test binary during the
+      # build, where an older installed libpolymath_kinematics.so wins on LD_LIBRARY_PATH.
+      catch_discover_tests(${TEST_NAME} DISCOVERY_MODE PRE_TEST)
     else()
       add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME})
     endif()
   endfunction()
 
-  add_kinematics_catch2_test(test_differential_drive test/test_differential_drive.cpp)
   add_kinematics_catch2_test(test_bicycle_model test/test_bicycle_model.cpp)
   add_kinematics_catch2_test(test_articulated_model test/test_articulated_model.cpp)
-  ament_add_pytest_test(test_python_bindings test)
-endif()
+  add_kinematics_catch2_test(test_differential_drive test/test_differential_drive.cpp)
+  add_kinematics_catch2_test(test_bicycle_projector test/test_bicycle_projector.cpp)
+  add_kinematics_catch2_test(test_articulated_projector test/test_articulated_projector.cpp)
+  add_kinematics_catch2_test(test_differential_drive_projector test/test_differential_drive_projector.cpp)
 
-# Export targets
-ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET)
-ament_export_include_directories(include)
-ament_export_libraries(polymath_kinematics)
+  if(ament_cmake_FOUND)
+    find_package(ament_cmake_pytest REQUIRED)
+    ament_add_pytest_test(test_python_bindings test/test_python_bindings.py
+      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+    ament_add_pytest_test(test_explorer test/test_explorer.py
+      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+  endif()
+endif()
 
-ament_package()
+if(ament_cmake_FOUND)
+  ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET)
+  ament_export_include_directories(include)
+  ament_export_libraries(polymath_kinematics)
+  ament_package()
+endif()