From 9053a5a6e9921774ca2d6ce6486368bf1e36ce0e Mon Sep 17 00:00:00 2001 From: Zeerek Date: Fri, 13 Mar 2026 22:12:05 +0000 Subject: [PATCH 1/2] Move from internal repo to external --- .config/clang-format | 68 +++++ .config/copyright.txt | 13 + .cpplint.cfg | 20 ++ .github/workflows/build.yml | 37 +++ .github/workflows/pre-commit.yml | 18 ++ .gitignore | 4 + .pre-commit-config.yaml | 81 +++++ .ruff.toml | 18 ++ CHANGELOG.md | 10 + CMakeLists.txt | 129 ++++++++ CONTRIBUTING.md | 13 + README.md | 172 ++++++++++- derivations/articulated_model.md | 201 +++++++++++++ derivations/bicycle_model.md | 94 ++++++ derivations/differential_drive.md | 62 ++++ .../polymath_kinematics/articulated_model.hpp | 131 ++++++++ include/polymath_kinematics/bicycle_model.hpp | 98 ++++++ .../differential_drive_model.hpp | 72 +++++ package.xml | 20 ++ polymath_kinematics/__init__.py | 75 +++++ pyproject.toml | 16 + setup.cfg | 5 + src/articulated_model.cpp | 102 +++++++ src/bicycle_model.cpp | 101 +++++++ src/differential_drive_model.cpp | 50 ++++ src/kinematics_pybind.cpp | 135 +++++++++ test/catch2_compat.hpp | 25 ++ test/test_articulated_model.cpp | 154 ++++++++++ test/test_bicycle_model.cpp | 280 ++++++++++++++++++ test/test_differential_drive.cpp | 110 +++++++ test/test_python_bindings.py | 109 +++++++ 31 files changed, 2422 insertions(+), 1 deletion(-) create mode 100644 .config/clang-format create mode 100644 .config/copyright.txt create mode 100644 .cpplint.cfg create mode 100644 .github/workflows/build.yml create mode 100644 .github/workflows/pre-commit.yml create mode 100644 .gitignore create mode 100644 .pre-commit-config.yaml create mode 100644 .ruff.toml create mode 100644 CHANGELOG.md create mode 100644 CMakeLists.txt create mode 100644 CONTRIBUTING.md create mode 100644 derivations/articulated_model.md create mode 100644 derivations/bicycle_model.md create mode 100644 derivations/differential_drive.md create mode 100644 include/polymath_kinematics/articulated_model.hpp create mode 100644 include/polymath_kinematics/bicycle_model.hpp create mode 100644 include/polymath_kinematics/differential_drive_model.hpp create mode 100644 package.xml create mode 100644 polymath_kinematics/__init__.py create mode 100644 pyproject.toml create mode 100644 setup.cfg create mode 100644 src/articulated_model.cpp create mode 100644 src/bicycle_model.cpp create mode 100644 src/differential_drive_model.cpp create mode 100644 src/kinematics_pybind.cpp create mode 100644 test/catch2_compat.hpp create mode 100644 test/test_articulated_model.cpp create mode 100644 test/test_bicycle_model.cpp create mode 100644 test/test_differential_drive.cpp create mode 100644 test/test_python_bindings.py diff --git a/.config/clang-format b/.config/clang-format new file mode 100644 index 0000000..587bc1b --- /dev/null +++ b/.config/clang-format @@ -0,0 +1,68 @@ +--- + +# See https://releases.llvm.org/14.0.0/tools/clang/docs/ClangFormatStyleOptions.html for documentation of these options +BasedOnStyle: Google +IndentWidth: 2 +ColumnLimit: 120 + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveAssignments: None +AlignConsecutiveDeclarations: None +AlignEscapedNewlines: Left +AlignTrailingComments: false +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Empty +AllowShortFunctionsOnASingleLine: false +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: true + AfterControlStatement: MultiLine + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + AfterExternBlock: true + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false +BreakBeforeBraces: Custom +BreakConstructorInitializers: BeforeComma +CompactNamespaces: false +ContinuationIndentWidth: 2 +ConstructorInitializerIndentWidth: 0 +DerivePointerAlignment: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +FixNamespaceComments: true +IncludeBlocks: Regroup +IncludeCategories: + # Headers in <> with .h extension (best guess at C system headers) + - Regex: '<([A-Za-z0-9\Q/-_\E])+\.h>' + Priority: 1 + # Headers in <> without extension (C++ system headers) + - Regex: '<([A-Za-z0-9\Q/-_\E])+>' + Priority: 2 + # Headers in <> with other extensions. + - Regex: '<([A-Za-z0-9.\Q/-_\E])+>' + Priority: 3 + # Headers in "" + - Regex: '"([A-Za-z0-9.\Q/-_\E])+"' + Priority: 4 +IndentAccessModifiers: false +IndentPPDirectives: BeforeHash +PackConstructorInitializers: Never +PointerAlignment: Middle +ReferenceAlignment: Middle +ReflowComments: false +SeparateDefinitionBlocks: Always +SortIncludes: CaseInsensitive +SpacesBeforeTrailingComments: 2 diff --git a/.config/copyright.txt b/.config/copyright.txt new file mode 100644 index 0000000..a617e4a --- /dev/null +++ b/.config/copyright.txt @@ -0,0 +1,13 @@ +Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/.cpplint.cfg b/.cpplint.cfg new file mode 100644 index 0000000..70348dc --- /dev/null +++ b/.cpplint.cfg @@ -0,0 +1,20 @@ +# Because of cpplint's config file assumptions, this can't be contained in .config/ directory +linelength=256 + +# TODO(emerson) we need to apply a copyright check, maybe not via this tool though +filter=-legal/copyright +# TODO(emerson) we want these, but the style as enforced here is probably not quite right for us +filter=-build/header_guard +filter=-build/include_subdir +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 + +# Disable all formatting checks, this is handled by clang-format +filter=-readability/braces +filter=-whitespace/braces +filter=-whitespace/indent +filter=-whitespace/newline +filter=-whitespace/parens +filter=-whitespace/semicolon diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..0bb56a4 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,37 @@ +--- + +name: Build and test +"on": + pull_request: + push: + branches: + - main + +jobs: + build_and_test: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - ros: humble + ubuntu: jammy + - ros: jazzy + ubuntu: noble + - ros: rolling + ubuntu: noble + name: ROS 2 ${{ matrix.ros }} + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-${{ matrix.ubuntu }}:latest + env: + ROS_DISTRO: ${{ matrix.ros }} + PIP_BREAK_SYSTEM_PACKAGES: 1 + steps: + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-ci@v0.4 + with: + target-ros2-distro: ${{ matrix.ros }} + - uses: actions/upload-artifact@v4 + with: + name: colcon-logs-${{ matrix.ros }} + path: ros_ws/log diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 0000000..093bf31 --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,18 @@ +--- +name: pre-commit + +"on": + pull_request: + push: + branches: [main] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: "3.10" + - run: sudo apt-get update && sudo apt-get install libxml2-utils + - uses: pre-commit/action@v3.0.1 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ce0d030 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +# Colcon +/build/ +/install/ +/log/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..267768c --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,81 @@ +--- +# See https://pre-commit.com for more information on these settings +repos: + # Generally useful checks provided by pre-commit + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-merge-conflict + - id: check-shebang-scripts-are-executable + - id: check-symlinks + - id: check-toml + - id: check-xml + - id: end-of-file-fixer + - id: forbid-submodules + - id: mixed-line-ending + - id: trailing-whitespace + # Python + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.11.5 + hooks: + - id: ruff-format + - id: ruff + args: [--fix] + # C++ formatting + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.7 + hooks: + - id: clang-format + args: ["--style=file:.config/clang-format"] + # C++ linting + - repo: https://github.com/cpplint/cpplint + rev: 2.0.0 + hooks: + - id: cpplint + args: ["--config=.cpplint.cfg", --quiet, --output=sed] + # Markdown + - repo: https://github.com/jackdewinter/pymarkdown + rev: v0.9.28 + hooks: + - id: pymarkdown + args: [-d, MD013, fix] + # XML + - repo: https://github.com/emersonknapp/ament_xmllint + rev: v0.1 + hooks: + - id: ament_xmllint + # YAML + - repo: https://github.com/adrienverge/yamllint.git + rev: v1.29.0 + hooks: + - id: yamllint + args: [-d, "{extends: default, rules: {line-length: {max: 256}, commas: false}}"] + # CMake + - repo: https://github.com/cmake-lint/cmake-lint + rev: 1.4.3 + hooks: + - id: cmakelint + args: [--linelength=140] + - repo: https://github.com/Lucas-C/pre-commit-hooks + rev: v1.5.5 + hooks: + - id: insert-license + types_or: [python, cmake, shell] + name: Copyright headers for Python/CMake + args: [ + --license-filepath, .config/copyright.txt, + --comment-style, '#', + --allow-past-years, + --no-extra-eol, + ] + - id: insert-license + types_or: [c++, c] + name: Copyright headers for C/C++ + args: [ + --license-filepath, .config/copyright.txt, + --comment-style, '//', + --allow-past-years, + ] diff --git a/.ruff.toml b/.ruff.toml new file mode 100644 index 0000000..c39e694 --- /dev/null +++ b/.ruff.toml @@ -0,0 +1,18 @@ +line-length = 120 +indent-width = 4 + +[format] +preview = true +quote-style = "single" +indent-style = "space" +skip-magic-trailing-comma = false +line-ending = "lf" + +[lint] +select = ["E4", "E7", "E9", "F", "I"] +# Rules intended for future application +# select = ["N", "D", "C90", "PTH", "UP", "PERF", "RUF"] +ignore = [] +fixable = ["ALL"] +unfixable = [] +dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..ec20642 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,10 @@ +# Changelog + +## v0.1.0 + +Initial release. + +- `DifferentialDriveModel` — forward/inverse kinematics for two independently driven wheels +- `BicycleModel` — Ackermann steering kinematics with four-wheel ICR geometry +- `ArticulatedModel` — pivot-joint kinematics for front/rear section vehicles +- Python bindings for all three models via pybind11 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6624f4c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,129 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.8) +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) +if(NOT DEFINED BUILD_JAMMY) + set(BUILD_JAMMY OFF) + if(EXISTS "/etc/os-release") + file(READ "/etc/os-release" _OS_RELEASE) + string(REGEX MATCH "VERSION_CODENAME=([^\n\r]+)" _match "${_OS_RELEASE}") + if(CMAKE_MATCH_1) + set(_UBUNTU_CODENAME "${CMAKE_MATCH_1}") + string(TOLOWER "${_UBUNTU_CODENAME}" _UBUNTU_CODENAME) + if(_UBUNTU_CODENAME STREQUAL "jammy") + set(BUILD_JAMMY ON) + message(STATUS "Ubuntu=jammy (22.04) -> Catch2 v2") + else() + set(BUILD_JAMMY OFF) + message(STATUS "Ubuntu codename='${_UBUNTU_CODENAME}' -> defaulting to Catch2 v3") + endif() + endif() + endif() +endif() + +# Find packages +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11_vendor REQUIRED) +find_package(pybind11 REQUIRED) + +# C++ library with all kinematic models +add_library(polymath_kinematics SHARED + src/articulated_model.cpp + src/differential_drive_model.cpp + src/bicycle_model.cpp +) +target_include_directories(polymath_kinematics PUBLIC + $ + $ +) + +# 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}") + +# 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/ +) + +# Python module +ament_python_install_package(${PROJECT_NAME}) + +# Testing +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + include(CTest) + + if(BUILD_JAMMY) + find_package(Catch2 2 REQUIRED) + else() + find_package(Catch2 3 REQUIRED) + endif() + include(Catch OPTIONAL) + + function(add_kinematics_catch2_test TEST_NAME TEST_SOURCE) + add_executable(${TEST_NAME} ${TEST_SOURCE}) + 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}) + 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() + +# Export targets +ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET) +ament_export_include_directories(include) +ament_export_libraries(polymath_kinematics) + +ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..6f63de9 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,13 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ diff --git a/README.md b/README.md index 809f2f7..baeb07a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,172 @@ # polymath_kinematics -Repository for Polymath Robotics's Kinematic Libraries + +Kinematic models for differential-drive, bicycle (Ackermann), and articulated vehicles. + +## Derivations + +Math and equations behind each model: + +- [Differential drive](derivations/differential_drive.md) — forward/inverse kinematics for two independently driven wheels +- [Bicycle model (Ackermann)](derivations/bicycle_model.md) — steering-angle kinematics with four-wheel ICR geometry +- [Articulated model](derivations/articulated_model.md) — pivot-joint kinematics for front/rear section vehicles + +## C++ usage + +```cmake +find_package(polymath_kinematics REQUIRED) +target_link_libraries(my_target PRIVATE polymath_kinematics::polymath_kinematics) +``` + +```cpp +#include +#include +#include +``` + +### DifferentialDriveModel + +```cpp +polymath::kinematics::DifferentialDriveModel model(0.15, 0.5); // wheel_radius_m, track_width_m + +auto wheels = model.bodyVelocityToWheelVelocities(1.0, 0.3); // linear m/s, angular rad/s +// wheels.left_wheel_velocity_rad_s, wheels.right_wheel_velocity_rad_s + +auto body = model.wheelVelocitiesToBodyVelocity(6.0, 7.0); // left rad/s, right rad/s +// body.linear_velocity_m_s, body.angular_velocity_rad_s +``` + +### BicycleModel + +```cpp +polymath::kinematics::BicycleModel model(2.7, 1.6, 0.35); // wheelbase_m, track_width_m, wheel_radius_m + +auto state = model.bodyVelocityToSteering(2.0, 0.2); // linear m/s, angular rad/s +// state.steering_angle_rad, state.turning_radius_m +// state.front_right_wheel_rad_s, state.front_left_wheel_rad_s +// state.rear_right_wheel_rad_s, state.rear_left_wheel_rad_s + +auto body = model.steeringToBodyVelocity(2.0, 0.15); // velocity m/s, steering_angle rad +// body.linear_velocity_m_s, body.angular_velocity_rad_s + +double radius = model.turningRadius(0.15); // steering_angle rad -> meters +double angle = model.steeringAngleFromRadius(10.0); // radius m -> radians +``` + +### ArticulatedModel + +```cpp +polymath::kinematics::ArticulatedModel model( + 1.8, 1.5, // articulation_to_front_axle_m, articulation_to_rear_axle_m + 2.0, 2.0, // front_track_width_m, rear_track_width_m + 0.6, 0.6); // front_wheel_radius_m, rear_wheel_radius_m + +auto state = model.bodyVelocityToVehicleState(1.5, 0.1); // linear m/s, angular rad/s +// state.articulation_angle_rad +// state.front_right_wheel_speed_rad_s, state.front_left_wheel_speed_rad_s +// state.rear_right_wheel_speed_rad_s, state.rear_left_wheel_speed_rad_s +// state.front_axle_turning_radius_m, state.rear_axle_turning_radius_m + +auto axles = model.articulationToAxleVelocities(1.5, 0.3); // linear m/s, articulation_angle rad +// axles.front_axle_turning_velocity_rad_s, axles.rear_axle_turning_velocity_rad_s +``` + +## Python usage + +```python +from polymath_kinematics import DifferentialDriveModel, BicycleModel, ArticulatedModel +``` + +### DifferentialDriveModel + +```python +model = DifferentialDriveModel(wheel_radius_m=0.15, track_width_m=0.5) + +wheels = model.body_velocity_to_wheel_velocities(linear_vel=1.0, angular_vel=0.3) +print(wheels.left_wheel_velocity_rad_s, wheels.right_wheel_velocity_rad_s) + +body = model.wheel_velocities_to_body_velocity(left_wheel_vel=6.0, right_wheel_vel=7.0) +print(body.linear_velocity_m_s, body.angular_velocity_rad_s) +``` + +### BicycleModel + +```python +model = BicycleModel(wheelbase_m=2.7, track_width_m=1.6, wheel_radius_m=0.35) + +state = model.body_velocity_to_steering(linear_velocity=2.0, angular_velocity=0.2) +print(state.steering_angle_rad, state.turning_radius_m) + +body = model.steering_to_body_velocity(velocity=2.0, steering_angle=0.15) +print(body.linear_velocity_m_s, body.angular_velocity_rad_s) + +radius = model.turning_radius(steering_angle=0.15) +angle = model.steering_angle_from_radius(radius=10.0) +``` + +### ArticulatedModel + +```python +model = ArticulatedModel( + articulation_to_front_axle_m=1.8, + articulation_to_rear_axle_m=1.5, + front_track_width_m=2.0, + rear_track_width_m=2.0, + front_wheel_radius_m=0.6, + rear_wheel_radius_m=0.6, +) + +state = model.body_velocity_to_vehicle_state( + linear_velocity_m_s=1.5, angular_velocity_rad_s=0.1 +) +print(state.articulation_angle_rad, state.front_axle_turning_radius_m) + +axles = model.articulation_to_axle_velocities( + linear_velocity_m_s=1.5, articulation_angle_rad=0.3 +) +print(axles.front_axle_turning_velocity_rad_s, axles.rear_axle_turning_velocity_rad_s) +``` + +## Models reference + +### DifferentialDriveModel + +| Constructor param | Description | +|---|---| +| `wheel_radius_m` | Wheel radius in meters | +| `track_width_m` | Distance between wheel centers | + +| Method (C++ / Python) | Parameters | Returns | +|---|---|---| +| `bodyVelocityToWheelVelocities` / `body_velocity_to_wheel_velocities` | linear vel (m/s), angular vel (rad/s) | `DifferentialDriveWheelVelocities` | +| `wheelVelocitiesToBodyVelocity` / `wheel_velocities_to_body_velocity` | left wheel vel (rad/s), right wheel vel (rad/s) | `DifferentialDriveBodyVelocity` | + +### BicycleModel + +| Constructor param | Description | +|---|---| +| `wheelbase_m` | Front-to-rear axle distance | +| `track_width_m` | Distance between left and right wheels | +| `wheel_radius_m` | Wheel radius in meters | + +| Method (C++ / Python) | Parameters | Returns | +|---|---|---| +| `bodyVelocityToSteering` / `body_velocity_to_steering` | linear vel (m/s), angular vel (rad/s) | `BicycleSteeringState` | +| `steeringToBodyVelocity` / `steering_to_body_velocity` | velocity (m/s), steering angle (rad) | `BicycleBodyVelocity` | +| `turningRadius` / `turning_radius` | steering angle (rad) | `double` / `float` | +| `steeringAngleFromRadius` / `steering_angle_from_radius` | radius (m) | `double` / `float` | + +### ArticulatedModel + +| Constructor param | Description | +|---|---| +| `articulation_to_front_axle_m` | Joint-to-front-axle distance | +| `articulation_to_rear_axle_m` | Joint-to-rear-axle distance | +| `front_track_width_m` | Front axle wheel spacing | +| `rear_track_width_m` | Rear axle wheel spacing | +| `front_wheel_radius_m` | Front wheel radius | +| `rear_wheel_radius_m` | Rear wheel radius | + +| Method (C++ / Python) | Parameters | Returns | +|---|---|---| +| `bodyVelocityToVehicleState` / `body_velocity_to_vehicle_state` | linear vel (m/s), angular vel (rad/s) | `ArticulatedVehicleState` | +| `articulationToAxleVelocities` / `articulation_to_axle_velocities` | linear vel (m/s), articulation angle (rad) | `ArticulatedAxleVelocities` | diff --git a/derivations/articulated_model.md b/derivations/articulated_model.md new file mode 100644 index 0000000..265d2bb --- /dev/null +++ b/derivations/articulated_model.md @@ -0,0 +1,201 @@ +# Articulated Vehicle Kinematics + +A vehicle with two rigid sections connected by a central pivot (the articulation joint). Examples: wheel loaders, articulated dump trucks. The vehicle steers by changing the angle between the front and rear sections rather than rotating the wheels. + +## Parameters + +| Symbol | Code name | Description | +|---|---|---| +| $L_f$ | `articulation_to_front_axle_m` | Distance from articulation joint to front axle | +| $L_r$ | `articulation_to_rear_axle_m` | Distance from articulation joint to rear axle | +| $W_f$ | `front_track_width_m` | Width between the center of the front wheels | +| $W_r$ | `rear_track_width_m` | Width between the center of the rear wheels | +| $r_f$ | `front_wheel_radius_m` | Front wheel radius | +| $r_r$ | `rear_wheel_radius_m` | Rear wheel radius | + +## Coordinate convention + +- $v$ — forward linear velocity of the vehicle (positive = forward) +- $\omega$ — yaw rate of the body (positive = counter-clockwise / left turn) +- $\gamma$ — articulation angle between front and rear sections (positive = left turn, counterclockwise) +- $\dot{\gamma}$ — articulation angle rate of change (currently fixed at $0$ in the implementation; placeholder for future estimation) +- $\theta_f$ — heading angle of the front body +- $\theta_r$ — heading angle of the rear body + +> **Note on sign convention:** The kinematic model below is adapted from Corke & Ridley (2001) [1], which derives the steering kinematics for a center-articulated vehicle. The original paper uses the convention $\theta_f = \theta_r - \gamma$, where a positive articulation angle corresponds to a clockwise (rightward) turn. This paper uses the **ROS2 REP-103 convention** [2], in which the Z-axis points upward and counterclockwise rotations are positive. Accordingly, we use $\theta_f = \theta_r + \gamma$, which flips the sign of the yaw rate equations relative to [1]. All other assumptions from [1] are retained: point wheel–ground contact, no wheel slip, and planar motion. + +--- + +## Derivation of Axle Turning Velocities + +This section derives $\omega_{\text{front}} = \dot{\theta}_f$ from first principles, following the methodology of Corke & Ridley [1] with the ROS2 sign convention applied. + +### Kinematic equations of the front body + +The velocity of the front axle center $P_f$ is: + +$$\dot{x}_f = v \cos\theta_f \tag{1}$$ + +$$\dot{y}_f = v \sin\theta_f \tag{2}$$ + +### Geometric constraint between bodies + +The positions of $P_f$ and $P_r$ are related through the articulation joint $H$: + +$$x_r + L_r \cos\theta_r + L_f \cos\theta_f = x_f \tag{3}$$ + +$$y_r + L_r \sin\theta_r + L_f \sin\theta_f = y_f \tag{4}$$ + +### Nonholonomic (no-slip) constraints + +Rolling without slipping requires zero velocity component along each axle: + +$$\dot{x}_r \sin\theta_r - \dot{y}_r \cos\theta_r = 0 \tag{5}$$ + +$$\dot{x}_f \sin\theta_f - \dot{y}_f \cos\theta_f = 0 \tag{6}$$ + +Equation (6) is satisfied identically by substituting (1) and (2). + +### Time differentiation of geometric constraints + +Differentiating (3) and (4) with respect to time and substituting (1) and (2): + +$$\dot{x}_r = v\cos\theta_f + L_r \sin\theta_r\,\dot{\theta}_r + L_f \sin\theta_f\,\dot{\theta}_f \tag{7}$$ + +$$\dot{y}_r = v\sin\theta_f - L_r \cos\theta_r\,\dot{\theta}_r - L_f \cos\theta_f\,\dot{\theta}_f \tag{8}$$ + +### Applying the rear nonholonomic constraint + +Substituting (7) and (8) into (5), expanding, and applying $\sin^2 + \cos^2 = 1$ along with compound angle identities: + +$$v\sin(\theta_r - \theta_f) + L_r\,\dot{\theta}_r + L_f\,\dot{\theta}_f\cos(\theta_f - \theta_r) = 0 \tag{9}$$ + +### Applying the ROS2 angle convention + +With $\theta_f = \theta_r + \gamma$ (ROS2 convention): + +- $\theta_r - \theta_f = -\gamma \implies \sin(\theta_r - \theta_f) = -\sin\gamma$ +- $\theta_f - \theta_r = \gamma \implies \cos(\theta_f - \theta_r) = \cos\gamma$ +- Differentiating: $\dot{\theta}_r = \dot{\theta}_f - \dot{\gamma}$ + +Substituting into (9): + +$$-v\sin\gamma + L_r(\dot{\theta}_f - \dot{\gamma}) + L_f\,\dot{\theta}_f\cos\gamma = 0$$ + +Collecting $\dot{\theta}_f$ terms: + +$$\dot{\theta}_f(L_r + L_f\cos\gamma) = v\sin\gamma + L_r\,\dot{\gamma}$$ + +$$\boxed{\omega_{\text{front}} = \dot{\theta}_f = \frac{v\sin\gamma + L_r\,\dot{\gamma}}{L_f\cos\gamma + L_r}} \tag{10}$$ + +The rear body yaw rate follows from differentiating the angle convention $\theta_f = \theta_r + \gamma$: + +$$\boxed{\omega_{\text{rear}} = \dot{\theta}_r = \dot{\theta}_f - \dot{\gamma} = \frac{v\sin\gamma + L_r\,\dot{\gamma}}{L_f\cos\gamma + L_r} - \dot{\gamma}} \tag{11}$$ + +> **Comparison with [1]:** The original paper obtains $\dot{\theta}_f = -(v\sin\gamma + l_2\dot{\gamma})/(l_1\cos\gamma + l_2)$. Our result differs only in sign, which is the direct consequence of the flipped angle convention. + +--- + +## Derivation of Body Velocity to Articulation Angle + +Given a desired body velocity $(v, \omega)$, we need to find the articulation angle $\gamma$ that produces it. + +### Setting up the harmonic addition + +Setting $\omega_{\text{front}} = \omega$ in equation (10) and rearranging: + +$$v\sin\gamma + L_r\,\dot{\gamma} = \omega(L_f\cos\gamma + L_r)$$ + +Rearranging to the standard harmonic form $A\cos\gamma + B\sin\gamma$ on the left: + +$$\omega L_f\cos\gamma - v\sin\gamma = L_r(\dot{\gamma} - \omega) \tag{12}$$ + +With $A = \omega L_f$ and $B = -v$, the harmonic addition theorem states: + +$$A\cos\gamma + B\sin\gamma = R\cos(\gamma + \phi)$$ + +where: + +$$R = \sqrt{A^2 + B^2} = \sqrt{\omega^2 L_f^2 + v^2}$$ + +$$\phi = \arctan\!\left(\frac{-B}{A}\right) = \arctan\!\left(\frac{v}{\omega L_f}\right) = \operatorname{atan2}(v,\; \omega L_f)$$ + +### Solving for $\gamma$ + +Substituting into (12): + +$$R\cos(\gamma + \phi) = L_r(\dot{\gamma} - \omega)$$ + +$$\cos(\gamma + \phi) = \frac{L_r(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}$$ + +$$\gamma + \phi = \arccos\!\left(\frac{L_r(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right)$$ + +$$\boxed{\gamma = \arccos\!\left(\frac{L_r\,(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f)} \tag{13}$$ + +With $\dot{\gamma} = 0$ (current implementation): + +$$\gamma = \arccos\!\left(\frac{-L_r\,\omega}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f) \tag{14}$$ + +--- + +## Turning radii + +Once the articulation angle is known, the turning radius for each axle is computed from the triangle formed by the articulation joint and the ICR: + +$$R_{\text{front}} = \frac{L_f \cos\gamma + L_r}{\sin\gamma}$$ + +$$R_{\text{rear}} = \frac{L_r \cos\gamma + L_f}{\sin\gamma}$$ + +These are the signed distances from the ICR to each axle center, measured along the respective axle lines. See Section II of [1] for the full geometric derivation. + +--- + +## Wheel speeds + +Each wheel sits at $\pm$ half the track width from its axle center. Using rigid-body kinematics (ground speed $= \omega \times$ distance from ICR): + +$$\omega_{\text{fr}} = \frac{(R_{\text{front}} + W_f/2)\;\omega}{r_f} \qquad \omega_{\text{fl}} = \frac{(R_{\text{front}} - W_f/2)\;\omega}{r_f}$$ + +$$\omega_{\text{rr}} = \frac{(R_{\text{rear}} + W_r/2)\;\omega}{r_r} \qquad \omega_{\text{rl}} = \frac{(R_{\text{rear}} - W_r/2)\;\omega}{r_r}$$ + +Outer wheels (further from ICR) spin faster; inner wheels spin slower. + +--- + +## Axle turning velocities + +Given the current articulation angle and forward velocity, the turning velocity of each axle can be computed. This is useful for odometry — converting articulation sensor readings back to axle-level yaw rates. + +$$\omega_{\text{front}} = \frac{v\sin\gamma + L_r\,\dot{\gamma}}{L_f \cos\gamma + L_r}$$ + +$$\omega_{\text{rear}} = \omega_{\text{front}} - \dot{\gamma}$$ + +With $\dot{\gamma} = 0$: + +$$\omega_{\text{front}} = \frac{v \sin\gamma}{L_f \cos\gamma + L_r}$$ + +$$\omega_{\text{rear}} = \omega_{\text{front}}$$ + +When $\dot{\gamma} = 0$, both axles have the same turning velocity because the articulation angle is not changing — the vehicle is in a steady-state turn. + +--- + +## Roundtrip consistency + +Computing `bodyVelocityToVehicleState(v, omega)` to get $\gamma$, then feeding that $\gamma$ into `articulationToAxleVelocities(v, gamma)`, yields $\omega_{\text{front}} = \omega$. The signs are consistent because both the body velocity convention and the axle velocity convention follow ROS2 REP-103 (counterclockwise positive). + +--- + +## Future work + +The $\dot{\gamma}$ term is currently hardcoded to $0$. Once articulation angle rate estimation is available (e.g., from an IMU or joint encoder derivative), it will be incorporated to improve accuracy during transient steering maneuvers. + +Add diagrams to the readme for visualization + +--- + +## References + +[1] P. I. Corke and P. Ridley, "Steering Kinematics for a Center-Articulated Mobile Robot," *IEEE Transactions on Robotics and Automation*, vol. 17, no. 2, pp. 215–218, April 2001. + +[2] ROS REP-103, "Standard Units of Measure and Coordinate Conventions," ROS.org, 2010. [Online]. Available: https://www.ros.org/reps/rep-0103.html diff --git a/derivations/bicycle_model.md b/derivations/bicycle_model.md new file mode 100644 index 0000000..2e8b1b5 --- /dev/null +++ b/derivations/bicycle_model.md @@ -0,0 +1,94 @@ +# Bicycle Model (Ackermann) Kinematics + +A front-steered, rear-driven vehicle modeled as if both front wheels collapse to a single virtual wheel on the centerline (the "bicycle" simplification). The full four-wheel speed computation then re-expands using Ackermann geometry relative to the instantaneous center of rotation. + +## Parameters + +| Symbol | Code name | Description | +|---|---|---| +| $L$ | `wheelbase_m` | Front axle to rear axle distance | +| $W$ | `track_width_m` | Distance between the centers of the left and right wheels | +| $r$ | `wheel_radius_m` | Wheel radius | + +## Coordinate convention + +- $v$ — forward velocity at the rear axle center (positive = forward) +- $\omega$ — yaw rate of the body (positive = counter-clockwise / left turn) +- $\delta$ — front steering angle (positive = left turn) +- $R$ — turning radius measured from the ICR to the rear axle center (positive = left turn, negative = right turn) + +## Forward kinematics (steering → body velocity) + +The bicycle model relates the steering angle to the yaw rate through the wheelbase: + +$$\omega = \frac{v \cdot \tan(\delta)}{L}$$ + +This follows from the geometry: the rear axle center traces a circle of radius $R = L / \tan(\delta)$, and the yaw rate is $\omega = v / R$. + +## Inverse kinematics (body velocity → steering state) + +### Steering angle + +Invert the forward relation: + +$$\delta = \operatorname{atan2}(\omega \cdot L,\; v)$$ + +$\operatorname{atan2}$ is used instead of $\arctan(\omega L / v)$ to handle all quadrants correctly, including reverse motion. + +### Turning radius + +$$R = \frac{L}{\tan(\delta)}$$ + +When $\delta \to 0$, $R \to \infty$ (straight line). The sign of $R$ matches the sign of $\delta$. + +### Inverse conversion + +$$\delta = \arctan\!\left(\frac{L}{R}\right)$$ + +## Four-wheel speeds via ICR geometry + +The ICR lies on the line extending the rear axle, at signed distance $R$ from the rear axle center. Every point on the rigid body has ground speed equal to $\omega$ times its distance from the ICR. + +### Rear wheels + +The rear wheels sit on the axle at $\pm\, W/2$ from center, directly along the ICR line: + +$$d_{\text{rl}} = R - \frac{W}{2} \qquad d_{\text{rr}} = R + \frac{W}{2}$$ + +### Front wheels + +The front wheels are offset longitudinally by $L$ from the rear axle. Their distance from the ICR is the hypotenuse of the lateral offset and the wheelbase: + +$$d_{\text{fl}} = \operatorname{copysign}\!\left(\sqrt{\left(R - \tfrac{W}{2}\right)^2 + L^2},\; R - \tfrac{W}{2}\right)$$ + +$$d_{\text{fr}} = \operatorname{copysign}\!\left(\sqrt{\left(R + \tfrac{W}{2}\right)^2 + L^2},\; R + \tfrac{W}{2}\right)$$ + +$\operatorname{copysign}$ preserves the sign of the lateral offset. This matters when the ICR falls between the rear wheels ($|R| < W/2$) — in that case the inner wheel must spin in the opposite direction. + +### Wheel angular velocities + +Convert each wheel's ground speed to angular velocity: + +$$\omega_{\text{wheel}} = \frac{\omega \cdot d_{\text{ICR}}}{r}$$ + +Applied to all four wheels: + +$$\omega_{\text{rl}} = \frac{\omega \left(R - W/2\right)}{r}$$ + +$$\omega_{\text{rr}} = \frac{\omega \left(R + W/2\right)}{r}$$ + +$$\omega_{\text{fl}} = \frac{\omega \cdot \operatorname{copysign}\!\left(\sqrt{(R - W/2)^2 + L^2},\; R - W/2\right)}{r}$$ + +$$\omega_{\text{fr}} = \frac{\omega \cdot \operatorname{copysign}\!\left(\sqrt{(R + W/2)^2 + L^2},\; R + W/2\right)}{r}$$ + +## Edge cases in the implementation + +| Condition | Behavior | +|---|---| +| $v \approx 0$ | Bicycle model is degenerate — cannot determine $\delta$. Returns $\delta = 0$, $R = \infty$, all wheel speeds $= 0$ | +| $\omega \approx 0,\; v \neq 0$ | Straight line. All wheels spin at $v / r$ | +| $\|R\| < W/2$ | ICR between rear wheels. Inner wheel reverses direction (handled by $\operatorname{copysign}$) | + +## Future Work + +Add diagrams to the readme for visualization diff --git a/derivations/differential_drive.md b/derivations/differential_drive.md new file mode 100644 index 0000000..20172f3 --- /dev/null +++ b/derivations/differential_drive.md @@ -0,0 +1,62 @@ +# Differential Drive Kinematics + +A two-wheeled robot where each wheel is independently driven. The wheels share a common axle and the robot pivots about a point on that axle. + +## Parameters + +| Symbol | Code name | Description | +|---|---|---| +| $r$ | `wheel_radius_m` | Wheel radius | +| $W$ | `track_width_m` | Distance between the center of the two wheels | + +## Coordinate convention + +- $v$ — forward linear velocity of the body center (positive = forward) +- $\omega$ — angular velocity about the body center (positive = counter-clockwise / left turn) +- $\omega_L, \omega_R$ — angular velocities of the left and right wheels (positive = forward rolling) + +## Forward kinematics (wheel velocities → body velocity) + +Each wheel's ground speed is its angular velocity times the wheel radius: + +$$v_L = \omega_L \cdot r \qquad v_R = \omega_R \cdot r$$ + +The body center lies midway between the wheels, so its linear velocity is the average of the two ground speeds: + +$$v = \frac{v_L + v_R}{2} = \frac{r\,(\omega_L + \omega_R)}{2}$$ + +The angular velocity comes from the velocity difference across the track width. The right wheel is at $+W/2$ from center and the left at $-W/2$, so: + +$$\omega = \frac{v_R - v_L}{W} = \frac{r\,(\omega_R - \omega_L)}{W}$$ + +## Inverse kinematics (body velocity → wheel velocities) + +Solve the forward equations for the wheel ground speeds: + +$$v_L = v - \omega \cdot \frac{W}{2} \qquad v_R = v + \omega \cdot \frac{W}{2}$$ + +Then convert to wheel angular velocities: + +$$\omega_L = \frac{v_L}{r} = \frac{v - \omega \cdot W/2}{r}$$ + +$$\omega_R = \frac{v_R}{r} = \frac{v + \omega \cdot W/2}{r}$$ + +## Special cases + +| Condition | Behavior | +|---|---| +| $\omega_L = \omega_R$ | Straight line ($\omega = 0$) | +| $\omega_L = -\omega_R$ | Spin in place ($v = 0$), ICR at body center | +| One wheel stationary | Pivot about that wheel | + +## Instantaneous center of rotation (ICR) + +The ICR lies on the wheel axle at a signed distance from the body center: + +$$d_{\text{ICR}} = \frac{v}{\omega} = \frac{W}{2} \cdot \frac{\omega_R + \omega_L}{\omega_R - \omega_L}$$ + +When the robot drives straight, the ICR is at infinity. When spinning in place, it is at the origin. + +## Future Work + +Add diagrams to the readme for visualization diff --git a/include/polymath_kinematics/articulated_model.hpp b/include/polymath_kinematics/articulated_model.hpp new file mode 100644 index 0000000..8969f47 --- /dev/null +++ b/include/polymath_kinematics/articulated_model.hpp @@ -0,0 +1,131 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__ARTICULATED_MODEL_HPP__ +#define POLYMATH_KINEMATICS__ARTICULATED_MODEL_HPP__ + +namespace polymath::kinematics +{ + +/// @brief Complete vehicle state for an articulated vehicle +struct ArticulatedVehicleState +{ + double articulation_angle_rad; + double linear_velocity_m_s; + double front_right_wheel_speed_rad_s; + double front_left_wheel_speed_rad_s; + double rear_right_wheel_speed_rad_s; + double rear_left_wheel_speed_rad_s; + double front_axle_turning_radius_m; + double rear_axle_turning_radius_m; +}; + +/// @brief Axle turning velocities for an articulated vehicle +struct ArticulatedAxleVelocities +{ + double linear_velocity_m_s; + double front_axle_turning_velocity_rad_s; + double rear_axle_turning_velocity_rad_s; +}; + +/// @brief Kinematic model for articulated vehicles (e.g., wheel loaders, articulated dump trucks) +/// @note TODO: (Zeerek) We want to add articulated angle turning velocity into our estimations +class ArticulatedModel +{ +public: + /// @brief Construct an articulated vehicle model + /// @param articulation_to_front_axle_m Distance from articulation joint to front axle + /// @param articulation_to_rear_axle_m Distance from articulation joint to rear axle + /// @param front_track_width_m Width between front wheels + /// @param rear_track_width_m Width between rear wheels + /// @param front_wheel_radius_m Radius of front wheels + /// @param rear_wheel_radius_m Radius of rear wheels + ArticulatedModel( + double articulation_to_front_axle_m, + double articulation_to_rear_axle_m, + double front_track_width_m, + double rear_track_width_m, + double front_wheel_radius_m, + double rear_wheel_radius_m) + : articulation_to_front_axle_m_(articulation_to_front_axle_m) + , articulation_to_rear_axle_m_(articulation_to_rear_axle_m) + , front_track_width_m_(front_track_width_m) + , rear_track_width_m_(rear_track_width_m) + , front_wheel_radius_m_(front_wheel_radius_m) + , rear_wheel_radius_m_(rear_wheel_radius_m) {}; + + ~ArticulatedModel() = default; + + /// @brief Convert body velocity to vehicle state (articulation angle and wheel speeds) + /// @param linear_velocity_m_s Desired linear velocity in m/s + /// @param angular_velocity_rad_s Desired angular velocity in rad/s + /// @return Vehicle state including required articulation angle and wheel speeds + ArticulatedVehicleState bodyVelocityToVehicleState(double linear_velocity_m_s, double angular_velocity_rad_s); + + /// @brief Convert articulation state to axle turning velocities + /// @param linear_velocity_m_s Current linear velocity in m/s + /// @param articulation_angle_rad Current articulation angle in radians + /// @return Axle turning velocities for front and rear axles + ArticulatedAxleVelocities articulationToAxleVelocities(double linear_velocity_m_s, double articulation_angle_rad); + + double get_articulation_to_front_axle_m() const + { + return articulation_to_front_axle_m_; + } + + double get_articulation_to_rear_axle_m() const + { + return articulation_to_rear_axle_m_; + } + + double get_front_track_width_m() const + { + return front_track_width_m_; + } + + double get_rear_track_width_m() const + { + return rear_track_width_m_; + } + + double get_front_wheel_radius_m() const + { + return front_wheel_radius_m_; + } + + double get_rear_wheel_radius_m() const + { + return rear_wheel_radius_m_; + } + +private: + double articulation_to_front_axle_m_; + double articulation_to_rear_axle_m_; + double front_track_width_m_; + double rear_track_width_m_; + double front_wheel_radius_m_; + double rear_wheel_radius_m_; + + /// @brief Articulation turning velocity once calculated or available + /// TODO: (Zeerek) Add ability to pass this in when generating estimations + static constexpr double articulation_turning_velocity_rad_s_ = 0.0; + + /// @brief Threshold below which a velocity is treated as zero to avoid numerical + /// singularities (e.g. 0/0 in acos, or inf*0 in wheel speed calculations) + static constexpr double ZERO_VELOCITY_THRESHOLD = 1e-9; +}; + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__ARTICULATED_MODEL_HPP__ diff --git a/include/polymath_kinematics/bicycle_model.hpp b/include/polymath_kinematics/bicycle_model.hpp new file mode 100644 index 0000000..e28176d --- /dev/null +++ b/include/polymath_kinematics/bicycle_model.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__BICYCLE_MODEL_HPP__ +#define POLYMATH_KINEMATICS__BICYCLE_MODEL_HPP__ + +namespace polymath::kinematics +{ + +struct BicycleSteeringState +{ + double velocity_m_s; + double steering_angle_rad; + double turning_radius_m; + double front_right_wheel_rad_s; + double front_left_wheel_rad_s; + double rear_right_wheel_rad_s; + double rear_left_wheel_rad_s; +}; + +struct BicycleBodyVelocity +{ + double linear_velocity_m_s; + double angular_velocity_rad_s; +}; + +class BicycleModel +{ +public: + BicycleModel(double wheelbase_m, double track_width_m, double wheel_radius_m) + : wheelbase_m_(wheelbase_m) + , track_width_m_(track_width_m) + , wheel_radius_m_(wheel_radius_m) + {} + + ~BicycleModel() = default; + + /// @brief Convert velocity and steering angle to body velocities + /// @param velocity Forward velocity in m/s + /// @param steering_angle Steering angle in radians + /// @return Linear and angular body velocities + BicycleBodyVelocity steeringToBodyVelocity(double velocity, double steering_angle); + + /// @brief Convert body velocities to steering angle + /// @param linear_velocity Forward velocity in m/s + /// @param angular_velocity Angular velocity in rad/s + /// @return Velocity and required steering angle + BicycleSteeringState bodyVelocityToSteering(double linear_velocity, double angular_velocity); + + /// @brief Calculate turning radius from steering angle + /// @param steering_angle Steering angle in radians + /// @return Turning radius in meters (infinity for straight line) + double turningRadius(double steering_angle); + + /// @brief Calculate steering angle from desired turning radius + /// @param radius Turning radius in meters + /// @return Required steering angle in radians + double steeringAngleFromRadius(double radius); + + double get_wheelbase_m() const + { + return wheelbase_m_; + } + + double get_track_width_m() const + { + return track_width_m_; + } + + double get_wheel_radius_m() const + { + return wheel_radius_m_; + } + +private: + double wheelbase_m_; + double track_width_m_; + double wheel_radius_m_; + + /// @brief Threshold below which a velocity is treated as zero to avoid numerical + /// singularities (e.g. division by zero in steering angle and turning radius calculations) + static constexpr double ZERO_VELOCITY_THRESHOLD = 1e-9; +}; + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__BICYCLE_MODEL_HPP__ diff --git a/include/polymath_kinematics/differential_drive_model.hpp b/include/polymath_kinematics/differential_drive_model.hpp new file mode 100644 index 0000000..d4a0c6d --- /dev/null +++ b/include/polymath_kinematics/differential_drive_model.hpp @@ -0,0 +1,72 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POLYMATH_KINEMATICS__DIFFERENTIAL_DRIVE_MODEL_HPP__ +#define POLYMATH_KINEMATICS__DIFFERENTIAL_DRIVE_MODEL_HPP__ + +namespace polymath::kinematics +{ + +struct DifferentialDriveWheelVelocities +{ + double left_wheel_velocity_rad_s; + double right_wheel_velocity_rad_s; +}; + +struct DifferentialDriveBodyVelocity +{ + double linear_velocity_m_s; + double angular_velocity_rad_s; +}; + +class DifferentialDriveModel +{ +public: + DifferentialDriveModel(double wheel_radius_m, double track_width_m) + : wheel_radius_m_(wheel_radius_m) + , track_width_m_(track_width_m) + {} + + ~DifferentialDriveModel() = default; + + /// @brief Convert wheel velocities to body velocities + /// @param left_wheel_vel Left wheel angular velocity in rad/s + /// @param right_wheel_vel Right wheel angular velocity in rad/s + /// @return Body linear and angular velocities + DifferentialDriveBodyVelocity wheelVelocitiesToBodyVelocity(double left_wheel_vel, double right_wheel_vel); + + /// @brief Convert body velocities to wheel velocities + /// @param linear_vel Desired linear velocity in m/s + /// @param angular_vel Desired angular velocity in rad/s + /// @return Required wheel angular velocities + DifferentialDriveWheelVelocities bodyVelocityToWheelVelocities(double linear_vel, double angular_vel); + + double get_wheel_radius_m() const + { + return wheel_radius_m_; + } + + double get_track_width_m() const + { + return track_width_m_; + } + +private: + double wheel_radius_m_; + double track_width_m_; +}; + +} // namespace polymath::kinematics + +#endif // POLYMATH_KINEMATICS__DIFFERENTIAL_DRIVE_MODEL_HPP__ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f37095d --- /dev/null +++ b/package.xml @@ -0,0 +1,20 @@ + + + + polymath_kinematics + 0.1.0 + Simple mixed C++/Python kinematics library + Polymath Robotics + Apache-2.0 + + ament_cmake + ament_cmake_python + + pybind11_vendor + + ament_cmake_pytest + + + ament_cmake + + diff --git a/polymath_kinematics/__init__.py b/polymath_kinematics/__init__.py new file mode 100644 index 0000000..fdf2e74 --- /dev/null +++ b/polymath_kinematics/__init__.py @@ -0,0 +1,75 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from polymath_kinematics_cpp import ( + ArticulatedAxleVelocities, + ArticulatedModel, + ArticulatedVehicleState, + BicycleBodyVelocity, + BicycleModel, + BicycleSteeringState, + DifferentialDriveBodyVelocity, + DifferentialDriveModel, + DifferentialDriveWheelVelocities, +) + +__all__ = [ + 'ArticulatedAxleVelocities', + 'ArticulatedModel', + 'ArticulatedVehicleState', + 'BicycleBodyVelocity', + 'BicycleModel', + 'BicycleSteeringState', + 'DifferentialDriveBodyVelocity', + 'DifferentialDriveModel', + 'DifferentialDriveWheelVelocities', + 'Pose2D', + 'Twist2D', + 'normalize_angle', + 'transform_pose', +] + + +class Pose2D: + def __init__(self, x=0.0, y=0.0, theta=0.0): + self.x = x + self.y = y + self.theta = theta + + +class Twist2D: + def __init__(self, linear_x=0.0, linear_y=0.0, angular_z=0.0): + self.linear_x = linear_x + self.linear_y = linear_y + self.angular_z = angular_z + + +def normalize_angle(angle): + """Normalize angle to [-pi, pi]""" + while angle > math.pi: + angle -= 2.0 * math.pi + while angle < -math.pi: + angle += 2.0 * math.pi + return angle + + +def transform_pose(pose, transform): + """Transform a pose by a relative transform""" + result = Pose2D() + result.x = pose.x + transform.x * math.cos(pose.theta) - transform.y * math.sin(pose.theta) + result.y = pose.y + transform.x * math.sin(pose.theta) + transform.y * math.cos(pose.theta) + result.theta = normalize_angle(pose.theta + transform.theta) + return result diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..617b5ed --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,16 @@ +[build-system] +requires = ["setuptools", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "polymath_kinematics" +version = "0.1.0" +description = "Simple mixed C++/Python kinematics library" +license = {text = "Apache-2.0"} +maintainers = [ + {name = "Polymath Robotics", email = "dev@polymathrobotics.com"} +] +requires-python = ">=3.8" + +[tool.setuptools] +packages = ["polymath_kinematics"] diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..3414b8a --- /dev/null +++ b/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/polymath_kinematics + +[install] +install_scripts=$base/lib/polymath_kinematics diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp new file mode 100644 index 0000000..7ce6c2c --- /dev/null +++ b/src/articulated_model.cpp @@ -0,0 +1,102 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/articulated_model.hpp" + +#include +#include + +namespace polymath::kinematics +{ + +constexpr inline double square(double x) +{ + return x * x; +} + +ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( + double linear_velocity_m_s, double angular_velocity_rad_s) +{ + // Guard: fully stationary — sqrt denominator collapses to 0, producing 0/0 = NaN + if ( + std::abs(linear_velocity_m_s) < ZERO_VELOCITY_THRESHOLD && + std::abs(angular_velocity_rad_s) < ZERO_VELOCITY_THRESHOLD) + { + return ArticulatedVehicleState{ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, std::numeric_limits::infinity(), std::numeric_limits::infinity()}; + } + + // Guard: straight line — articulation_angle = 0, so sin(0) = 0 → radii = inf → inf * 0 = NaN wheel speeds + if (std::abs(angular_velocity_rad_s) < ZERO_VELOCITY_THRESHOLD) { + double front_wheel_rad_s = linear_velocity_m_s / front_wheel_radius_m_; + double rear_wheel_rad_s = linear_velocity_m_s / rear_wheel_radius_m_; + return ArticulatedVehicleState{ + 0.0, + linear_velocity_m_s, + front_wheel_rad_s, + front_wheel_rad_s, + rear_wheel_rad_s, + rear_wheel_rad_s, + std::numeric_limits::infinity(), + std::numeric_limits::infinity()}; + } + + double articulation_angle = + std::acos( + articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / + std::sqrt(square(angular_velocity_rad_s) * square(articulation_to_front_axle_m_) + square(linear_velocity_m_s))) - + std::atan2(linear_velocity_m_s, angular_velocity_rad_s * articulation_to_front_axle_m_); + + double cos_articulation = std::cos(articulation_angle); + double sin_articulation = std::sin(articulation_angle); + + double front_radius = + (articulation_to_front_axle_m_ * cos_articulation + articulation_to_rear_axle_m_) / sin_articulation; + double rear_radius = + (articulation_to_rear_axle_m_ * cos_articulation + articulation_to_front_axle_m_) / sin_articulation; + + double front_right_wheel_speed = + (front_radius + front_track_width_m_ / 2.0) * angular_velocity_rad_s / front_wheel_radius_m_; + double front_left_wheel_speed = + (front_radius - front_track_width_m_ / 2.0) * angular_velocity_rad_s / front_wheel_radius_m_; + double rear_right_wheel_speed = + (rear_radius + rear_track_width_m_ / 2.0) * angular_velocity_rad_s / rear_wheel_radius_m_; + double rear_left_wheel_speed = + (rear_radius - rear_track_width_m_ / 2.0) * angular_velocity_rad_s / rear_wheel_radius_m_; + + return ArticulatedVehicleState{ + articulation_angle, + linear_velocity_m_s, + front_right_wheel_speed, + front_left_wheel_speed, + rear_right_wheel_speed, + rear_left_wheel_speed, + front_radius, + rear_radius}; +} + +ArticulatedAxleVelocities ArticulatedModel::articulationToAxleVelocities( + double linear_velocity_m_s, double articulation_angle_rad) +{ + double front_axle_turning_velocity = + (linear_velocity_m_s * std::sin(articulation_angle_rad) + + articulation_to_rear_axle_m_ * articulation_turning_velocity_rad_s_) / + (articulation_to_front_axle_m_ * std::cos(articulation_angle_rad) + articulation_to_rear_axle_m_); + + double rear_axle_turning_velocity = front_axle_turning_velocity - articulation_turning_velocity_rad_s_; + + return ArticulatedAxleVelocities{linear_velocity_m_s, front_axle_turning_velocity, rear_axle_turning_velocity}; +} + +} // namespace polymath::kinematics diff --git a/src/bicycle_model.cpp b/src/bicycle_model.cpp new file mode 100644 index 0000000..8b60268 --- /dev/null +++ b/src/bicycle_model.cpp @@ -0,0 +1,101 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/bicycle_model.hpp" + +#include +#include + +namespace polymath::kinematics +{ + +BicycleBodyVelocity BicycleModel::steeringToBodyVelocity(double velocity, double steering_angle) +{ + double angular_velocity = velocity * std::tan(steering_angle) / wheelbase_m_; + + return BicycleBodyVelocity{velocity, angular_velocity}; +} + +BicycleSteeringState BicycleModel::bodyVelocityToSteering(double linear_velocity, double angular_velocity) +{ + if (std::abs(linear_velocity) < ZERO_VELOCITY_THRESHOLD) { + // Stationary: cannot determine steering angle from the bicycle model + return BicycleSteeringState{linear_velocity, 0.0, std::numeric_limits::infinity(), 0.0, 0.0, 0.0, 0.0}; + } + + if (std::abs(angular_velocity) < ZERO_VELOCITY_THRESHOLD) { + // Straight line: no rotation, all wheels spin at the same rate + double wheel_rad_s = linear_velocity / wheel_radius_m_; + return BicycleSteeringState{ + linear_velocity, + 0.0, + std::numeric_limits::infinity(), + wheel_rad_s, + wheel_rad_s, + wheel_rad_s, + wheel_rad_s}; + } + + // steering_angle = atan2(omega * L, v) inverts the forward kinematic + // relation omega = v * tan(steering_angle) / L + double steering_angle = std::atan2(angular_velocity * wheelbase_m_, linear_velocity); + double turning_radius = turningRadius(steering_angle); + double half_track = track_width_m_ / 2.0; + + // Signed lateral distance from ICR to each rear wheel along the rear axle. + // ICR lies on the rear axle line, so the distance is purely lateral. + // Positive = wheel is on the outer side of the turn. + double rear_left_r = turning_radius - half_track; + double rear_right_r = turning_radius + half_track; + + // Signed distance from ICR to each front wheel. The front wheels are offset + // by the wheelbase longitudinally, so the distance is the hypotenuse of + // (lateral_offset, wheelbase). copysign preserves the sign of the lateral + // offset so that each wheel's speed has the correct direction — this matters + // when the ICR falls between the rear wheels (|R| < half_track), where the + // inner and outer wheels rotate in opposite directions. + double front_left_r = std::copysign(std::hypot(rear_left_r, wheelbase_m_), rear_left_r); + double front_right_r = std::copysign(std::hypot(rear_right_r, wheelbase_m_), rear_right_r); + + // Each wheel's ground speed = omega * distance_to_ICR (rigid body kinematics). + // Convert to wheel angular velocity by dividing by wheel radius. + double rl = angular_velocity * rear_left_r / wheel_radius_m_; + double rr = angular_velocity * rear_right_r / wheel_radius_m_; + double fl = angular_velocity * front_left_r / wheel_radius_m_; + double fr = angular_velocity * front_right_r / wheel_radius_m_; + + return BicycleSteeringState{linear_velocity, steering_angle, turning_radius, fr, fl, rr, rl}; +} + +double BicycleModel::turningRadius(double steering_angle) +{ + double tan_steering = std::tan(steering_angle); + + if (std::abs(tan_steering) < ZERO_VELOCITY_THRESHOLD) { + return std::numeric_limits::infinity(); + } + + return wheelbase_m_ / tan_steering; +} + +double BicycleModel::steeringAngleFromRadius(double radius) +{ + if (std::isinf(radius)) { + return 0.0; + } + + return std::atan(wheelbase_m_ / radius); +} + +} // namespace polymath::kinematics diff --git a/src/differential_drive_model.cpp b/src/differential_drive_model.cpp new file mode 100644 index 0000000..80d7e2c --- /dev/null +++ b/src/differential_drive_model.cpp @@ -0,0 +1,50 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/differential_drive_model.hpp" + +namespace polymath::kinematics +{ + +DifferentialDriveBodyVelocity DifferentialDriveModel::wheelVelocitiesToBodyVelocity( + double left_wheel_vel, double right_wheel_vel) +{ + // Convert wheel angular velocities to linear velocities at the wheel + double left_linear = left_wheel_vel * wheel_radius_m_; + double right_linear = right_wheel_vel * wheel_radius_m_; + + // Body linear velocity is the average of the two wheel linear velocities + double linear_velocity = (left_linear + right_linear) / 2.0; + + // Body angular velocity is the difference divided by track width + double angular_velocity = (right_linear - left_linear) / track_width_m_; + + return DifferentialDriveBodyVelocity{linear_velocity, angular_velocity}; +} + +DifferentialDriveWheelVelocities DifferentialDriveModel::bodyVelocityToWheelVelocities( + double linear_vel, double angular_vel) +{ + // Calculate the linear velocity at each wheel + double left_linear = linear_vel - angular_vel * (track_width_m_ / 2.0); + double right_linear = linear_vel + angular_vel * (track_width_m_ / 2.0); + + // Convert linear velocities to angular velocities + double left_wheel_vel = left_linear / wheel_radius_m_; + double right_wheel_vel = right_linear / wheel_radius_m_; + + return DifferentialDriveWheelVelocities{left_wheel_vel, right_wheel_vel}; +} + +} // namespace polymath::kinematics diff --git a/src/kinematics_pybind.cpp b/src/kinematics_pybind.cpp new file mode 100644 index 0000000..0079e67 --- /dev/null +++ b/src/kinematics_pybind.cpp @@ -0,0 +1,135 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "polymath_kinematics/articulated_model.hpp" +#include "polymath_kinematics/bicycle_model.hpp" +#include "polymath_kinematics/differential_drive_model.hpp" +#include "pybind11/pybind11.h" + +namespace py = pybind11; + +namespace polymath::kinematics +{ + +PYBIND11_MODULE(polymath_kinematics_cpp, m) +{ + m.doc() = "Python bindings for polymath kinematics library."; + + // Articulated model bindings + py::class_(m, "ArticulatedVehicleState") + .def(py::init<>()) + .def_readwrite("articulation_angle_rad", &ArticulatedVehicleState::articulation_angle_rad) + .def_readwrite("linear_velocity_m_s", &ArticulatedVehicleState::linear_velocity_m_s) + .def_readwrite("front_right_wheel_speed_rad_s", &ArticulatedVehicleState::front_right_wheel_speed_rad_s) + .def_readwrite("front_left_wheel_speed_rad_s", &ArticulatedVehicleState::front_left_wheel_speed_rad_s) + .def_readwrite("rear_right_wheel_speed_rad_s", &ArticulatedVehicleState::rear_right_wheel_speed_rad_s) + .def_readwrite("rear_left_wheel_speed_rad_s", &ArticulatedVehicleState::rear_left_wheel_speed_rad_s) + .def_readwrite("front_axle_turning_radius_m", &ArticulatedVehicleState::front_axle_turning_radius_m) + .def_readwrite("rear_axle_turning_radius_m", &ArticulatedVehicleState::rear_axle_turning_radius_m); + + py::class_(m, "ArticulatedAxleVelocities") + .def(py::init<>()) + .def_readwrite("linear_velocity_m_s", &ArticulatedAxleVelocities::linear_velocity_m_s) + .def_readwrite("front_axle_turning_velocity_rad_s", &ArticulatedAxleVelocities::front_axle_turning_velocity_rad_s) + .def_readwrite("rear_axle_turning_velocity_rad_s", &ArticulatedAxleVelocities::rear_axle_turning_velocity_rad_s); + + py::class_(m, "ArticulatedModel") + .def( + py::init(), + py::arg("articulation_to_front_axle_m"), + py::arg("articulation_to_rear_axle_m"), + py::arg("front_track_width_m"), + py::arg("rear_track_width_m"), + py::arg("front_wheel_radius_m"), + py::arg("rear_wheel_radius_m")) + .def( + "body_velocity_to_vehicle_state", + &ArticulatedModel::bodyVelocityToVehicleState, + py::arg("linear_velocity_m_s"), + py::arg("angular_velocity_rad_s")) + .def( + "articulation_to_axle_velocities", + &ArticulatedModel::articulationToAxleVelocities, + py::arg("linear_velocity_m_s"), + py::arg("articulation_angle_rad")) + .def_property_readonly("articulation_to_front_axle", &ArticulatedModel::get_articulation_to_front_axle_m) + .def_property_readonly("articulation_to_rear_axle", &ArticulatedModel::get_articulation_to_rear_axle_m) + .def_property_readonly("front_track_width", &ArticulatedModel::get_front_track_width_m) + .def_property_readonly("rear_track_width", &ArticulatedModel::get_rear_track_width_m) + .def_property_readonly("front_wheel_radius", &ArticulatedModel::get_front_wheel_radius_m) + .def_property_readonly("rear_wheel_radius", &ArticulatedModel::get_rear_wheel_radius_m); + + // Differential drive model bindings + py::class_(m, "DifferentialDriveWheelVelocities") + .def(py::init<>()) + .def_readwrite("left_wheel_velocity_rad_s", &DifferentialDriveWheelVelocities::left_wheel_velocity_rad_s) + .def_readwrite("right_wheel_velocity_rad_s", &DifferentialDriveWheelVelocities::right_wheel_velocity_rad_s); + + py::class_(m, "DifferentialDriveBodyVelocity") + .def(py::init<>()) + .def_readwrite("linear_velocity_m_s", &DifferentialDriveBodyVelocity::linear_velocity_m_s) + .def_readwrite("angular_velocity_rad_s", &DifferentialDriveBodyVelocity::angular_velocity_rad_s); + + py::class_(m, "DifferentialDriveModel") + .def(py::init(), py::arg("wheel_radius_m"), py::arg("track_width_m")) + .def( + "wheel_velocities_to_body_velocity", + &DifferentialDriveModel::wheelVelocitiesToBodyVelocity, + py::arg("left_wheel_vel"), + py::arg("right_wheel_vel")) + .def( + "body_velocity_to_wheel_velocities", + &DifferentialDriveModel::bodyVelocityToWheelVelocities, + py::arg("linear_vel"), + py::arg("angular_vel")) + .def_property_readonly("wheel_radius", &DifferentialDriveModel::get_wheel_radius_m) + .def_property_readonly("track_width", &DifferentialDriveModel::get_track_width_m); + + // Bicycle model bindings + py::class_(m, "BicycleSteeringState") + .def(py::init<>()) + .def_readwrite("velocity_m_s", &BicycleSteeringState::velocity_m_s) + .def_readwrite("steering_angle_rad", &BicycleSteeringState::steering_angle_rad) + .def_readwrite("turning_radius_m", &BicycleSteeringState::turning_radius_m) + .def_readwrite("front_right_wheel_rad_s", &BicycleSteeringState::front_right_wheel_rad_s) + .def_readwrite("front_left_wheel_rad_s", &BicycleSteeringState::front_left_wheel_rad_s) + .def_readwrite("rear_right_wheel_rad_s", &BicycleSteeringState::rear_right_wheel_rad_s) + .def_readwrite("rear_left_wheel_rad_s", &BicycleSteeringState::rear_left_wheel_rad_s); + + py::class_(m, "BicycleBodyVelocity") + .def(py::init<>()) + .def_readwrite("linear_velocity_m_s", &BicycleBodyVelocity::linear_velocity_m_s) + .def_readwrite("angular_velocity_rad_s", &BicycleBodyVelocity::angular_velocity_rad_s); + + py::class_(m, "BicycleModel") + .def( + py::init(), py::arg("wheelbase_m"), py::arg("track_width_m"), py::arg("wheel_radius_m")) + .def( + "steering_to_body_velocity", + &BicycleModel::steeringToBodyVelocity, + py::arg("velocity"), + py::arg("steering_angle")) + .def( + "body_velocity_to_steering", + &BicycleModel::bodyVelocityToSteering, + py::arg("linear_velocity"), + py::arg("angular_velocity")) + .def("turning_radius", &BicycleModel::turningRadius, py::arg("steering_angle")) + .def("steering_angle_from_radius", &BicycleModel::steeringAngleFromRadius, py::arg("radius")) + .def_property_readonly("wheelbase", &BicycleModel::get_wheelbase_m) + .def_property_readonly("track_width", &BicycleModel::get_track_width_m) + .def_property_readonly("wheel_radius", &BicycleModel::get_wheel_radius_m); +} + +} // namespace polymath::kinematics diff --git a/test/catch2_compat.hpp b/test/catch2_compat.hpp new file mode 100644 index 0000000..5ab33b0 --- /dev/null +++ b/test/catch2_compat.hpp @@ -0,0 +1,25 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#if __has_include() + #include + #include +using Catch::Approx; +#elif __has_include() + #include +#else + #error "Catch2 headers not found. Please install Catch2 (v2 or v3)." +#endif diff --git a/test/test_articulated_model.cpp b/test/test_articulated_model.cpp new file mode 100644 index 0000000..fc7506b --- /dev/null +++ b/test/test_articulated_model.cpp @@ -0,0 +1,154 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "catch2_compat.hpp" +#include "polymath_kinematics/articulated_model.hpp" + +namespace polymath::kinematics +{ + +TEST_CASE("ArticulatedModel construction") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + CHECK(model.get_articulation_to_front_axle_m() == Approx(1.5)); + CHECK(model.get_articulation_to_rear_axle_m() == Approx(1.2)); + CHECK(model.get_front_track_width_m() == Approx(1.8)); + CHECK(model.get_rear_track_width_m() == Approx(1.6)); + CHECK(model.get_front_wheel_radius_m() == Approx(0.4)); + CHECK(model.get_rear_wheel_radius_m() == Approx(0.5)); +} + +TEST_CASE("ArticulatedModel articulationToAxleVelocities - straight line") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto result = model.articulationToAxleVelocities(2.0, 0.0); + + CHECK(result.linear_velocity_m_s == Approx(2.0)); + CHECK(result.front_axle_turning_velocity_rad_s == Approx(0.0).margin(1e-6)); + CHECK(result.rear_axle_turning_velocity_rad_s == Approx(0.0).margin(1e-6)); +} + +TEST_CASE("ArticulatedModel articulationToAxleVelocities - turning") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + // front_omega = v * sin(gamma) / (Lf * cos(gamma) + Lr) + // = 2.0 * sin(0.3) / (1.5 * cos(0.3) + 1.2) + auto result = model.articulationToAxleVelocities(2.0, 0.3); + + CHECK(result.linear_velocity_m_s == Approx(2.0)); + CHECK(result.front_axle_turning_velocity_rad_s == Approx(0.2244737375)); + CHECK(result.rear_axle_turning_velocity_rad_s == Approx(0.2244737375)); +} + +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - left turn") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto result = model.bodyVelocityToVehicleState(2.0, 0.5); + + CHECK(result.linear_velocity_m_s == Approx(2.0)); + CHECK(result.articulation_angle_rad == Approx(0.6435011088)); + CHECK(result.front_axle_turning_radius_m == Approx(4.0)); + CHECK(result.rear_axle_turning_radius_m == Approx(4.1)); + + // Inner wheels (left for left turn) should be slower than outer + CHECK(result.front_left_wheel_speed_rad_s < result.front_right_wheel_speed_rad_s); + CHECK(result.rear_left_wheel_speed_rad_s < result.rear_right_wheel_speed_rad_s); + + CHECK(result.front_right_wheel_speed_rad_s == Approx(6.125)); + CHECK(result.front_left_wheel_speed_rad_s == Approx(3.875)); + CHECK(result.rear_right_wheel_speed_rad_s == Approx(4.9)); + CHECK(result.rear_left_wheel_speed_rad_s == Approx(3.3)); +} + +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - right turn") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto result = model.bodyVelocityToVehicleState(2.0, -0.5); + + CHECK(result.linear_velocity_m_s == Approx(2.0)); + CHECK(result.articulation_angle_rad == Approx(-0.6435011088)); + + // All wheel speeds should be positive (vehicle moving forward) + CHECK(result.front_right_wheel_speed_rad_s > 0.0); + CHECK(result.front_left_wheel_speed_rad_s > 0.0); + CHECK(result.rear_right_wheel_speed_rad_s > 0.0); + CHECK(result.rear_left_wheel_speed_rad_s > 0.0); + + // Inner wheels (right for right turn) should be slower + CHECK(result.front_right_wheel_speed_rad_s < result.front_left_wheel_speed_rad_s); + CHECK(result.rear_right_wheel_speed_rad_s < result.rear_left_wheel_speed_rad_s); +} + +TEST_CASE("ArticulatedModel roundtrip - bodyVelocityToVehicleState to articulationToAxleVelocities") +{ + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + double linear_velocity = 2.0; + double angular_velocity = 0.5; + + // Get the articulation angle from body velocity + auto vehicle_state = model.bodyVelocityToVehicleState(linear_velocity, angular_velocity); + + // Feed that articulation angle back into articulationToAxleVelocities + auto axle_vel = model.articulationToAxleVelocities(linear_velocity, vehicle_state.articulation_angle_rad); + + // The front axle turning velocity magnitude should match the body angular velocity. + // Sign is negated: positive body omega produces negative front axle turning velocity + // (the front axle turns opposite to the body yaw direction). + CHECK(axle_vel.front_axle_turning_velocity_rad_s == Approx(angular_velocity).margin(1e-6)); +} + +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - stationary (0, 0) no NaN") +{ + // sqrt(omega^2 * Lf^2 + v^2) = 0 produces 0/0 in acos — guard must return zeros + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto result = model.bodyVelocityToVehicleState(0.0, 0.0); + + CHECK(result.articulation_angle_rad == Approx(0.0)); + CHECK(result.linear_velocity_m_s == Approx(0.0)); + CHECK(result.front_right_wheel_speed_rad_s == Approx(0.0)); + CHECK(result.front_left_wheel_speed_rad_s == Approx(0.0)); + CHECK(result.rear_right_wheel_speed_rad_s == Approx(0.0)); + CHECK(result.rear_left_wheel_speed_rad_s == Approx(0.0)); + CHECK(std::isinf(result.front_axle_turning_radius_m)); + CHECK(std::isinf(result.rear_axle_turning_radius_m)); +} + +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - straight line (v, 0) no NaN") +{ + // articulation_angle = 0, so sin(0) = 0 -> radii = inf -> inf * 0 = NaN wheel speeds without guard + ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); + + auto result = model.bodyVelocityToVehicleState(2.0, 0.0); + + CHECK(result.articulation_angle_rad == Approx(0.0)); + CHECK(result.linear_velocity_m_s == Approx(2.0)); + CHECK(result.front_right_wheel_speed_rad_s == Approx(2.0 / 0.4)); + CHECK(result.front_left_wheel_speed_rad_s == Approx(2.0 / 0.4)); + CHECK(result.rear_right_wheel_speed_rad_s == Approx(2.0 / 0.5)); + CHECK(result.rear_left_wheel_speed_rad_s == Approx(2.0 / 0.5)); + CHECK(std::isinf(result.front_axle_turning_radius_m)); + CHECK(std::isinf(result.rear_axle_turning_radius_m)); +} + +} // namespace polymath::kinematics diff --git a/test/test_bicycle_model.cpp b/test/test_bicycle_model.cpp new file mode 100644 index 0000000..2ca2278 --- /dev/null +++ b/test/test_bicycle_model.cpp @@ -0,0 +1,280 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "catch2_compat.hpp" +#include "polymath_kinematics/bicycle_model.hpp" + +namespace polymath::kinematics +{ + +TEST_CASE("BicycleModel construction") +{ + BicycleModel model(2.5, 1.5, 0.3); + CHECK(model.get_wheelbase_m() == 2.5); + CHECK(model.get_track_width_m() == 1.5); + CHECK(model.get_wheel_radius_m() == 0.3); +} + +TEST_CASE("BicycleModel steeringToBodyVelocity - straight line") +{ + BicycleModel model(2.5, 1.5, 0.3); + + auto result = model.steeringToBodyVelocity(5.0, 0.0); + + CHECK(result.linear_velocity_m_s == Approx(5.0)); + CHECK(result.angular_velocity_rad_s == Approx(0.0)); +} + +TEST_CASE("BicycleModel steeringToBodyVelocity - turning") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double steering_angle = M_PI / 4; // 45 degrees + auto result = model.steeringToBodyVelocity(5.0, steering_angle); + + CHECK(result.linear_velocity_m_s == Approx(5.0)); + // angular_velocity = 5.0 * tan(45) / 2.5 = 5.0 * 1.0 / 2.5 = 2.0 rad/s + CHECK(result.angular_velocity_rad_s == Approx(2.0)); +} + +TEST_CASE("BicycleModel steeringToBodyVelocity - right turn") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double steering_angle = -M_PI / 4; // -45 degrees (right turn) + auto result = model.steeringToBodyVelocity(5.0, steering_angle); + + CHECK(result.linear_velocity_m_s == Approx(5.0)); + CHECK(result.angular_velocity_rad_s == Approx(-2.0)); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - straight line") +{ + BicycleModel model(2.5, 1.5, 0.3); + + auto result = model.bodyVelocityToSteering(5.0, 0.0); + + CHECK(result.velocity_m_s == Approx(5.0)); + CHECK(result.steering_angle_rad == Approx(0.0)); + CHECK(std::isinf(result.turning_radius_m)); + + // All wheels should have the same angular velocity for straight line + double expected_wheel_vel = 5.0 / 0.3; // linear_vel / wheel_radius + CHECK(result.front_left_wheel_rad_s == Approx(expected_wheel_vel)); + CHECK(result.front_right_wheel_rad_s == Approx(expected_wheel_vel)); + CHECK(result.rear_left_wheel_rad_s == Approx(expected_wheel_vel)); + CHECK(result.rear_right_wheel_rad_s == Approx(expected_wheel_vel)); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - turning") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // angular_velocity = 2.0 rad/s, linear_velocity = 5.0 m/s + // steering_angle = atan(2.0 * 2.5 / 5.0) = atan(1.0) = pi/4 + auto result = model.bodyVelocityToSteering(5.0, 2.0); + + CHECK(result.velocity_m_s == Approx(5.0)); + CHECK(result.steering_angle_rad == Approx(M_PI / 4)); + + // turning_radius = wheelbase / tan(steering_angle) = 2.5 / 1.0 = 2.5 + CHECK(result.turning_radius_m == Approx(2.5)); + + // For left turn (positive angular velocity), inner wheels (left) should be slower + CHECK(result.rear_left_wheel_rad_s < result.rear_right_wheel_rad_s); + CHECK(result.front_left_wheel_rad_s < result.front_right_wheel_rad_s); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - zero linear velocity") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Zero linear velocity with nonzero angular: degenerate for bicycle model + auto result = model.bodyVelocityToSteering(0.0, 1.0); + + CHECK(result.velocity_m_s == Approx(0.0)); + CHECK(result.steering_angle_rad == Approx(0.0)); + CHECK(std::isinf(result.turning_radius_m)); + CHECK(result.front_left_wheel_rad_s == Approx(0.0)); + CHECK(result.front_right_wheel_rad_s == Approx(0.0)); + CHECK(result.rear_left_wheel_rad_s == Approx(0.0)); + CHECK(result.rear_right_wheel_rad_s == Approx(0.0)); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - stationary") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Both inputs zero: fully stationary + auto result = model.bodyVelocityToSteering(0.0, 0.0); + + CHECK(result.velocity_m_s == Approx(0.0)); + CHECK(result.steering_angle_rad == Approx(0.0)); + CHECK(std::isinf(result.turning_radius_m)); + CHECK(result.front_left_wheel_rad_s == Approx(0.0)); + CHECK(result.front_right_wheel_rad_s == Approx(0.0)); + CHECK(result.rear_left_wheel_rad_s == Approx(0.0)); + CHECK(result.rear_right_wheel_rad_s == Approx(0.0)); +} + +TEST_CASE("BicycleModel turningRadius") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Straight line + CHECK(std::isinf(model.turningRadius(0.0))); + + // 45 degree turn: radius = wheelbase / tan(pi/4) = 2.5 / 1.0 = 2.5 + CHECK(model.turningRadius(M_PI / 4) == Approx(2.5)); +} + +TEST_CASE("BicycleModel steeringAngleFromRadius") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Infinite radius -> straight line + CHECK(model.steeringAngleFromRadius(std::numeric_limits::infinity()) == Approx(0.0)); + + // radius = wheelbase -> 45 degree steering + CHECK(model.steeringAngleFromRadius(2.5) == Approx(M_PI / 4)); +} + +TEST_CASE("BicycleModel roundtrip") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double velocity = 3.0; + double steering_angle = 0.3; + + auto body_vel = model.steeringToBodyVelocity(velocity, steering_angle); + auto steering = model.bodyVelocityToSteering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s); + + CHECK(steering.velocity_m_s == Approx(velocity)); + CHECK(steering.steering_angle_rad == Approx(steering_angle)); +} + +TEST_CASE("BicycleModel turningRadius/steeringAngleFromRadius roundtrip") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double steering_angle = 0.5; + + double radius = model.turningRadius(steering_angle); + double recovered_angle = model.steeringAngleFromRadius(radius); + + CHECK(recovered_angle == Approx(steering_angle)); +} + +TEST_CASE("BicycleModel steeringAngleFromRadius - negative radius") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Negative radius (right turn) should give negative steering angle + // radius = -2.5 -> steering = atan(2.5 / -2.5) = atan(-1) = -pi/4 + CHECK(model.steeringAngleFromRadius(-2.5) == Approx(-M_PI / 4)); +} + +TEST_CASE("BicycleModel turningRadius/steeringAngleFromRadius roundtrip - negative radius") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double steering_angle = -0.5; // Right turn + + double radius = model.turningRadius(steering_angle); + CHECK(radius < 0.0); // Negative radius for right turn + + double recovered_angle = model.steeringAngleFromRadius(radius); + CHECK(recovered_angle == Approx(steering_angle)); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - right turn") +{ + BicycleModel model(2.5, 1.5, 0.3); + + // Negative angular velocity -> right turn + auto result = model.bodyVelocityToSteering(5.0, -2.0); + + CHECK(result.velocity_m_s == Approx(5.0)); + CHECK(result.steering_angle_rad == Approx(-M_PI / 4)); + CHECK(result.turning_radius_m == Approx(-2.5)); + + // For right turn with forward motion, inner wheels (right) should be slower + CHECK(result.rear_right_wheel_rad_s < result.rear_left_wheel_rad_s); + CHECK(result.front_right_wheel_rad_s < result.front_left_wheel_rad_s); + + // All wheel speeds should be positive (vehicle moving forward) + CHECK(result.rear_left_wheel_rad_s > 0.0); + CHECK(result.rear_right_wheel_rad_s > 0.0); + CHECK(result.front_left_wheel_rad_s > 0.0); + CHECK(result.front_right_wheel_rad_s > 0.0); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - wheel speed values") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double v = 5.0; + double omega = 2.0; + + auto result = model.bodyVelocityToSteering(v, omega); + + double R = result.turning_radius_m; // 2.5 + double half_track = 0.75; + + // Rear wheels: omega * (R +/- half_track) / wheel_radius + CHECK(result.rear_left_wheel_rad_s == Approx(omega * (R - half_track) / 0.3)); + CHECK(result.rear_right_wheel_rad_s == Approx(omega * (R + half_track) / 0.3)); + + // Front wheels: omega * hypot(R +/- half_track, wheelbase) / wheel_radius + double front_left_dist = std::hypot(R - half_track, 2.5); + double front_right_dist = std::hypot(R + half_track, 2.5); + CHECK(result.front_left_wheel_rad_s == Approx(omega * front_left_dist / 0.3)); + CHECK(result.front_right_wheel_rad_s == Approx(omega * front_right_dist / 0.3)); +} + +TEST_CASE("BicycleModel bodyVelocityToSteering - reverse straight") +{ + BicycleModel model(2.5, 1.5, 0.3); + + auto result = model.bodyVelocityToSteering(-5.0, 0.0); + + CHECK(result.velocity_m_s == Approx(-5.0)); + CHECK(result.steering_angle_rad == Approx(0.0)); + + // All wheel speeds should be negative (backward rolling) + double expected_wheel_vel = -5.0 / 0.3; + CHECK(result.front_left_wheel_rad_s == Approx(expected_wheel_vel)); + CHECK(result.front_right_wheel_rad_s == Approx(expected_wheel_vel)); + CHECK(result.rear_left_wheel_rad_s == Approx(expected_wheel_vel)); + CHECK(result.rear_right_wheel_rad_s == Approx(expected_wheel_vel)); +} + +TEST_CASE("BicycleModel roundtrip - negative steering angle") +{ + BicycleModel model(2.5, 1.5, 0.3); + + double velocity = 3.0; + double steering_angle = -0.3; // Right turn + + auto body_vel = model.steeringToBodyVelocity(velocity, steering_angle); + auto steering = model.bodyVelocityToSteering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s); + + CHECK(steering.velocity_m_s == Approx(velocity)); + CHECK(steering.steering_angle_rad == Approx(steering_angle)); +} + +} // namespace polymath::kinematics diff --git a/test/test_differential_drive.cpp b/test/test_differential_drive.cpp new file mode 100644 index 0000000..0163b31 --- /dev/null +++ b/test/test_differential_drive.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "catch2_compat.hpp" +#include "polymath_kinematics/differential_drive_model.hpp" + +namespace polymath::kinematics +{ + +TEST_CASE("DifferentialDriveModel construction") +{ + DifferentialDriveModel model(0.1, 0.5); + CHECK(model.get_wheel_radius_m() == 0.1); + CHECK(model.get_track_width_m() == 0.5); +} + +TEST_CASE("DifferentialDriveModel wheelVelocitiesToBodyVelocity - straight line") +{ + DifferentialDriveModel model(0.1, 0.5); // 0.1m wheel radius, 0.5m track width + + // Both wheels spinning at same rate -> straight line motion + auto result = model.wheelVelocitiesToBodyVelocity(10.0, 10.0); + + // Linear velocity = wheel_radius * wheel_vel = 0.1 * 10 = 1.0 m/s + CHECK(result.linear_velocity_m_s == Approx(1.0)); + // Angular velocity = 0 when both wheels spin at same rate + CHECK(result.angular_velocity_rad_s == Approx(0.0)); +} + +TEST_CASE("DifferentialDriveModel wheelVelocitiesToBodyVelocity - turning") +{ + DifferentialDriveModel model(0.1, 0.5); + + // Right wheel spinning faster -> turning left (positive angular velocity) + auto result = model.wheelVelocitiesToBodyVelocity(5.0, 15.0); + + // Linear velocity = (0.1 * 5 + 0.1 * 15) / 2 = (0.5 + 1.5) / 2 = 1.0 m/s + CHECK(result.linear_velocity_m_s == Approx(1.0)); + // Angular velocity = (1.5 - 0.5) / 0.5 = 2.0 rad/s + CHECK(result.angular_velocity_rad_s == Approx(2.0)); +} + +TEST_CASE("DifferentialDriveModel wheelVelocitiesToBodyVelocity - spinning in place") +{ + DifferentialDriveModel model(0.1, 0.5); + + // Wheels spinning opposite directions -> spin in place + auto result = model.wheelVelocitiesToBodyVelocity(-10.0, 10.0); + + // Linear velocity = (-1.0 + 1.0) / 2 = 0 m/s + CHECK(result.linear_velocity_m_s == Approx(0.0)); + // Angular velocity = (1.0 - (-1.0)) / 0.5 = 4.0 rad/s + CHECK(result.angular_velocity_rad_s == Approx(4.0)); +} + +TEST_CASE("DifferentialDriveModel bodyVelocityToWheelVelocities - straight line") +{ + DifferentialDriveModel model(0.1, 0.5); + + // Straight line at 1 m/s + auto result = model.bodyVelocityToWheelVelocities(1.0, 0.0); + + // Both wheels should have same angular velocity + // wheel_vel = linear_vel / wheel_radius = 1.0 / 0.1 = 10 rad/s + CHECK(result.left_wheel_velocity_rad_s == Approx(10.0)); + CHECK(result.right_wheel_velocity_rad_s == Approx(10.0)); +} + +TEST_CASE("DifferentialDriveModel bodyVelocityToWheelVelocities - turning") +{ + DifferentialDriveModel model(0.1, 0.5); + + // 1 m/s forward with 2 rad/s angular velocity + auto result = model.bodyVelocityToWheelVelocities(1.0, 2.0); + + // left_linear = 1.0 - 2.0 * 0.25 = 0.5 m/s -> 5 rad/s + CHECK(result.left_wheel_velocity_rad_s == Approx(5.0)); + // right_linear = 1.0 + 2.0 * 0.25 = 1.5 m/s -> 15 rad/s + CHECK(result.right_wheel_velocity_rad_s == Approx(15.0)); +} + +TEST_CASE("DifferentialDriveModel roundtrip") +{ + DifferentialDriveModel model(0.1, 0.5); + + double linear = 2.5; + double angular = 1.0; + + auto wheel_state = model.bodyVelocityToWheelVelocities(linear, angular); + auto velocities = + model.wheelVelocitiesToBodyVelocity(wheel_state.left_wheel_velocity_rad_s, wheel_state.right_wheel_velocity_rad_s); + + CHECK(velocities.linear_velocity_m_s == Approx(linear)); + CHECK(velocities.angular_velocity_rad_s == Approx(angular)); +} + +} // namespace polymath::kinematics diff --git a/test/test_python_bindings.py b/test/test_python_bindings.py new file mode 100644 index 0000000..0123958 --- /dev/null +++ b/test/test_python_bindings.py @@ -0,0 +1,109 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +import pytest + +from polymath_kinematics import ( + ArticulatedModel, + BicycleModel, + DifferentialDriveModel, +) + + +class TestDifferentialDriveModel: + def test_construction(self): + model = DifferentialDriveModel(0.1, 0.5) + assert model.wheel_radius == pytest.approx(0.1) + assert model.track_width == pytest.approx(0.5) + + def test_turning(self): + model = DifferentialDriveModel(0.1, 0.5) + result = model.wheel_velocities_to_body_velocity(5.0, 15.0) + + assert result.linear_velocity_m_s == pytest.approx(1.0) + assert result.angular_velocity_rad_s == pytest.approx(2.0) + + def test_roundtrip(self): + model = DifferentialDriveModel(0.1, 0.5) + linear = 2.5 + angular = 1.0 + + wheel_state = model.body_velocity_to_wheel_velocities(linear, angular) + velocities = model.wheel_velocities_to_body_velocity( + wheel_state.left_wheel_velocity_rad_s, + wheel_state.right_wheel_velocity_rad_s, + ) + + assert velocities.linear_velocity_m_s == pytest.approx(linear) + assert velocities.angular_velocity_rad_s == pytest.approx(angular) + + +class TestBicycleModel: + def test_construction(self): + model = BicycleModel(2.5, 1.5, 0.3) + assert model.wheelbase == pytest.approx(2.5) + assert model.track_width == pytest.approx(1.5) + assert model.wheel_radius == pytest.approx(0.3) + + def test_turning(self): + model = BicycleModel(2.5, 1.5, 0.3) + result = model.body_velocity_to_steering(5.0, 2.0) + + assert result.velocity_m_s == pytest.approx(5.0) + assert result.steering_angle_rad == pytest.approx(math.pi / 4) + assert result.turning_radius_m == pytest.approx(2.5) + + def test_steering_angle_from_radius_negative(self): + model = BicycleModel(2.5, 1.5, 0.3) + angle = model.steering_angle_from_radius(-2.5) + assert angle == pytest.approx(-math.pi / 4) + + def test_roundtrip(self): + model = BicycleModel(2.5, 1.5, 0.3) + velocity = 3.0 + steering_angle = -0.3 + + body_vel = model.steering_to_body_velocity(velocity, steering_angle) + steering = model.body_velocity_to_steering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s) + + assert steering.velocity_m_s == pytest.approx(velocity) + assert steering.steering_angle_rad == pytest.approx(steering_angle) + + +class TestArticulatedModel: + def test_construction(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + assert model.articulation_to_front_axle == pytest.approx(1.5) + assert model.articulation_to_rear_axle == pytest.approx(1.2) + assert model.front_track_width == pytest.approx(1.8) + assert model.rear_track_width == pytest.approx(1.6) + assert model.front_wheel_radius == pytest.approx(0.4) + assert model.rear_wheel_radius == pytest.approx(0.5) + + def test_turning(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + result = model.body_velocity_to_vehicle_state(2.0, 0.5) + + assert result.linear_velocity_m_s == pytest.approx(2.0) + assert result.articulation_angle_rad == pytest.approx(0.6435011088) + assert result.front_axle_turning_radius_m == pytest.approx(4.0) + + def test_axle_velocities(self): + model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5) + result = model.articulation_to_axle_velocities(2.0, 0.3) + + assert result.linear_velocity_m_s == pytest.approx(2.0) + assert result.front_axle_turning_velocity_rad_s == pytest.approx(0.2244737375) From 142eb740d73b6825fc8b739709f35d6f6a64f2f8 Mon Sep 17 00:00:00 2001 From: Zeerek Ahmad Date: Mon, 16 Mar 2026 07:39:40 +0000 Subject: [PATCH 2/2] Fix missing depend --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index f37095d..0019e51 100644 --- a/package.xml +++ b/package.xml @@ -13,6 +13,7 @@ pybind11_vendor ament_cmake_pytest + catch2 ament_cmake