diff --git a/.github/actions/pregen/action.yml b/.github/actions/pregen/action.yml new file mode 100644 index 00000000000..2c61be413ac --- /dev/null +++ b/.github/actions/pregen/action.yml @@ -0,0 +1,61 @@ +name: 'Setup and run pregeneration' +description: 'Sets up the dependencies needed to generate generated files and runs all generation scripts' + +runs: + using: "composite" + steps: + - name: Set up Python 3.12 + uses: actions/setup-python@v5 + with: + python-version: '3.12' + - name: Install jinja and protobuf + run: python -m pip install jinja2 protobuf grpcio-tools + shell: bash + - name: Install protobuf dependencies + run: | + sudo apt-get update + sudo apt-get install -y protobuf-compiler + wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe + chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe + shell: bash + - name: Regenerate hal + run: ./hal/generate_usage_reporting.py + shell: bash + + - name: Regenerate ntcore + run: ./ntcore/generate_topics.py + shell: bash + + - name: Regenerate imgui + run: | + ./thirdparty/imgui_suite/generate_fonts.sh + ./thirdparty/imgui_suite/generate_gl3w.py + shell: bash + + - name: Regenerate HIDs + run: | + ./wpilibc/generate_hids.py + ./wpilibj/generate_hids.py + ./wpilibNewCommands/generate_hids.py + shell: bash + + - name: Regenerate PWM motor controllers + run: | + ./wpilibc/generate_pwm_motor_controllers.py + ./wpilibj/generate_pwm_motor_controllers.py + shell: bash + + - name: Regenerate wpimath + run: | + ./wpimath/generate_nanopb.py + ./wpimath/generate_numbers.py + ./wpimath/generate_quickbuf.py --quickbuf_plugin protoc-gen-quickbuf-1.3.3-linux-x86_64.exe + shell: bash + + - name: Regenerate wpiunits + run: ./wpiunits/generate_units.py + shell: bash + + - name: Regenerate wpiutil nanopb + run: ./wpiutil/generate_nanopb.py + shell: bash diff --git a/.github/labeler.yml b/.github/labeler.yml new file mode 100644 index 00000000000..f9ae9fa6dda --- /dev/null +++ b/.github/labeler.yml @@ -0,0 +1,56 @@ +'2027': +- base-branch: '2027' +'component: apriltag': +- changed-files: + - any-glob-to-any-file: apriltag/** +'component: command-based': +- changed-files: + - any-glob-to-any-file: wpilibNewCommands/** +'component: cscore': +- changed-files: + - any-glob-to-any-file: cscore/** +'component: datalogtool': +- changed-files: + - any-glob-to-any-file: datalogtool/** +'component: epilogue': +- changed-files: + - any-glob-to-any-file: epilogue-*/** +'component: examples': +- changed-files: + - any-glob-to-any-file: wpilib*Examples/** +'component: glass': +- changed-files: + - any-glob-to-any-file: glass/** +'component: hal': +- changed-files: + - any-glob-to-any-file: hal/** +'component: ntcore': +- changed-files: + - any-glob-to-any-file: ntcore/** +'component: outlineviewer': +- changed-files: + - any-glob-to-any-file: outlineviewer/** +'component: sysid': +- changed-files: + - any-glob-to-any-file: sysid/** +'component: teamnumbersetter': +- changed-files: + - any-glob-to-any-file: roborioteamnumbersetter/** +'component: wpilibc': +- changed-files: + - any-glob-to-any-file: wpilibc/** +'component: wpilibj': +- changed-files: + - any-glob-to-any-file: wpilibj/** +'component: wpimath': +- changed-files: + - any-glob-to-any-file: wpimath/** +'component: wpinet': +- changed-files: + - any-glob-to-any-file: wpinet/** +'component: wpiunits': +- changed-files: + - any-glob-to-any-file: wpiunits/** +'component: wpiutil': +- changed-files: + - any-glob-to-any-file: wpiutil/** diff --git a/.github/workflows/comment-command.yml b/.github/workflows/comment-command.yml deleted file mode 100644 index 4e733374873..00000000000 --- a/.github/workflows/comment-command.yml +++ /dev/null @@ -1,101 +0,0 @@ -name: Comment Commands -on: - issue_comment: - types: [ created ] - -jobs: - format: - if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/format') - runs-on: ubuntu-22.04 - steps: - - name: React Rocket - uses: actions/github-script@v7 - with: - script: | - const {owner, repo} = context.issue - github.rest.reactions.createForIssueComment({ - owner, - repo, - comment_id: context.payload.comment.id, - content: "rocket", - }); - - uses: actions/checkout@v4 - with: - fetch-depth: 0 - token: ${{ secrets.COMMENT_COMMAND_PAT_TOKEN }} - - name: Fetch all history and metadata - run: | - git checkout -b pr - git branch -f main origin/main - - name: Checkout PR - run: | - gh pr checkout $NUMBER - env: - GITHUB_TOKEN: "${{ secrets.COMMENT_COMMAND_PAT_TOKEN }}" - NUMBER: ${{ github.event.issue.number }} - - name: Set up Python 3.12 - uses: actions/setup-python@v5 - with: - python-version: '3.12' - - name: Setup Java - uses: actions/setup-java@v4 - with: - distribution: 'temurin' - java-version: 17 - - name: Install wpiformat - run: pip3 install wpiformat==2024.45 - - name: Run wpiformat - run: wpiformat - - name: Run spotlessApply - run: ./gradlew spotlessApply - - name: Commit - run: | - # Set credentials - git config user.name "github-actions[bot]" - git config user.email "41898282+github-actions[bot]@users.noreply.github.com" - # Commit - git commit -am "Formatting fixes" - git push - - pregen: - if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/pregen') - runs-on: ubuntu-22.04 - steps: - - name: React Rocket - uses: actions/github-script@v7 - with: - script: | - const {owner, repo} = context.issue - github.rest.reactions.createForIssueComment({ - owner, - repo, - comment_id: context.payload.comment.id, - content: "rocket", - }); - - uses: actions/checkout@v4 - with: - token: ${{ secrets.COMMENT_COMMAND_PAT_TOKEN }} - - name: Checkout PR - run: | - gh pr checkout $NUMBER - env: - GITHUB_TOKEN: "${{ secrets.COMMENT_COMMAND_PAT_TOKEN }}" - NUMBER: ${{ github.event.issue.number }} - - name: Set up Python 3.12 - uses: actions/setup-python@v5 - with: - python-version: '3.12' - - name: Install jinja - run: python -m pip install jinja2 - - name: Install protobuf dependencies - run: sudo apt-get update && sudo apt-get install -y protobuf-compiler && wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe && chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe - - name: Regenerate all - run: ./.github/workflows/pregen_all.py --quickbuf_plugin=protoc-gen-quickbuf-1.3.3-linux-x86_64.exe - - name: Commit - run: | - # Set credentials - git config user.name "github-actions[bot]" - git config user.email "41898282+github-actions[bot]@users.noreply.github.com" - # Commit - git commit -am "Regenerate pregenerated files" - git push diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index e6935cfba98..5ee9430ac00 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -15,10 +15,10 @@ jobs: - container: wpilib/roborio-cross-ubuntu:2025-22.04 artifact-name: Athena build-options: "-Ponlylinuxathena" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 artifact-name: Arm64 build-options: "-Ponlylinuxarm64" - container: wpilib/ubuntu-base:22.04 diff --git a/.github/workflows/labeler.yml b/.github/workflows/labeler.yml new file mode 100644 index 00000000000..e57cd86e2b3 --- /dev/null +++ b/.github/workflows/labeler.yml @@ -0,0 +1,12 @@ +name: "Pull Request Labeler" +on: +- pull_request_target + +jobs: + labeler: + permissions: + contents: read + pull-requests: write + runs-on: ubuntu-latest + steps: + - uses: actions/labeler@v5 diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index 8de7bd9c200..9b75d4ff895 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -27,7 +27,7 @@ jobs: with: python-version: '3.12' - name: Install wpiformat - run: pip3 install wpiformat==2024.45 + run: pip3 install wpiformat==2024.50 - name: Run run: wpiformat - name: Check output @@ -66,7 +66,7 @@ jobs: with: python-version: '3.12' - name: Install wpiformat - run: pip3 install wpiformat==2024.45 + run: pip3 install wpiformat==2024.50 - name: Create compile_commands.json run: | ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio diff --git a/.github/workflows/pregen_all.py b/.github/workflows/pregen_all.py deleted file mode 100755 index 278e5c84cd8..00000000000 --- a/.github/workflows/pregen_all.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 - -import argparse -import subprocess -import sys -from pathlib import Path - - -def main(): - script_path = Path(__file__).resolve() - REPO_ROOT = script_path.parent.parent.parent - parser = argparse.ArgumentParser() - parser.add_argument( - "--quickbuf_plugin", - help="Path to the quickbuf protoc plugin", - required=True, - ) - args = parser.parse_args() - subprocess.run( - [sys.executable, f"{REPO_ROOT}/hal/generate_usage_reporting.py"], check=True - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/ntcore/generate_topics.py"], check=True - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpimath/generate_numbers.py"], check=True - ) - subprocess.run( - [ - sys.executable, - f"{REPO_ROOT}/wpimath/generate_quickbuf.py", - f"--quickbuf_plugin={args.quickbuf_plugin}", - ], - check=True, - ) - subprocess.run( - [ - sys.executable, - f"{REPO_ROOT}/wpimath/generate_nanopb.py", - ], - check=True, - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpiunits/generate_units.py"], check=True - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpilibc/generate_hids.py"], check=True - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpilibj/generate_hids.py"], check=True - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpilibNewCommands/generate_hids.py"], check=True - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpilibc/generate_pwm_motor_controllers.py"], - check=True, - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/wpilibj/generate_pwm_motor_controllers.py"], - check=True, - ) - subprocess.run( - [ - sys.executable, - f"{REPO_ROOT}/wpiutil/generate_nanopb.py", - ], - check=True, - ) - subprocess.run( - [sys.executable, f"{REPO_ROOT}/thirdparty/imgui_suite/generate_gl3w.py"], - check=True, - ) - subprocess.run(f"{REPO_ROOT}/thirdparty/imgui_suite/generate_fonts.sh", check=True) - - -if __name__ == "__main__": - main() diff --git a/.github/workflows/pregenerate.yml b/.github/workflows/pregenerate.yml index 12e09da6a58..8bc6c9a28df 100644 --- a/.github/workflows/pregenerate.yml +++ b/.github/workflows/pregenerate.yml @@ -18,16 +18,8 @@ jobs: - uses: actions/checkout@v4 with: fetch-depth: 0 - - name: Set up Python 3.12 - uses: actions/setup-python@v5 - with: - python-version: '3.12' - - name: Install jinja and protobuf - run: python -m pip install jinja2 protobuf grpcio-tools - - name: Install protobuf dependencies - run: sudo apt-get update && sudo apt-get install -y protobuf-compiler && wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe && chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe - - name: Regenerate all - run: python ./.github/workflows/pregen_all.py --quickbuf_plugin protoc-gen-quickbuf-1.3.3-linux-x86_64.exe + - name: Run pregen + uses: ./.github/actions/pregen - name: Add untracked files to index so they count as changes run: git add -A - name: Check output diff --git a/.github/workflows/sentinel-build.yml b/.github/workflows/sentinel-build.yml index 9a8eb85184f..18adcf7675c 100644 --- a/.github/workflows/sentinel-build.yml +++ b/.github/workflows/sentinel-build.yml @@ -19,10 +19,10 @@ jobs: - container: wpilib/roborio-cross-ubuntu:2025-22.04 artifact-name: Athena build-options: "-Ponlylinuxathena" - - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 artifact-name: Arm64 build-options: "-Ponlylinuxarm64" - container: wpilib/ubuntu-base:22.04 diff --git a/WORKSPACE b/WORKSPACE index 602b2ed4297..0d7786c2a23 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -35,8 +35,8 @@ maven_install( # Download toolchains http_archive( name = "rules_bzlmodrio_toolchains", - sha256 = "2ef1cafce7f4fd4e909bb5de8b0dc771a934646afd55d5f100ff31f6b500df98", - url = "https://github.com/wpilibsuite/rules_bzlmodRio_toolchains/releases/download/2024-1.bcr1/rules_bzlmodRio_toolchains-2024-1.bcr1.tar.gz", + sha256 = "fe267e2af53c1def1e962700a9aeda9e8fdfa9fb46b72167c615ec0e25447dd6", + url = "https://github.com/wpilibsuite/rules_bzlmodRio_toolchains/releases/download/2025-1/rules_bzlmodRio_toolchains-2025-1.tar.gz", ) load("@rules_bzlmodrio_toolchains//:maven_deps.bzl", "setup_legacy_setup_toolchains_dependencies") @@ -71,6 +71,12 @@ register_toolchains( "@local_bullseye_64//:macos", "@local_bullseye_64//:linux", "@local_bullseye_64//:windows", + "@local_bookworm_32//:macos", + "@local_bookworm_32//:linux", + "@local_bookworm_32//:windows", + "@local_bookworm_64//:macos", + "@local_bookworm_64//:linux", + "@local_bookworm_64//:windows", ) setup_legacy_setup_jdk_dependencies() @@ -87,8 +93,8 @@ setup_legacy_bzlmodrio_ni_cpp_dependencies() http_archive( name = "bzlmodrio-opencv", - sha256 = "5314cce05b49451a46bf3e3140fc401342e53d5f3357612ed4473e59bb616cba", - url = "https://github.com/wpilibsuite/bzlmodRio-opencv/releases/download/2024.4.8.0-4.bcr1/bzlmodRio-opencv-2024.4.8.0-4.bcr1.tar.gz", + sha256 = "4f4a607956ca8555618736c3058dd96e09d02df19e95088c1e352d2319fd70c7", + url = "https://github.com/wpilibsuite/bzlmodRio-opencv/releases/download/2025.4.10.0-2/bzlmodRio-opencv-2025.4.10.0-2.tar.gz", ) load("@bzlmodrio-opencv//:maven_cpp_deps.bzl", "setup_legacy_bzlmodrio_opencv_cpp_dependencies") @@ -98,3 +104,16 @@ setup_legacy_bzlmodrio_opencv_cpp_dependencies() load("@bzlmodrio-opencv//:maven_java_deps.bzl", "setup_legacy_bzlmodrio_opencv_java_dependencies") setup_legacy_bzlmodrio_opencv_java_dependencies() + +http_archive( + name = "build_bazel_apple_support", + sha256 = "c4bb2b7367c484382300aee75be598b92f847896fb31bbd22f3a2346adf66a80", + url = "https://github.com/bazelbuild/apple_support/releases/download/1.15.1/apple_support.1.15.1.tar.gz", +) + +load( + "@build_bazel_apple_support//lib:repositories.bzl", + "apple_support_dependencies", +) + +apple_support_dependencies() diff --git a/apriltag/BUILD.bazel b/apriltag/BUILD.bazel new file mode 100644 index 00000000000..ea1ef1abe00 --- /dev/null +++ b/apriltag/BUILD.bazel @@ -0,0 +1,112 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") +load("@rules_java//java:defs.bzl", "java_binary", "java_library") +load("@rules_python//python:defs.bzl", "py_binary") +load("//shared/bazel/rules/gen:gen-resources.bzl", "generate_resources") + +cc_library( + name = "thirdparty-apriltag", + srcs = glob(["src/main/native/thirdparty/apriltag/src/**"]), + hdrs = glob(["src/main/native/thirdparty/apriltag/include/**"]), + copts = select({ + "@bazel_tools//src/conditions:darwin": [ + "-Wno-format-nonliteral", + "-Wno-gnu-zero-variadic-macro-arguments", + "-Wno-uninitialized", + "-Wno-sign-compare", + "-Wno-type-limits", + ], + "@bazel_tools//src/conditions:windows": [ + "/wd4005", + "/wd4018", + "/wd4244", + "/wd4267", + "/wd4996", + ], + "@rules_bzlmodrio_toolchains//constraints/combined:is_linux": [ + "-Wno-format-nonliteral", + "-Wno-maybe-uninitialized", + "-Wno-sign-compare", + "-Wno-type-limits", + ], + }), + includes = ["src/main/native/thirdparty/apriltag/include/common"], + strip_include_prefix = "src/main/native/thirdparty/apriltag/include", + visibility = ["//visibility:public"], +) + +generate_resources( + name = "generate-resources", + namespace = "frc", + prefix = "APRILTAG", + resource_files = glob(["src/main/native/resources/**"]), + visibility = ["//visibility:public"], +) + +cc_library( + name = "apriltag.static", + srcs = [":generate-resources"] + glob( + ["src/main/native/cpp/**"], + exclude = ["src/main/native/cpp/jni/**"], + ), + hdrs = glob(["src/main/native/include/**/*"]), + defines = ["WPILIB_EXPORTS"], + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + ":thirdparty-apriltag", + "//wpimath:wpimath.static", + "//wpiutil:wpiutil.static", + ], +) + +java_library( + name = "apriltag-java", + srcs = glob(["src/main/java/**/*.java"]), + resource_strip_prefix = "apriltag/src/main/native/resources", + resources = glob(["src/main/native/resources/**"]), + visibility = ["//visibility:public"], + deps = [ + "//wpimath:wpimath-java", + "//wpiutil:wpiutil-java", + "@bzlmodrio-opencv//libraries/java/opencv", + "@maven//:com_fasterxml_jackson_core_jackson_annotations", + "@maven//:com_fasterxml_jackson_core_jackson_core", + "@maven//:com_fasterxml_jackson_core_jackson_databind", + ], +) + +cc_test( + name = "apriltag-cpp-test", + size = "small", + srcs = glob(["src/test/native/cpp/**"]), + tags = [ + "no-asan", + ], + deps = [ + ":apriltag.static", + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + ":apriltag.static", + ], +) + +java_binary( + name = "DevMain-Java", + srcs = ["src/dev/java/edu/wpi/first/apriltag/DevMain.java"], + main_class = "edu.wpi.first.apriltag.DevMain", + deps = [ + ":apriltag-java", + ], +) + +py_binary( + name = "convert_apriltag_layouts", + srcs = ["convert_apriltag_layouts.py"], + tags = ["manual"], +) diff --git a/build.gradle b/build.gradle index 8a51182b787..2db70836038 100644 --- a/build.gradle +++ b/build.gradle @@ -18,7 +18,7 @@ plugins { id 'idea' id 'visual-studio' id 'net.ltgt.errorprone' version '3.1.0' apply false - id 'com.github.johnrengelman.shadow' version '8.1.1' apply false + id 'com.gradleup.shadow' version '8.3.4' apply false id 'com.diffplug.spotless' version '6.20.0' apply false id 'com.github.spotbugs' version '6.0.2' apply false } diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle index 5c555c1c53a..8a0ad0c1a91 100644 --- a/buildSrc/build.gradle +++ b/buildSrc/build.gradle @@ -9,5 +9,5 @@ repositories { } } dependencies { - implementation "edu.wpi.first:native-utils:2025.7.1" + implementation "edu.wpi.first:native-utils:2025.9.0" } diff --git a/cameraserver/BUILD.bazel b/cameraserver/BUILD.bazel index 542f0f3c961..73995c01353 100644 --- a/cameraserver/BUILD.bazel +++ b/cameraserver/BUILD.bazel @@ -1,6 +1,22 @@ -load("@rules_cc//cc:defs.bzl", "cc_binary") +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") load("@rules_java//java:defs.bzl", "java_binary", "java_library") +cc_library( + name = "cameraserver.static", + srcs = glob(["src/main/native/cpp/**"]), + hdrs = glob(["src/main/native/include/**/*"]), + includes = [ + "cpp", + "src/main/native/include", + ], + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + "//cscore:cscore.static", + "//ntcore:ntcore.static", + ], +) + java_library( name = "cameraserver-java", srcs = glob(["src/main/java/**/*.java"]), @@ -16,10 +32,21 @@ java_library( ], ) +cc_test( + name = "cameraserver-cpp-test", + size = "small", + srcs = glob(["src/test/native/**"]), + deps = [ + ":cameraserver.static", + "//thirdparty/googletest:googletest.static", + ], +) + cc_binary( name = "DevMain-Cpp", srcs = ["src/dev/native/cpp/main.cpp"], deps = [ + ":cameraserver.static", ], ) diff --git a/cameraserver/multiCameraServer/build.gradle b/cameraserver/multiCameraServer/build.gradle index d13637c7110..377bdbcf3c5 100644 --- a/cameraserver/multiCameraServer/build.gradle +++ b/cameraserver/multiCameraServer/build.gradle @@ -22,7 +22,7 @@ application { mainClass = 'edu.wpi.Main' } -apply plugin: 'com.github.johnrengelman.shadow' +apply plugin: 'com.gradleup.shadow' repositories { maven { diff --git a/cscore/BUILD.bazel b/cscore/BUILD.bazel index a2470f8afe1..d40fb4d2fd9 100644 --- a/cscore/BUILD.bazel +++ b/cscore/BUILD.bazel @@ -1,5 +1,76 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test", "objc_library") load("@rules_java//java:defs.bzl", "java_binary", "java_library") +WIN_SRCS = glob([ + "src/main/native/windows/**/*.cpp", + "src/main/native/windows/**/*.h", +]) + +LINUX_SRCS = glob([ + "src/main/native/linux/**/*.cpp", + "src/main/native/linux/**/*.h", +]) + +MAC_SRCS = glob(["src/main/native/osx/**/*.cpp"]) + +filegroup( + name = "native-srcs", + srcs = select({ + "@bazel_tools//src/conditions:darwin": MAC_SRCS, + "@bazel_tools//src/conditions:windows": WIN_SRCS, + "@rules_bzlmodrio_toolchains//constraints/combined:is_linux": LINUX_SRCS, + }), +) + +objc_library( + name = "cscore-mac", + srcs = glob([ + "src/main/native/objcpp/**/*.mm", + "src/main/native/cpp/*.h", + ]), + hdrs = glob([ + "src/main/native/include/**/*", + "src/main/native/objcpp/**/*.h", + ]), + copts = [ + "-std=c++20", + ], + includes = [ + "src/main/native/cpp", + "src/main/native/include", + "src/main/native/objcpp", + ], + tags = ["manual"], + deps = [ + "//wpinet:wpinet.static", + "//wpiutil:wpiutil.static", + "@bzlmodrio-opencv//libraries/cpp/opencv", + ], +) + +cc_library( + name = "cscore.static", + srcs = [":native-srcs"] + glob( + ["src/main/native/cpp/**"], + exclude = ["src/main/native/cpp/jni/**"], + ), + hdrs = glob(["src/main/native/include/**/*"]), + includes = [ + "src/main/native/cpp", + "src/main/native/include", + ], + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + "//wpinet:wpinet.static", + "//wpiutil:wpiutil.static", + "@bzlmodrio-opencv//libraries/cpp/opencv", + ] + select({ + "@bazel_tools//src/conditions:darwin": [":cscore-mac"], + "//conditions:default": [], + }), +) + java_library( name = "cscore-java", srcs = glob(["src/main/java/**/*.java"]), @@ -10,6 +81,24 @@ java_library( ], ) +cc_test( + name = "cscore-cpp-test", + size = "small", + srcs = glob(["src/test/native/**"]), + deps = [ + ":cscore.static", + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + ":cscore.static", + ], +) + java_binary( name = "DevMain-Java", srcs = ["src/dev/java/edu/wpi/first/cscore/DevMain.java"], diff --git a/developerRobot/CMakeLists.txt b/developerRobot/CMakeLists.txt index f942e01bb08..3b2b9827a54 100644 --- a/developerRobot/CMakeLists.txt +++ b/developerRobot/CMakeLists.txt @@ -5,4 +5,4 @@ include(CompileWarnings) file(GLOB developerRobotCpp_src src/main/native/cpp/*.cpp) add_executable(developerRobotCpp ${developerRobotCpp_src}) -target_link_libraries(developerRobotCpp wpilibc) +target_link_libraries(developerRobotCpp wpilibc wpilibNewCommands apriltag) diff --git a/developerRobot/README.md b/developerRobot/README.md index 8e011314406..2b332f8afe2 100644 --- a/developerRobot/README.md +++ b/developerRobot/README.md @@ -9,7 +9,12 @@ This command builds everything. ## Simulation -This command runs the C++ subproject on desktop. +This command runs the Java project on desktop. +```bash +./gradlew developerRobot:run +``` + +This command runs the C++ project on desktop. ```bash ./gradlew developerRobot:runCpp ``` diff --git a/developerRobot/build.gradle b/developerRobot/build.gradle index 684d0ae63d4..efd30cf8e11 100644 --- a/developerRobot/build.gradle +++ b/developerRobot/build.gradle @@ -35,7 +35,7 @@ application { mainClass = 'frc.robot.Main' } -apply plugin: 'com.github.johnrengelman.shadow' +apply plugin: 'com.gradleup.shadow' repositories { maven { @@ -54,6 +54,9 @@ dependencies { implementation project(':cameraserver') implementation project(':wpilibNewCommands') implementation project(':apriltag') + implementation project(':wpiunits') + implementation project(':epilogue-runtime') + annotationProcessor project(':epilogue-processor') } tasks.withType(com.github.spotbugs.snom.SpotBugsTask).configureEach { @@ -139,6 +142,19 @@ deploy { } } +// Prevent the eclipse compiler (used by the VS Code extension for intellisense and debugging) +// from generating bad class files from annotation processors like Epilogue +eclipse { + classpath { + containers 'org.eclipse.buildship.core.gradleclasspathcontainer' + file.whenMerged { cp -> + def entries = cp.entries; + def src = new org.gradle.plugins.ide.eclipse.model.SourceFolder('build/generated/sources/annotationProcessor/java/main/', null) + entries.add(src) + } + } +} + tasks.register('deployJava') { try { dependsOn tasks.named('deployjreroborio') diff --git a/epilogue-processor/BUILD.bazel b/epilogue-processor/BUILD.bazel new file mode 100644 index 00000000000..a9a8e083ab4 --- /dev/null +++ b/epilogue-processor/BUILD.bazel @@ -0,0 +1,22 @@ +load("@rules_java//java:defs.bzl", "java_library", "java_plugin") + +java_library( + name = "processor", + srcs = glob(["src/main/java/**/*.java"]), + visibility = ["//visibility:public"], + runtime_deps = [ + "//wpilibNewCommands:wpilibNewCommands-java", + ], + deps = [ + "//epilogue-runtime:epilogue", + ], +) + +java_plugin( + name = "plugin", + processor_class = "edu.wpi.first.epilogue.processor.AnnotationProcessor", + visibility = ["//visibility:public"], + deps = [ + ":processor", + ], +) diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java index 22f0e894595..2781c055c8c 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java @@ -11,9 +11,12 @@ import java.util.ArrayList; import java.util.Comparator; import java.util.HashMap; +import java.util.LinkedHashSet; import java.util.List; import java.util.Map; import java.util.Set; +import java.util.stream.Collectors; +import java.util.stream.Stream; import javax.annotation.processing.AbstractProcessor; import javax.annotation.processing.RoundEnvironment; import javax.annotation.processing.SupportedAnnotationTypes; @@ -90,15 +93,15 @@ public boolean process(Set annotations, RoundEnvironment e); }); + var loggedTypes = getLoggedTypes(roundEnv); + // Handlers are declared in order of priority. If an element could be logged in more than one // way (eg a class implements both Sendable and StructSerializable), the order of the handlers // in this list will determine how it gets logged. m_handlers = List.of( new LoggableHandler( - processingEnv, - roundEnv.getElementsAnnotatedWith( - Logged.class)), // prioritize epilogue logging over Sendable + processingEnv, loggedTypes), // prioritize epilogue logging over Sendable new ConfiguredLoggerHandler( processingEnv, customLoggers), // then customized logging configs new ArrayHandler(processingEnv), @@ -118,12 +121,39 @@ public boolean process(Set annotations, RoundEnvironment .findAny() .ifPresent( epilogue -> { - processEpilogue(roundEnv, epilogue); + processEpilogue(roundEnv, epilogue, loggedTypes); }); return false; } + /** + * Gets the set of all loggable types in the compilation unit. A type is considered loggable if it + * is directly annotated with {@code @Logged} or contains a field or method with a {@code @Logged} + * annotation. + * + * @param roundEnv the compilation round environment + * @return the set of all loggable types + */ + private Set getLoggedTypes(RoundEnvironment roundEnv) { + // Fetches everything annotated with @Logged; classes, methods, values, etc. + var annotatedElements = roundEnv.getElementsAnnotatedWith(Logged.class); + return Stream.concat( + // 1. All type elements (classes, interfaces, or enums) with the @Logged annotation + annotatedElements.stream() + .filter(e -> e instanceof TypeElement) + .map(e -> (TypeElement) e), + // 2. All type elements containing a field or method with the @Logged annotation + annotatedElements.stream() + .filter(e -> e instanceof VariableElement || e instanceof ExecutableElement) + .map(Element::getEnclosingElement) + .filter(e -> e instanceof TypeElement) + .map(e -> (TypeElement) e)) + .sorted(Comparator.comparing(e -> e.getSimpleName().toString())) + .collect( + Collectors.toCollection(LinkedHashSet::new)); // Collect to a set to avoid duplicates + } + private boolean validateFields(Set annotatedElements) { var fields = annotatedElements.stream() @@ -340,7 +370,8 @@ private Map processCustomLoggers( return customLoggers; } - private void processEpilogue(RoundEnvironment roundEnv, TypeElement epilogueAnnotation) { + private void processEpilogue( + RoundEnvironment roundEnv, TypeElement epilogueAnnotation, Set loggedTypes) { var annotatedElements = roundEnv.getElementsAnnotatedWith(epilogueAnnotation); List loggerClassNames = new ArrayList<>(); @@ -358,12 +389,7 @@ private void processEpilogue(RoundEnvironment roundEnv, TypeElement epilogueAnno return; } - var classes = - annotatedElements.stream() - .filter(e -> e instanceof TypeElement) - .map(e -> (TypeElement) e) - .toList(); - for (TypeElement clazz : classes) { + for (TypeElement clazz : loggedTypes) { try { warnOfNonLoggableElements(clazz); m_loggerGenerator.writeLoggerFile(clazz); @@ -391,7 +417,7 @@ private void processEpilogue(RoundEnvironment roundEnv, TypeElement epilogueAnno private void warnOfNonLoggableElements(TypeElement clazz) { var config = clazz.getAnnotation(Logged.class); - if (config.strategy() == Logged.Strategy.OPT_IN) { + if (config == null || config.strategy() == Logged.Strategy.OPT_IN) { // field and method validations will have already checked everything return; } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java index 37d2ce2460f..c097d591370 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java @@ -127,8 +127,10 @@ public String elementAccess(Element element) { private static String fieldAccess(VariableElement field) { if (field.getModifiers().contains(Modifier.PRIVATE)) { - // (com.example.Foo) $fooField.get(object) - return "(" + field.asType() + ") $" + field.getSimpleName() + ".get(object)"; + // ((com.example.Foo) $fooField.get(object)) + // Extra parentheses so cast evaluates before appended methods + // (e.g. when appending .getAsDouble()) + return "((" + field.asType() + ") $" + field.getSimpleName() + ".get(object))"; } else { // object.fooField return "object." + field.getSimpleName(); diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java index 8dea822c4aa..ba14022a2c1 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java @@ -15,7 +15,6 @@ import javax.lang.model.element.ExecutableElement; import javax.lang.model.element.TypeElement; import javax.lang.model.element.VariableElement; -import javax.lang.model.type.DeclaredType; import javax.lang.model.type.TypeKind; import javax.lang.model.type.TypeMirror; @@ -35,10 +34,8 @@ protected LoggableHandler( @Override public boolean isLoggable(Element element) { - var dataType = dataType(element); - return dataType.getAnnotation(Logged.class) != null - || dataType instanceof DeclaredType decl - && decl.asElement().getAnnotation(Logged.class) != null; + return m_processingEnv.getTypeUtils().asElement(dataType(element)) instanceof TypeElement t + && m_loggedTypes.contains(t); } @Override diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java index 15fa2104dcd..e7cb686f71b 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java @@ -15,6 +15,7 @@ import edu.wpi.first.epilogue.NotLogged; import java.io.IOException; import java.io.PrintWriter; +import java.lang.annotation.Annotation; import java.util.ArrayDeque; import java.util.ArrayList; import java.util.Deque; @@ -43,6 +44,35 @@ public class LoggerGenerator { private final ProcessingEnvironment m_processingEnv; private final List m_handlers; + @SuppressWarnings("BadAnnotationImplementation") + private final Logged m_defaultConfig = + new Logged() { + @Override + public Class annotationType() { + return Logged.class; + } + + @Override + public String name() { + return ""; + } + + @Override + public Strategy strategy() { + return Strategy.OPT_IN; + } + + @Override + public Importance importance() { + return Importance.DEBUG; + } + + @Override + public Naming defaultNaming() { + return Naming.USE_CODE_NAME; + } + }; + public LoggerGenerator(ProcessingEnvironment processingEnv, List handlers) { this.m_processingEnv = processingEnv; this.m_handlers = handlers; @@ -76,6 +106,9 @@ private static boolean isBuiltInJavaMethod(ExecutableElement e) { */ public void writeLoggerFile(TypeElement clazz) throws IOException { var config = clazz.getAnnotation(Logged.class); + if (config == null) { + config = m_defaultConfig; + } boolean requireExplicitOptIn = config.strategy() == Logged.Strategy.OPT_IN; Predicate notSkipped = LoggerGenerator::isNotSkipped; diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java index 3e9af00801e..d25ce09d318 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java @@ -4,24 +4,32 @@ package edu.wpi.first.epilogue.processor; +import java.util.Optional; import javax.annotation.processing.ProcessingEnvironment; import javax.lang.model.element.Element; +import javax.lang.model.element.TypeElement; import javax.lang.model.type.TypeMirror; public class SendableHandler extends ElementHandler { - private final TypeMirror m_sendableType; - private final TypeMirror m_commandType; - private final TypeMirror m_subsystemType; + private final Optional m_sendableType; + private final Optional m_commandType; + private final Optional m_subsystemType; protected SendableHandler(ProcessingEnvironment processingEnv) { super(processingEnv); m_sendableType = - lookupTypeElement(processingEnv, "edu.wpi.first.util.sendable.Sendable").asType(); + Optional.ofNullable( + lookupTypeElement(processingEnv, "edu.wpi.first.util.sendable.Sendable")) + .map(TypeElement::asType); m_commandType = - lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.Command").asType(); + Optional.ofNullable( + lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.Command")) + .map(TypeElement::asType); m_subsystemType = - lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.SubsystemBase").asType(); + Optional.ofNullable( + lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.SubsystemBase")) + .map(TypeElement::asType); } @Override @@ -30,20 +38,28 @@ public boolean isLoggable(Element element) { // Accept any sendable type. However, the log invocation will return null // for sendable types that should not be logged (commands, subsystems) - return m_processingEnv.getTypeUtils().isAssignable(dataType, m_sendableType); + return m_sendableType + .map(t -> m_processingEnv.getTypeUtils().isAssignable(dataType, t)) + .orElse(false); } @Override public String logInvocation(Element element) { var dataType = dataType(element); - if (m_processingEnv.getTypeUtils().isAssignable(dataType, m_commandType) - || m_processingEnv.getTypeUtils().isAssignable(dataType, m_subsystemType)) { - // Do not log commands or subsystems via their sendable implementations - // We accept all sendable objects to prevent them from being reported as not loggable, - // but their sendable implementations do not include helpful information. - // Users are free to provide custom logging implementations for commands, and tag their - // subsystems with @Logged to log their contents automatically + // Do not log commands or subsystems via their sendable implementations + // We accept all sendable objects to prevent them from being reported as not loggable, + // but their sendable implementations do not include helpful information. + // Users are free to provide custom logging implementations for commands, and tag their + // subsystems with @Logged to log their contents automatically + if (m_commandType + .map(t -> m_processingEnv.getTypeUtils().isAssignable(dataType, t)) + .orElse(false)) { + return null; + } + if (m_subsystemType + .map(t -> m_processingEnv.getTypeUtils().isAssignable(dataType, t)) + .orElse(false)) { return null; } diff --git a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java index 23fa2964bae..44063eed654 100644 --- a/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java +++ b/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java @@ -123,7 +123,7 @@ public static String loggerClassName(TypeElement clazz) { String packageName = p.getQualifiedName().toString(); String className; - if (config.name().isEmpty()) { + if (config == null || config.name().isEmpty()) { className = String.join("$", nesting); } else { className = capitalize(config.name()).replaceAll(" ", ""); diff --git a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java index cdfb99e0824..ec4e52d5f4f 100644 --- a/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java +++ b/epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java @@ -9,6 +9,7 @@ import static edu.wpi.first.epilogue.processor.CompileTestOptions.kJavaVersionOptions; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; import com.google.testing.compile.Compilation; import com.google.testing.compile.JavaFileObjects; @@ -59,6 +60,107 @@ public void update(EpilogueBackend backend, Example object) { assertLoggerGenerates(source, expectedGeneratedSource); } + @Test + void optInFields() { + String source = + """ + package edu.wpi.first.epilogue; + + class Example { + @Logged double x; + @Logged int y; + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("x", object.x); + backend.log("y", object.y); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + + @Test + void optInMethods() { + String source = + """ + package edu.wpi.first.epilogue; + + class Example { + @Logged public double getValue() { return 2.0; } + @Logged public String getName() { return "Example"; } + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("getValue", object.getValue()); + backend.log("getName", object.getName()); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + + @Test + void shouldNotLog() { + String source = + """ + class Example { + public double getValue() { return 2.0; } + public String getName() { return "Example"; } + } + """; + + Compilation compilation = + javac() + .withOptions(kJavaVersionOptions) + .withProcessors(new AnnotationProcessor()) + .compile(JavaFileObjects.forSourceString("edu.wpi.first.epilogue.Example", source)); + + assertThat(compilation).succeeded(); + // nothing is annotated with @Logged; so, no logger file should be generated + assertTrue( + compilation.generatedSourceFiles().stream() + .noneMatch(jfo -> jfo.getName().contains("Example"))); + } + @Test void multiple() { String source = @@ -141,7 +243,7 @@ public ExampleLogger() { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - backend.log("x", (double) $x.get(object)); + backend.log("x", ((double) $x.get(object))); } } } @@ -150,6 +252,59 @@ public void update(EpilogueBackend backend, Example object) { assertLoggerGenerates(source, expectedGeneratedSource); } + @Test + void privateSuppliers() { + String source = + """ + package edu.wpi.first.epilogue; + + import java.util.function.DoubleSupplier; + + @Logged + class Example { + private DoubleSupplier x; + } + """; + + String expectedGeneratedSource = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + import java.lang.invoke.MethodHandles; + import java.lang.invoke.VarHandle; + + public class ExampleLogger extends ClassSpecificLogger { + private static final VarHandle $x; + + static { + try { + var lookup = MethodHandles.privateLookupIn(Example.class, MethodHandles.lookup()); + $x = lookup.findVarHandle(Example.class, "x", java.util.function.DoubleSupplier.class); + } catch (ReflectiveOperationException e) { + throw new RuntimeException("[EPILOGUE] Could not load private fields for logging!", e); + } + } + + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + backend.log("x", ((java.util.function.DoubleSupplier) $x.get(object)).getAsDouble()); + } + } + } + """; + + assertLoggerGenerates(source, expectedGeneratedSource); + } + @Test void privateWithGenerics() { String source = @@ -192,7 +347,7 @@ public ExampleLogger() { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - logSendable(backend.getNested("chooser"), (edu.wpi.first.wpilibj.smartdashboard.SendableChooser) $chooser.get(object)); + logSendable(backend.getNested("chooser"), ((edu.wpi.first.wpilibj.smartdashboard.SendableChooser) $chooser.get(object))); } } } @@ -1401,7 +1556,7 @@ public ExampleLogger() { @Override public void update(EpilogueBackend backend, Example object) { if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { - var $$theField = (edu.wpi.first.epilogue.I) $theField.get(object); + var $$theField = ((edu.wpi.first.epilogue.I) $theField.get(object)); if ($$theField instanceof edu.wpi.first.epilogue.Base edu_wpi_first_epilogue_Base) { Epilogue.baseLogger.tryUpdate(backend.getNested("theField"), edu_wpi_first_epilogue_Base, Epilogue.getConfig().errorHandler); } else { @@ -1416,6 +1571,47 @@ public void update(EpilogueBackend backend, Example object) { assertLoggerGenerates(source, expectedRootLogger); } + @Test + void nestedOptIn() { + String source = + """ + package edu.wpi.first.epilogue; + + class Implicit { + @Logged double x; + } + + class Example { + @Logged Implicit i; + } + """; + + String expectedRootLogger = + """ + package edu.wpi.first.epilogue; + + import edu.wpi.first.epilogue.Logged; + import edu.wpi.first.epilogue.Epilogue; + import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + import edu.wpi.first.epilogue.logging.EpilogueBackend; + + public class ExampleLogger extends ClassSpecificLogger { + public ExampleLogger() { + super(Example.class); + } + + @Override + public void update(EpilogueBackend backend, Example object) { + if (Epilogue.shouldLog(Logged.Importance.DEBUG)) { + Epilogue.implicitLogger.tryUpdate(backend.getNested("i"), object.i, Epilogue.getConfig().errorHandler); + } + } + } + """; + + assertLoggerGenerates(source, expectedRootLogger); + } + @Test void customLogger() { String source = diff --git a/epilogue-runtime/BUILD.bazel b/epilogue-runtime/BUILD.bazel new file mode 100644 index 00000000000..05db6b75270 --- /dev/null +++ b/epilogue-runtime/BUILD.bazel @@ -0,0 +1,12 @@ +load("@rules_java//java:defs.bzl", "java_library") + +java_library( + name = "epilogue", + srcs = glob(["src/main/java/**/*.java"]), + visibility = ["//visibility:public"], + deps = [ + "//ntcore:networktables-java", + "//wpiunits", + "//wpiutil:wpiutil-java", + ], +) diff --git a/fieldImages/BUILD.bazel b/fieldImages/BUILD.bazel new file mode 100644 index 00000000000..b5cfc82cfa4 --- /dev/null +++ b/fieldImages/BUILD.bazel @@ -0,0 +1,31 @@ +load("@rules_cc//cc:defs.bzl", "cc_library") +load("@rules_java//java:defs.bzl", "java_library") +load("//shared/bazel/rules/gen:gen-resources.bzl", "generate_resources") + +generate_resources( + name = "generate-resources", + namespace = "fields", + prefix = "FIELDS", + resource_files = glob(["src/main/native/resources/**"]), + visibility = ["//visibility:public"], +) + +cc_library( + name = "fieldImages", + srcs = [":generate-resources"] + glob(["src/main/native/cpp/**"]), + hdrs = glob(["src/main/native/include/**/*"]), + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], +) + +java_library( + name = "fieldImages-java", + srcs = glob(["src/main/java/**/*.java"]), + resource_strip_prefix = "fieldImages/src/main/native/resources", + resources = glob(["src/main/native/resources/**"]), + visibility = ["//visibility:public"], + deps = [ + "@maven//:com_fasterxml_jackson_core_jackson_annotations", + "@maven//:com_fasterxml_jackson_core_jackson_databind", + ], +) diff --git a/hal/src/generate/ResourceType.txt b/hal/src/generate/ResourceType.txt index 5723e815648..aafdeb0bf0e 100644 --- a/hal/src/generate/ResourceType.txt +++ b/hal/src/generate/ResourceType.txt @@ -124,3 +124,4 @@ kResourceType_Koors40 = 122 kResourceType_ThriftyNova = 123 kResourceType_PWFSEN36005 = 124 kResourceType_LaserShark = 125 +kResourceType_RevServoHub = 126 diff --git a/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java b/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java index 0f1b55c096d..9c738481997 100644 --- a/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java +++ b/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java @@ -271,6 +271,8 @@ private tResourceType() { public static final int kResourceType_PWFSEN36005 = 124; /** kResourceType_LaserShark = 125. */ public static final int kResourceType_LaserShark = 125; + /** kResourceType_RevServoHub = 126. */ + public static final int kResourceType_RevServoHub = 126; } /** diff --git a/hal/src/generated/main/native/include/hal/FRCUsageReporting.h b/hal/src/generated/main/native/include/hal/FRCUsageReporting.h index fa702d0b58f..60267b6cacd 100644 --- a/hal/src/generated/main/native/include/hal/FRCUsageReporting.h +++ b/hal/src/generated/main/native/include/hal/FRCUsageReporting.h @@ -177,6 +177,7 @@ namespace HALUsageReporting { kResourceType_ThriftyNova = 123, kResourceType_PWFSEN36005 = 124, kResourceType_LaserShark = 125, + kResourceType_RevServoHub = 126, }; enum tInstances : int32_t { kLanguage_LabVIEW = 1, diff --git a/hal/src/generated/main/native/include/hal/UsageReporting.h b/hal/src/generated/main/native/include/hal/UsageReporting.h index 2c37cfbe617..0f1271b7bf1 100644 --- a/hal/src/generated/main/native/include/hal/UsageReporting.h +++ b/hal/src/generated/main/native/include/hal/UsageReporting.h @@ -146,6 +146,7 @@ typedef enum kResourceType_ThriftyNova = 123, kResourceType_PWFSEN36005 = 124, kResourceType_LaserShark = 125, + kResourceType_RevServoHub = 126, // kResourceType_MaximumID = 255, } tResourceType; diff --git a/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java b/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java index 2d8e0dd6f5d..e485ee63993 100644 --- a/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java +++ b/hal/src/main/java/edu/wpi/first/hal/CANAPITypes.java @@ -47,6 +47,8 @@ public enum CANDeviceType { kMiscellaneous(10), /** IO breakout. */ kIOBreakout(11), + /** Servo Controller. */ + kServoController(12), /** Firmware update. */ kFirmwareUpdate(31); diff --git a/hal/src/main/native/include/hal/CANAPITypes.h b/hal/src/main/native/include/hal/CANAPITypes.h index 247732c8958..7cfe93b4c4d 100644 --- a/hal/src/main/native/include/hal/CANAPITypes.h +++ b/hal/src/main/native/include/hal/CANAPITypes.h @@ -44,6 +44,8 @@ HAL_ENUM(HAL_CANDeviceType) { HAL_CAN_Dev_kMiscellaneous = 10, /// IO breakout. HAL_CAN_Dev_kIOBreakout = 11, + // Servo controller. + HAL_CAN_Dev_kServoController = 12, /// Firmware update. HAL_CAN_Dev_kFirmwareUpdate = 31 }; diff --git a/roborioteamnumbersetter/src/main/native/cpp/SshSession.cpp b/roborioteamnumbersetter/src/main/native/cpp/SshSession.cpp index 8da62444b6c..187ad784b73 100644 --- a/roborioteamnumbersetter/src/main/native/cpp/SshSession.cpp +++ b/roborioteamnumbersetter/src/main/native/cpp/SshSession.cpp @@ -146,7 +146,7 @@ std::string SshSession::ExecuteResult(std::string_view cmd, int* exitStatus) { #if LIBSSH_VERSION_MAJOR == 0 && LIBSSH_VERSION_MINOR >= 11 ssh_channel_get_exit_state(channel, &exitCode, nullptr, nullptr); #else - ssh_channel_get_exit_status(channel); + exitCode = ssh_channel_get_exit_status(channel); #endif INFO("{} {}", exitCode, cmd); diff --git a/romiVendordep/BUILD.bazel b/romiVendordep/BUILD.bazel new file mode 100644 index 00000000000..7cc0913b6e7 --- /dev/null +++ b/romiVendordep/BUILD.bazel @@ -0,0 +1,53 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") +load("@rules_java//java:defs.bzl", "java_binary", "java_library") + +cc_library( + name = "romi-cpp.static", + srcs = glob([ + "src/main/native/cpp/**", + ]), + hdrs = glob(["src/main/native/include/**"]), + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + "//wpilibc:wpilibc.static", + ], +) + +java_library( + name = "romi-java", + srcs = glob(["src/main/java/**/*.java"]), + visibility = ["//visibility:public"], + deps = [ + "//hal:hal-java", + "//wpilibj", + ], +) + +cc_test( + name = "romi-test", + size = "small", + srcs = glob(["src/test/native/cpp/**"]), + deps = [ + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + ":romi-cpp.static", + ], +) + +java_binary( + name = "DevMain-Java", + srcs = ["src/dev/java/edu/wpi/first/wpilibj/romi/DevMain.java"], + main_class = "edu.wpi.first.wpilibj.romi.DevMain", + deps = [ + "//hal:hal-java", + "//ntcore:networktables-java", + "//wpiutil:wpiutil-java", + ], +) diff --git a/shared/bazel/compiler_flags/osx_flags.rc b/shared/bazel/compiler_flags/osx_flags.rc index da12307993e..2aa48df2804 100644 --- a/shared/bazel/compiler_flags/osx_flags.rc +++ b/shared/bazel/compiler_flags/osx_flags.rc @@ -42,3 +42,6 @@ build:macos --linkopt=-Wl,-rpath,'@loader_path'" # Things not in nativetools build:macos --copt=-Wno-shorten-64-to-32 + +build:macos --host_per_file_copt=external/zlib/.*\.c@-Wno-deprecated-non-prototype +build:macos --host_per_file_copt=external/com_google_protobuf/.*\.cc@-Wno-unused-function diff --git a/shared/bazel/compiler_flags/roborio_flags.rc b/shared/bazel/compiler_flags/roborio_flags.rc index 691da8d5e6e..a0a50557263 100644 --- a/shared/bazel/compiler_flags/roborio_flags.rc +++ b/shared/bazel/compiler_flags/roborio_flags.rc @@ -14,3 +14,5 @@ build:roborio --cxxopt=-Wno-error=deprecated-declarations # Extra 11 build:roborio --cxxopt=-Wno-error=deprecated-enum-enum-conversion + +build:roborio --host_per_file_copt=external/zlib/.*\.c@-Wno-deprecated-non-prototype diff --git a/shared/config.gradle b/shared/config.gradle index 6e68b00f932..7eebdb0a774 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -15,7 +15,7 @@ nativeUtils { configureDependencies { opencvYear = "frc2025" niLibVersion = "2025.0.0" - opencvVersion = "4.8.0-1" + opencvVersion = "4.10.0-3" } } } diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index 9463ea1c8f7..644c095cf21 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -67,9 +67,9 @@ def tagList = [ "SmartDashboard", "Shuffleboard", "Sendable", "DataLog", /* --- Controls --- */ - "Exponential Profile", "PID", "State-Space", "Ramsete", "Path Following", "Trajectory", - "SysId", "Simulation", "Trapezoid Profile", "Profiled PID", "Odometry", "LQR", - "Pose Estimator", + "Exponential Profile", "PID", "State-Space", "LTVUnicycleController", "Path Following", + "Trajectory", "SysId", "Simulation", "Trapezoid Profile", "Profiled PID", "Odometry", + "LQR", "Pose Estimator", /* --- Hardware --- */ "Analog", "Ultrasonic", "Gyro", "Pneumatics", "I2C", "Duty Cycle", "PDP", "DMA", "Relay", diff --git a/shared/opencv.gradle b/shared/opencv.gradle index 432a0658f24..32b45989323 100644 --- a/shared/opencv.gradle +++ b/shared/opencv.gradle @@ -1,4 +1,4 @@ -def opencvVersion = '4.8.0-1' +def opencvVersion = '4.10.0-3' if (project.hasProperty('useCpp') && project.useCpp) { model { diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 9b5d3e6aae0..ac25370ff8d 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -48,8 +48,8 @@ def copy_upstream_src(wpilib_root): def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2024-09-18 - tag = "8bbce85252bc351c5aacb0de9f50fa31b8b9e1ae" + # main on 2024-12-07 + tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch deleted file mode 100644 index 2a74fa6a849..00000000000 --- a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch +++ /dev/null @@ -1,638 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 24 Apr 2024 15:56:06 -0700 -Subject: [PATCH 1/5] Remove "using enum" declarations - ---- - include/sleipnir/autodiff/Expression.hpp | 161 +++++++----------- - .../optimization/SolverExitCondition.hpp | 22 ++- - 2 files changed, 73 insertions(+), 110 deletions(-) - -diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..51070613e82cdf5e4105519f39632deb5d2bf19e 100644 ---- a/include/sleipnir/autodiff/Expression.hpp -+++ b/include/sleipnir/autodiff/Expression.hpp -@@ -203,8 +203,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero -@@ -219,20 +217,22 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value * rhs->value); - } - - // Evaluate expression type - ExpressionType type; -- if (lhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant) { - type = rhs->type; -- } else if (rhs->type == kConstant) { -+ } else if (rhs->type == ExpressionType::kConstant) { - type = lhs->type; -- } else if (lhs->type == kLinear && rhs->type == kLinear) { -- type = kQuadratic; -+ } else if (lhs->type == ExpressionType::kLinear && -+ rhs->type == ExpressionType::kLinear) { -+ type = ExpressionType::kQuadratic; - } else { -- type = kNonlinear; -+ type = ExpressionType::kNonlinear; - } - - return MakeExpressionPtr( -@@ -258,8 +258,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero -@@ -269,16 +267,17 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value / rhs->value); - } - - // Evaluate expression type - ExpressionType type; -- if (rhs->type == kConstant) { -+ if (rhs->type == ExpressionType::kConstant) { - type = lhs->type; - } else { -- type = kNonlinear; -+ type = ExpressionType::kNonlinear; - } - - return MakeExpressionPtr( -@@ -306,8 +305,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs == nullptr || lhs->IsConstant(0.0)) { - return rhs; -@@ -316,7 +313,8 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value + rhs->value); - } - -@@ -340,8 +338,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - if (rhs->IsConstant(0.0)) { -@@ -355,7 +351,8 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant && rhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant && -+ rhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(lhs->value - rhs->value); - } - -@@ -377,8 +374,6 @@ struct SLEIPNIR_DLLEXPORT Expression { - * @param lhs Operand of unary minus. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { -- using enum ExpressionType; -- - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero -@@ -386,7 +381,7 @@ struct SLEIPNIR_DLLEXPORT Expression { - } - - // Evaluate constant -- if (lhs->type == kConstant) { -+ if (lhs->type == ExpressionType::kConstant) { - return MakeExpressionPtr(-lhs->value); - } - -@@ -469,8 +464,6 @@ inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -478,12 +471,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::abs(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::abs(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::abs(x); }, - [](double x, double, double parentAdjoint) { - if (x < 0.0) { - return -parentAdjoint; -@@ -514,20 +507,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(std::numbers::pi / 2.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::acos(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::acos(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::acos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint / std::sqrt(1.0 - x * x); - }, -@@ -546,8 +537,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -555,12 +544,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::asin(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::asin(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::asin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / std::sqrt(1.0 - x * x); - }, -@@ -579,8 +568,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -588,12 +575,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::atan(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::atan(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::atan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (1.0 + x * x); - }, -@@ -612,8 +599,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - const ExpressionPtr& y, const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (y->IsConstant(0.0)) { - // Return zero -@@ -623,12 +608,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - } - - // Evaluate constant -- if (y->type == kConstant && x->type == kConstant) { -+ if (y->type == ExpressionType::kConstant && -+ x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::atan2(y->value, x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double y, double x) { return std::atan2(y, x); }, -+ ExpressionType::kNonlinear, -+ [](double y, double x) { return std::atan2(y, x); }, - [](double y, double x, double parentAdjoint) { - return parentAdjoint * x / (y * y + x * x); - }, -@@ -653,20 +640,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::cos(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::cos(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::cos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint * std::sin(x); - }, -@@ -684,20 +669,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::cosh(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::cosh(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::cosh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::sinh(x); - }, -@@ -715,8 +698,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -724,12 +705,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::erf(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::erf(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::erf(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * - std::exp(-x * x); -@@ -750,20 +731,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::exp(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::exp(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::exp(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::exp(x); - }, -@@ -782,8 +761,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - const ExpressionPtr& x, const ExpressionPtr& y) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - return y; -@@ -792,12 +769,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant && y->type == kConstant) { -+ if (x->type == ExpressionType::kConstant && -+ y->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::hypot(x->value, y->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double y) { return std::hypot(x, y); }, -+ ExpressionType::kNonlinear, -+ [](double x, double y) { return std::hypot(x, y); }, - [](double x, double y, double parentAdjoint) { - return parentAdjoint * x / std::hypot(x, y); - }, -@@ -822,8 +801,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -831,12 +808,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::log(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::log(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::log(x); }, - [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, -@@ -850,8 +827,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -859,12 +834,13 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::log10(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::log10(x); }, -+ ExpressionType::kNonlinear, -+ [](double x, double) { return std::log10(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::numbers::ln10 * x); - }, -@@ -883,8 +859,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - const ExpressionPtr& base, const ExpressionPtr& power) { -- using enum ExpressionType; -- - // Prune expression - if (base->IsConstant(0.0)) { - // Return zero -@@ -899,12 +873,15 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - } - - // Evaluate constant -- if (base->type == kConstant && power->type == kConstant) { -+ if (base->type == ExpressionType::kConstant && -+ power->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::pow(base->value, power->value)); - } - - return MakeExpressionPtr( -- base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, -+ base->type == ExpressionType::kLinear && power->IsConstant(2.0) -+ ? ExpressionType::kQuadratic -+ : ExpressionType::kNonlinear, - [](double base, double power) { return std::pow(base, power); }, - [](double base, double power, double parentAdjoint) { - return parentAdjoint * std::pow(base, power - 1) * power; -@@ -945,10 +922,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - * @param x The argument. - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - if (x->value < 0.0) { - return MakeExpressionPtr(-1.0); - } else if (x->value == 0.0) { -@@ -960,7 +935,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { - } - - return MakeExpressionPtr( -- kNonlinear, -+ ExpressionType::kNonlinear, - [](double x, double) { - if (x < 0.0) { - return -1.0; -@@ -985,8 +960,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -994,12 +967,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::sin(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::sin(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::sin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cos(x); - }, -@@ -1016,8 +989,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - * @param x The argument. - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -1025,12 +996,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::sinh(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::sinh(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::sinh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cosh(x); - }, -@@ -1048,10 +1019,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - if (x->value == 0.0) { - // Return zero - return x; -@@ -1063,7 +1032,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::sqrt(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::sqrt(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (2.0 * std::sqrt(x)); - }, -@@ -1082,8 +1051,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -1091,12 +1058,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::tan(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::tan(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::tan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cos(x) * std::cos(x)); - }, -@@ -1114,8 +1081,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - * @param x The argument. - */ - SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { -- using enum ExpressionType; -- - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero -@@ -1123,12 +1088,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { - } - - // Evaluate constant -- if (x->type == kConstant) { -+ if (x->type == ExpressionType::kConstant) { - return MakeExpressionPtr(std::tanh(x->value)); - } - - return MakeExpressionPtr( -- kNonlinear, [](double x, double) { return std::tanh(x); }, -+ ExpressionType::kNonlinear, [](double x, double) { return std::tanh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cosh(x) * std::cosh(x)); - }, -diff --git a/include/sleipnir/optimization/SolverExitCondition.hpp b/include/sleipnir/optimization/SolverExitCondition.hpp -index 7d1445297e33e3c62bcdf9d03eebeaad20af9a1c..734cd3d127327e8ce01e1a42fe74ccc81fea1f90 100644 ---- a/include/sleipnir/optimization/SolverExitCondition.hpp -+++ b/include/sleipnir/optimization/SolverExitCondition.hpp -@@ -46,31 +46,29 @@ enum class SolverExitCondition : int8_t { - */ - SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( - const SolverExitCondition& exitCondition) { -- using enum SolverExitCondition; -- - switch (exitCondition) { -- case kSuccess: -+ case SolverExitCondition::kSuccess: - return "solved to desired tolerance"; -- case kSolvedToAcceptableTolerance: -+ case SolverExitCondition::kSolvedToAcceptableTolerance: - return "solved to acceptable tolerance"; -- case kCallbackRequestedStop: -+ case SolverExitCondition::kCallbackRequestedStop: - return "callback requested stop"; -- case kTooFewDOFs: -+ case SolverExitCondition::kTooFewDOFs: - return "problem has too few degrees of freedom"; -- case kLocallyInfeasible: -+ case SolverExitCondition::kLocallyInfeasible: - return "problem is locally infeasible"; -- case kFeasibilityRestorationFailed: -+ case SolverExitCondition::kFeasibilityRestorationFailed: - return "solver failed to reach the desired tolerance, and feasibility " - "restoration failed to converge"; -- case kNonfiniteInitialCostOrConstraints: -+ case SolverExitCondition::kNonfiniteInitialCostOrConstraints: - return "solver encountered nonfinite initial cost or constraints and " - "gave up"; -- case kDivergingIterates: -+ case SolverExitCondition::kDivergingIterates: - return "solver encountered diverging primal iterates xₖ and/or sₖ and " - "gave up"; -- case kMaxIterationsExceeded: -+ case SolverExitCondition::kMaxIterationsExceeded: - return "solution returned after maximum iterations exceeded"; -- case kTimeout: -+ case SolverExitCondition::kTimeout: - return "solution returned after maximum wall clock time exceeded"; - default: - return "unknown"; diff --git a/upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch similarity index 98% rename from upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch rename to upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index e5779dc71f0..deaaf7b5cb7 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 2/5] Use fmtlib +Subject: [PATCH 1/3] Use fmtlib --- include/.styleguide | 1 + diff --git a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch similarity index 75% rename from upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch rename to upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 550983549db..6b816784a7a 100644 --- a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -1,25 +1,26 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 16 Jun 2024 12:08:49 -0700 -Subject: [PATCH 4/5] Use wpi::SmallVector +Subject: [PATCH 2/3] Use wpi::SmallVector --- - include/.styleguide | 1 + - include/sleipnir/autodiff/Expression.hpp | 5 +++-- - include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- - include/sleipnir/autodiff/Hessian.hpp | 4 ++-- - include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- - include/sleipnir/autodiff/Variable.hpp | 10 +++++----- - include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- - include/sleipnir/optimization/Multistart.hpp | 7 ++++--- - .../sleipnir/optimization/OptimizationProblem.hpp | 8 ++++---- - include/sleipnir/util/Pool.hpp | 7 ++++--- - include/sleipnir/util/Spy.hpp | 4 ++-- - src/.styleguide | 1 + - src/optimization/solver/InteriorPoint.cpp | 4 ++-- - .../solver/util/FeasibilityRestoration.hpp | 10 +++++----- - src/optimization/solver/util/Filter.hpp | 4 ++-- - 15 files changed, 50 insertions(+), 44 deletions(-) + include/.styleguide | 1 + + include/sleipnir/autodiff/Expression.hpp | 13 +++++++------ + include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- + include/sleipnir/autodiff/Hessian.hpp | 4 ++-- + include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- + include/sleipnir/autodiff/Variable.hpp | 10 +++++----- + include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- + include/sleipnir/optimization/Multistart.hpp | 7 ++++--- + .../optimization/OptimizationProblem.hpp | 8 ++++---- + include/sleipnir/util/Pool.hpp | 7 ++++--- + include/sleipnir/util/Spy.hpp | 4 ++-- + src/.styleguide | 1 + + src/optimization/solver/InteriorPoint.cpp | 4 ++-- + src/optimization/solver/SQP.cpp | 4 ++-- + .../solver/util/FeasibilityRestoration.hpp | 18 +++++++++--------- + src/optimization/solver/util/Filter.hpp | 4 ++-- + 16 files changed, 60 insertions(+), 54 deletions(-) diff --git a/include/.styleguide b/include/.styleguide index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644 @@ -32,7 +33,7 @@ index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f185 + ^wpi/ } diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index dff8e2a6ef24413e3e6356bf0ec57286e50654cf..bdbeb4730223f88cfdc0c424a8f7b852b34742f7 100644 +index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp @@ -11,11 +11,12 @@ @@ -49,7 +50,33 @@ index dff8e2a6ef24413e3e6356bf0ec57286e50654cf..bdbeb4730223f88cfdc0c424a8f7b852 namespace sleipnir::detail { -@@ -427,7 +428,7 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { +@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true; + + struct SLEIPNIR_DLLEXPORT Expression; + +-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); +-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr); ++inline void IntrusiveSharedPtrIncRefCount(Expression* expr); ++inline void IntrusiveSharedPtrDecRefCount(Expression* expr); + + /** + * Typedef for intrusive shared pointer to Expression. +@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); + * + * @param expr The shared pointer's managed object. + */ +-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { ++inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { + ++expr->refCount; + } + +@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { + * + * @param expr The shared pointer's managed object. + */ +-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { ++inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { + // If a deeply nested tree is being deallocated all at once, calling the // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their // refcounts and deallocate them. @@ -308,7 +335,7 @@ index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d for (auto& future : futures) { diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp -index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b506bdf11 100644 +index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644 --- a/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/include/sleipnir/optimization/OptimizationProblem.hpp @@ -11,6 +11,7 @@ @@ -319,15 +346,15 @@ index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -21,7 +22,6 @@ - #include "sleipnir/optimization/solver/InteriorPoint.hpp" +@@ -22,7 +23,6 @@ + #include "sleipnir/optimization/solver/SQP.hpp" #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" -#include "sleipnir/util/small_vector.hpp" namespace sleipnir { -@@ -358,16 +358,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { +@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { private: // The list of decision variables, which are the root of the problem's // expression tree @@ -408,7 +435,7 @@ index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92 + ^wpi/ } diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp -index dcd8b56dc08516b80f89550c43cb7002745b93d8..892d2dd20f7fe92c2c91518a4ecb487425643585 100644 +index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644 --- a/src/optimization/solver/InteriorPoint.cpp +++ b/src/optimization/solver/InteriorPoint.cpp @@ -9,6 +9,7 @@ @@ -436,8 +463,37 @@ index dcd8b56dc08516b80f89550c43cb7002745b93d8..892d2dd20f7fe92c2c91518a4ecb4874 RegularizedLDLT solver; +diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp +index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644 +--- a/src/optimization/solver/SQP.cpp ++++ b/src/optimization/solver/SQP.cpp +@@ -9,6 +9,7 @@ + #include + + #include ++#include + + #include "optimization/RegularizedLDLT.hpp" + #include "optimization/solver/util/ErrorEstimate.hpp" +@@ -22,7 +23,6 @@ + #include "sleipnir/optimization/SolverExitCondition.hpp" + #include "sleipnir/util/Print.hpp" + #include "sleipnir/util/Spy.hpp" +-#include "sleipnir/util/small_vector.hpp" + #include "util/ScopeExit.hpp" + #include "util/ToMilliseconds.hpp" + +@@ -155,7 +155,7 @@ void SQP(std::span decisionVariables, + Filter filter{f}; + + // Kept outside the loop so its storage can be reused +- small_vector> triplets; ++ wpi::SmallVector> triplets; + + RegularizedLDLT solver; + diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp -index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d5498acaa18 100644 +index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644 --- a/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -8,6 +8,7 @@ @@ -456,7 +512,43 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 namespace sleipnir { -@@ -65,7 +65,7 @@ inline void FeasibilityRestoration( +@@ -57,7 +57,7 @@ inline void FeasibilityRestoration( + constexpr double ρ = 1000.0; + double μ = config.tolerance / 10.0; + +- small_vector fr_decisionVariables; ++ wpi::SmallVector fr_decisionVariables; + fr_decisionVariables.reserve(decisionVariables.size() + + 2 * equalityConstraints.size()); + +@@ -70,7 +70,7 @@ inline void FeasibilityRestoration( + fr_decisionVariables.emplace_back(); + } + +- auto it = fr_decisionVariables.cbegin(); ++ auto it = fr_decisionVariables.begin(); + + VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; + it += decisionVariables.size(); +@@ -128,7 +128,7 @@ inline void FeasibilityRestoration( + } + + // cₑ(x) - pₑ + nₑ = 0 +- small_vector fr_equalityConstraints; ++ wpi::SmallVector fr_equalityConstraints; + fr_equalityConstraints.assign(equalityConstraints.begin(), + equalityConstraints.end()); + for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { +@@ -137,7 +137,7 @@ inline void FeasibilityRestoration( + } + + // cᵢ(x) - s - pᵢ + nᵢ = 0 +- small_vector fr_inequalityConstraints; ++ wpi::SmallVector fr_inequalityConstraints; + + // pₑ ≥ 0 + std::copy(p_e.begin(), p_e.end(), +@@ -227,7 +227,7 @@ inline void FeasibilityRestoration( constexpr double ρ = 1000.0; @@ -465,7 +557,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 fr_decisionVariables.reserve(decisionVariables.size() + 2 * equalityConstraints.size() + 2 * inequalityConstraints.size()); -@@ -81,7 +81,7 @@ inline void FeasibilityRestoration( +@@ -243,7 +243,7 @@ inline void FeasibilityRestoration( fr_decisionVariables.emplace_back(); } @@ -474,7 +566,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; it += decisionVariables.size(); -@@ -157,7 +157,7 @@ inline void FeasibilityRestoration( +@@ -319,7 +319,7 @@ inline void FeasibilityRestoration( } // cₑ(x) - pₑ + nₑ = 0 @@ -483,7 +575,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 fr_equalityConstraints.assign(equalityConstraints.begin(), equalityConstraints.end()); for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { -@@ -166,7 +166,7 @@ inline void FeasibilityRestoration( +@@ -328,7 +328,7 @@ inline void FeasibilityRestoration( } // cᵢ(x) - s - pᵢ + nᵢ = 0 @@ -493,7 +585,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 inequalityConstraints.end()); for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp -index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564d7e0a2c7 100644 +index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644 --- a/src/optimization/solver/util/Filter.hpp +++ b/src/optimization/solver/util/Filter.hpp @@ -8,9 +8,9 @@ @@ -505,12 +597,12 @@ index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564 #include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/util/small_vector.hpp" - namespace sleipnir { + // See docs/algorithms.md#Works_cited for citation definitions. + +@@ -177,7 +177,7 @@ class Filter { -@@ -182,7 +182,7 @@ class Filter { private: Variable* m_f = nullptr; - double m_μ = 0.0; - small_vector m_filter; + wpi::SmallVector m_filter; }; diff --git a/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch b/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch deleted file mode 100644 index 853c4d29883..00000000000 --- a/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch +++ /dev/null @@ -1,42 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Mon, 20 May 2024 09:01:54 -0700 -Subject: [PATCH 3/5] Remove unsupported constexpr - ---- - include/sleipnir/autodiff/Expression.hpp | 8 ++++---- - 1 file changed, 4 insertions(+), 4 deletions(-) - -diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index 51070613e82cdf5e4105519f39632deb5d2bf19e..dff8e2a6ef24413e3e6356bf0ec57286e50654cf 100644 ---- a/include/sleipnir/autodiff/Expression.hpp -+++ b/include/sleipnir/autodiff/Expression.hpp -@@ -29,8 +29,8 @@ inline constexpr bool kUsePoolAllocator = true; - - struct SLEIPNIR_DLLEXPORT Expression; - --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr); -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr); -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr); - - /** - * Typedef for intrusive shared pointer to Expression. -@@ -413,7 +413,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); - * - * @param expr The shared pointer's managed object. - */ --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { - ++expr->refCount; - } - -@@ -422,7 +422,7 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { - * - * @param expr The shared pointer's managed object. - */ --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { - // If a deeply nested tree is being deallocated all at once, calling the - // Expression destructor when expr's refcount reaches zero can cause a stack - // overflow. Instead, we iterate over its children to decrement their diff --git a/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch similarity index 96% rename from upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch rename to upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch index a7f3de198e1..90ae6e261bd 100644 --- a/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 26 Jun 2024 12:13:33 -0700 -Subject: [PATCH 5/5] Suppress clang-tidy false positives +Subject: [PATCH 3/3] Suppress clang-tidy false positives --- include/sleipnir/autodiff/Variable.hpp | 4 ++-- diff --git a/wpilibNewCommands/BUILD.bazel b/wpilibNewCommands/BUILD.bazel new file mode 100644 index 00000000000..6add11137ad --- /dev/null +++ b/wpilibNewCommands/BUILD.bazel @@ -0,0 +1,87 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") +load("@rules_java//java:defs.bzl", "java_binary", "java_library") + +cc_library( + name = "generated_cc_headers", + hdrs = glob(["src/generated/main/native/include/**"]), + includes = ["src/generated/main/native/include"], + strip_include_prefix = "src/generated/main/native/include", + visibility = ["//wpilibNewCommands:__subpackages__"], +) + +filegroup( + name = "generated_cc_source", + srcs = glob(["src/generated/main/native/cpp/**"]), + visibility = ["//wpilibNewCommands:__subpackages__"], +) + +filegroup( + name = "generated_java", + srcs = glob(["src/generated/main/java/**/*.java"]), + visibility = ["//wpilibNewCommands:__subpackages__"], +) + +cc_library( + name = "wpilibNewCommands.static", + srcs = glob(["src/main/native/cpp/**"]) + [":generated_cc_source"], + hdrs = glob(["src/main/native/include/**"]), + includes = ["src/main/native/include"], + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + ":generated_cc_headers", + "//wpilibc:wpilibc.static", + ], +) + +java_library( + name = "wpilibNewCommands-java", + srcs = glob(["src/main/java/**/*.java"]) + [":generated_java"], + visibility = ["//visibility:public"], + deps = [ + "//cscore:cscore-java", + "//hal:hal-java", + "//ntcore:networktables-java", + "//wpilibj", + "//wpimath:wpimath-java", + "//wpinet:wpinet-java", + "//wpiunits", + "//wpiutil:wpiutil-java", + ], +) + +cc_test( + name = "wpilibNewCommands-cpp-test", + size = "small", + srcs = glob([ + "src/test/native/**/*.cpp", + "src/test/native/**/*.h", + ]), + tags = [ + "no-tsan", + "no-ubsan", + ], + deps = [ + ":wpilibNewCommands.static", + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + ], +) + +java_binary( + name = "DevMain-Java", + srcs = ["src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java"], + main_class = "edu.wpi.first.wpilibj2.commands.DevMain", + deps = [ + "//hal:hal-java", + "//ntcore:networktables-java", + "//wpimath:wpimath-java", + "//wpiutil:wpiutil-java", + ], +) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java index a53d792d0e4..d6cd4c8860d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java @@ -287,6 +287,24 @@ public SequentialCommandGroup andThen(Command... next) { return group; } + /** + * Creates a new command that runs this command and the deadline in parallel, finishing (and + * interrupting this command) when the deadline finishes. + * + *

Note: This decorator works by adding this command to a composition. The command the + * decorator was called on cannot be scheduled independently or be added to a different + * composition (namely, decorators), unless it is manually cleared from the list of composed + * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition + * returned from this method can be further decorated without issue. + * + * @param deadline the deadline of the command group + * @return the decorated command + * @see Command#deadlineFor + */ + public ParallelDeadlineGroup withDeadline(Command deadline) { + return new ParallelDeadlineGroup(deadline, this); + } + /** * Decorates this command with a set of commands to run parallel to it, ending when the calling * command ends and interrupting all the others. Often more convenient/less-verbose than @@ -321,6 +339,7 @@ public ParallelDeadlineGroup deadlineWith(Command... parallel) { * @param parallel the commands to run in parallel. Note the parallel commands will be interrupted * when the deadline command ends * @return the decorated command + * @see Command#withDeadline */ public ParallelDeadlineGroup deadlineFor(Command... parallel) { return new ParallelDeadlineGroup(this, parallel); diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index bb47e58a370..ee52cfa9c36 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.controller.HolonomicDriveController; @@ -19,7 +17,6 @@ import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; import java.util.function.Supplier; @@ -41,6 +38,7 @@ * *

This class is provided by the NewCommands VendorDep */ +@SuppressWarnings("removal") public class MecanumControllerCommand extends Command { private final Timer m_timer = new Timer(); private final boolean m_usePID; @@ -56,16 +54,12 @@ public class MecanumControllerCommand extends Command { private final PIDController m_frontRightController; private final PIDController m_rearRightController; private final Supplier m_currentWheelSpeeds; - private final Consumer m_outputDriveVoltages; + private final MecanumVoltagesConsumer m_outputDriveVoltages; private final Consumer m_outputWheelSpeeds; - private final MutLinearVelocity m_prevFrontLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRearLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevFrontRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRearRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_frontLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rearLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_frontRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rearRightSpeedSetpoint = MetersPerSecond.mutable(0); + private double m_prevFrontLeftSpeedSetpoint; // m/s + private double m_prevRearLeftSpeedSetpoint; // m/s + private double m_prevFrontRightSpeedSetpoint; // m/s + private double m_prevRearRightSpeedSetpoint; // m/s /** * Constructs a new MecanumControllerCommand that when executed will follow the provided @@ -91,8 +85,7 @@ public class MecanumControllerCommand extends Command { * @param frontRightController The front right wheel velocity PID. * @param rearRightController The rear right wheel velocity PID. * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds. - * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor - * voltages. + * @param outputDriveVoltages A MecanumVoltagesConsumer that consumes voltages of mecanum motors. * @param requirements The subsystems to require. */ @SuppressWarnings("this-escape") @@ -111,7 +104,7 @@ public MecanumControllerCommand( PIDController frontRightController, PIDController rearRightController, Supplier currentWheelSpeeds, - Consumer outputDriveVoltages, + MecanumVoltagesConsumer outputDriveVoltages, Subsystem... requirements) { m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand"); m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand"); @@ -152,6 +145,73 @@ public MecanumControllerCommand( addRequirements(requirements); } + /** + * Constructs a new MecanumControllerCommand that when executed will follow the provided + * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to + * 12 as a voltage output to the motor. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path + * this is left to the user, since it is not appropriate for paths with nonstationary endstates. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of the odometry classes to + * provide this. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller for the robot's x position. + * @param yController The Trajectory Tracker PID controller for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. This is sampled at each time + * step. + * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds. + * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor + * voltages. + * @param requirements The subsystems to require. + */ + @Deprecated(since = "2025", forRemoval = true) + public MecanumControllerCommand( + Trajectory trajectory, + Supplier pose, + SimpleMotorFeedforward feedforward, + MecanumDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + Supplier desiredRotation, + double maxWheelVelocityMetersPerSecond, + PIDController frontLeftController, + PIDController rearLeftController, + PIDController frontRightController, + PIDController rearRightController, + Supplier currentWheelSpeeds, + Consumer outputDriveVoltages, + Subsystem... requirements) { + this( + trajectory, + pose, + feedforward, + kinematics, + xController, + yController, + thetaController, + desiredRotation, + maxWheelVelocityMetersPerSecond, + frontLeftController, + rearLeftController, + frontRightController, + rearRightController, + currentWheelSpeeds, + (frontLeft, frontRight, rearLeft, rearRight) -> + outputDriveVoltages.accept( + new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)), + requirements); + } + /** * Constructs a new MecanumControllerCommand that when executed will follow the provided * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to @@ -179,8 +239,7 @@ public MecanumControllerCommand( * @param frontRightController The front right wheel velocity PID. * @param rearRightController The rear right wheel velocity PID. * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds. - * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor - * voltages. + * @param outputDriveVoltages A MecanumVoltagesConsumer that consumes voltages of mecanum motors. * @param requirements The subsystems to require. */ public MecanumControllerCommand( @@ -197,7 +256,7 @@ public MecanumControllerCommand( PIDController frontRightController, PIDController rearRightController, Supplier currentWheelSpeeds, - Consumer outputDriveVoltages, + MecanumVoltagesConsumer outputDriveVoltages, Subsystem... requirements) { this( trajectory, @@ -219,6 +278,74 @@ public MecanumControllerCommand( requirements); } + /** + * Constructs a new MecanumControllerCommand that when executed will follow the provided + * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to + * 12 as a voltage output to the motor. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path + * this is left to the user, since it is not appropriate for paths with nonstationary endstates. + * + *

Note 2: The final rotation of the robot will be set to the rotation of the final pose in the + * trajectory. The robot will not follow the rotations from the poses at each timestep. If + * alternate rotation behavior is desired, the other constructor with a supplier for rotation + * should be used. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of the odometry classes to + * provide this. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller for the robot's x position. + * @param yController The Trajectory Tracker PID controller for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller for angle for the robot. + * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds. + * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor + * voltages. + * @param requirements The subsystems to require. + */ + @Deprecated(since = "2025", forRemoval = true) + public MecanumControllerCommand( + Trajectory trajectory, + Supplier pose, + SimpleMotorFeedforward feedforward, + MecanumDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + double maxWheelVelocityMetersPerSecond, + PIDController frontLeftController, + PIDController rearLeftController, + PIDController frontRightController, + PIDController rearRightController, + Supplier currentWheelSpeeds, + Consumer outputDriveVoltages, + Subsystem... requirements) { + this( + trajectory, + pose, + feedforward, + kinematics, + xController, + yController, + thetaController, + maxWheelVelocityMetersPerSecond, + frontLeftController, + rearLeftController, + frontRightController, + rearRightController, + currentWheelSpeeds, + (frontLeft, frontRight, rearLeft, rearRight) -> + outputDriveVoltages.accept( + new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)), + requirements); + } + /** * Constructs a new MecanumControllerCommand that when executed will follow the provided * trajectory. The user should implement a velocity PID on the desired output wheel velocities. @@ -343,10 +470,10 @@ public void initialize() { MecanumDriveWheelSpeeds prevSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); - m_prevFrontLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontLeftMetersPerSecond); - m_prevRearLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearLeftMetersPerSecond); - m_prevFrontRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontRightMetersPerSecond); - m_prevRearRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearRightMetersPerSecond); + m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeftMetersPerSecond; + m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeftMetersPerSecond; + m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRightMetersPerSecond; + m_prevRearRightSpeedSetpoint = prevSpeeds.rearRightMetersPerSecond; m_timer.restart(); } @@ -363,10 +490,10 @@ public void execute() { targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond); - m_frontLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontLeftMetersPerSecond); - m_rearLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearLeftMetersPerSecond); - m_frontRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontRightMetersPerSecond); - m_rearRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearRightMetersPerSecond); + double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; + double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; + double frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; + double rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; double frontLeftOutput; double rearLeftOutput; @@ -375,54 +502,50 @@ public void execute() { if (m_usePID) { final double frontLeftFeedforward = - m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, m_frontLeftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities( + m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint); final double rearLeftFeedforward = - m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, m_rearLeftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint); final double frontRightFeedforward = - m_feedforward - .calculate(m_prevFrontRightSpeedSetpoint, m_frontRightSpeedSetpoint) - .in(Volts); + m_feedforward.calculateWithVelocities( + m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint); final double rearRightFeedforward = - m_feedforward.calculate(m_prevRearRightSpeedSetpoint, m_rearRightSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities( + m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint); frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate( - m_currentWheelSpeeds.get().frontLeftMetersPerSecond, - m_frontLeftSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint); rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate( - m_currentWheelSpeeds.get().rearLeftMetersPerSecond, - m_rearLeftSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint); frontRightOutput = frontRightFeedforward + m_frontRightController.calculate( - m_currentWheelSpeeds.get().frontRightMetersPerSecond, - m_frontRightSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint); rearRightOutput = rearRightFeedforward + m_rearRightController.calculate( - m_currentWheelSpeeds.get().rearRightMetersPerSecond, - m_rearRightSpeedSetpoint.in(MetersPerSecond)); + m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint); m_outputDriveVoltages.accept( - new MecanumDriveMotorVoltages( - frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput)); + frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput); } else { m_outputWheelSpeeds.accept( new MecanumDriveWheelSpeeds( - m_frontLeftSpeedSetpoint.in(MetersPerSecond), - m_frontRightSpeedSetpoint.in(MetersPerSecond), - m_rearLeftSpeedSetpoint.in(MetersPerSecond), - m_rearRightSpeedSetpoint.in(MetersPerSecond))); + frontLeftSpeedSetpoint, + frontRightSpeedSetpoint, + rearLeftSpeedSetpoint, + rearRightSpeedSetpoint)); } } @@ -435,4 +558,22 @@ public void end(boolean interrupted) { public boolean isFinished() { return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); } + + /** A consumer to represent an operation on the voltages of a mecanum drive. */ + @FunctionalInterface + public interface MecanumVoltagesConsumer { + /** + * Accepts the voltages to perform some operation with them. + * + * @param frontLeftVoltage The voltage of the front left motor. + * @param frontRightVoltage The voltage of the front right motor. + * @param rearLeftVoltage The voltage of the rear left motor. + * @param rearRightVoltage The voltage of the rear left motor. + */ + void accept( + double frontLeftVoltage, + double frontRightVoltage, + double rearLeftVoltage, + double rearRightVoltage); + } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java index 7225dee1a97..d3235c9e194 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java @@ -34,8 +34,7 @@ public class ProxyCommand extends Command { * @deprecated This constructor's similarity to {@link DeferredCommand} is confusing and opens * potential footguns for users who do not fully understand the semantics and implications of * proxying, but who simply want runtime construction. Users who do know what they are doing - * and need a supplier-constructed proxied command should instead proxy a DeferredCommand - * using the asProxy decorator. + * and need a supplier-constructed proxied command should instead defer a proxy command. * @see DeferredCommand */ @Deprecated(since = "2025", forRemoval = true) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 533db5b0398..88aae9cca16 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.controller.PIDController; @@ -16,7 +14,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Timer; import java.util.function.BiConsumer; @@ -49,10 +46,8 @@ public class RamseteCommand extends Command { private final PIDController m_rightController; private final BiConsumer m_output; private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds(); - private final MutLinearVelocity m_prevLeftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_prevRightSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_leftSpeedSetpoint = MetersPerSecond.mutable(0); - private final MutLinearVelocity m_rightSpeedSetpoint = MetersPerSecond.mutable(0); + private double m_prevLeftSpeedSetpoint; // m/s + private double m_prevRightSpeedSetpoint; // m/s private double m_prevTime; /** @@ -156,8 +151,8 @@ public void initialize() { initialState.velocityMetersPerSecond, 0, initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond)); - m_prevLeftSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.leftMetersPerSecond); - m_prevRightSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.rightMetersPerSecond); + m_prevLeftSpeedSetpoint = m_prevSpeeds.leftMetersPerSecond; + m_prevRightSpeedSetpoint = m_prevSpeeds.rightMetersPerSecond; m_timer.restart(); if (m_usePID) { m_leftController.reset(); @@ -179,21 +174,18 @@ public void execute() { m_kinematics.toWheelSpeeds( m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime))); - var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; - var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; - - m_leftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.leftMetersPerSecond); - m_rightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rightMetersPerSecond); + double leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond; + double rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond; double leftOutput; double rightOutput; if (m_usePID) { double leftFeedforward = - m_feedforward.calculate(m_prevLeftSpeedSetpoint, m_leftSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevLeftSpeedSetpoint, leftSpeedSetpoint); double rightFeedforward = - m_feedforward.calculate(m_prevRightSpeedSetpoint, m_rightSpeedSetpoint).in(Volts); + m_feedforward.calculateWithVelocities(m_prevRightSpeedSetpoint, rightSpeedSetpoint); leftOutput = leftFeedforward diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java index d7b1dd08937..8dc3ae52b3d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java @@ -24,6 +24,18 @@ *

This class is provided by the NewCommands VendorDep */ public class Trigger implements BooleanSupplier { + /** Functional interface for the body of a trigger binding. */ + @FunctionalInterface + private interface BindingBody { + /** + * Executes the body of the binding. + * + * @param previous The previous state of the condition. + * @param current The current state of the condition. + */ + void run(boolean previous, boolean current); + } + private final BooleanSupplier m_condition; private final EventLoop m_loop; @@ -50,26 +62,38 @@ public Trigger(BooleanSupplier condition) { } /** - * Starts the command when the condition changes. + * Adds a binding to the EventLoop. * - * @param command the command to start - * @return this trigger, so calls can be chained + * @param body The body of the binding to add. */ - public Trigger onChange(Command command) { - requireNonNullParam(command, "command", "onChange"); + private void addBinding(BindingBody body) { m_loop.bind( new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); + private boolean m_previous = m_condition.getAsBoolean(); @Override public void run() { - boolean pressed = m_condition.getAsBoolean(); + boolean current = m_condition.getAsBoolean(); - if (m_pressedLast != pressed) { - command.schedule(); - } + body.run(m_previous, current); - m_pressedLast = pressed; + m_previous = current; + } + }); + } + + /** + * Starts the command when the condition changes. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger onChange(Command command) { + requireNonNullParam(command, "command", "onChange"); + addBinding( + (previous, current) -> { + if (previous != current) { + command.schedule(); } }); return this; @@ -83,19 +107,10 @@ public void run() { */ public Trigger onTrue(Command command) { requireNonNullParam(command, "command", "onTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - command.schedule(); - } - - m_pressedLast = pressed; + addBinding( + (previous, current) -> { + if (!previous && current) { + command.schedule(); } }); return this; @@ -109,19 +124,10 @@ public void run() { */ public Trigger onFalse(Command command) { requireNonNullParam(command, "command", "onFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - command.schedule(); - } - - m_pressedLast = pressed; + addBinding( + (previous, current) -> { + if (previous && !current) { + command.schedule(); } }); return this; @@ -139,21 +145,12 @@ public void run() { */ public Trigger whileTrue(Command command) { requireNonNullParam(command, "command", "whileTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - command.schedule(); - } else if (m_pressedLast && !pressed) { - command.cancel(); - } - - m_pressedLast = pressed; + addBinding( + (previous, current) -> { + if (!previous && current) { + command.schedule(); + } else if (previous && !current) { + command.cancel(); } }); return this; @@ -171,21 +168,12 @@ public void run() { */ public Trigger whileFalse(Command command) { requireNonNullParam(command, "command", "whileFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - command.schedule(); - } else if (!m_pressedLast && pressed) { - command.cancel(); - } - - m_pressedLast = pressed; + addBinding( + (previous, current) -> { + if (previous && !current) { + command.schedule(); + } else if (!previous && current) { + command.cancel(); } }); return this; @@ -199,23 +187,14 @@ public void run() { */ public Trigger toggleOnTrue(Command command) { requireNonNullParam(command, "command", "toggleOnTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - if (command.isScheduled()) { - command.cancel(); - } else { - command.schedule(); - } + addBinding( + (previous, current) -> { + if (!previous && current) { + if (command.isScheduled()) { + command.cancel(); + } else { + command.schedule(); } - - m_pressedLast = pressed; } }); return this; @@ -229,23 +208,14 @@ public void run() { */ public Trigger toggleOnFalse(Command command) { requireNonNullParam(command, "command", "toggleOnFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - if (command.isScheduled()) { - command.cancel(); - } else { - command.schedule(); - } + addBinding( + (previous, current) -> { + if (previous && !current) { + if (command.isScheduled()) { + command.cancel(); + } else { + command.schedule(); } - - m_pressedLast = pressed; } }); return this; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index 365cf80a4ea..af8e30a33a5 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -121,6 +121,10 @@ CommandPtr Command::OnlyIf(std::function condition) && { return std::move(*this).ToPtr().OnlyIf(std::move(condition)); } +CommandPtr Command::WithDeadline(CommandPtr&& deadline) && { + return std::move(*this).ToPtr().WithDeadline(std::move(deadline)); +} + CommandPtr Command::DeadlineFor(CommandPtr&& parallel) && { return std::move(*this).ToPtr().DeadlineFor(std::move(parallel)); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp index 298cc2e50ca..d4125fdceb8 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp @@ -168,6 +168,15 @@ CommandPtr CommandPtr::OnlyIf(std::function condition) && { return std::move(*this).Unless(std::not_fn(std::move(condition))); } +CommandPtr CommandPtr::WithDeadline(CommandPtr&& deadline) && { + AssertValid(); + std::vector> vec; + vec.emplace_back(std::move(m_ptr)); + m_ptr = std::make_unique(std::move(deadline).Unwrap(), + std::move(vec)); + return std::move(*this); +} + CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && { AssertValid(); std::vector> vec; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index def91d64879..369e14367bc 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -55,14 +55,6 @@ class CommandScheduler::Impl { wpi::SmallVector interruptActions; wpi::SmallVector finishActions; - // Flag and queues for avoiding concurrent modification if commands are - // scheduled/canceled during run - bool inRunLoop = false; - wpi::SmallVector toSchedule; - wpi::SmallVector toCancelCommands; - wpi::SmallVector, 4> toCancelInterruptors; - wpi::SmallSet endingCommands; - // Map of Command* -> CommandPtr for CommandPtrs transferred to the scheduler // via Schedule(CommandPtr&&). These are erased (destroyed) at the very end of // the loop cycle when the command lifecycle is complete. @@ -118,11 +110,6 @@ frc::EventLoop* CommandScheduler::GetDefaultButtonLoop() const { } void CommandScheduler::Schedule(Command* command) { - if (m_impl->inRunLoop) { - m_impl->toSchedule.emplace_back(command); - return; - } - RequireUngrouped(command); if (m_impl->disabled || m_impl->scheduledCommands.contains(command) || @@ -208,10 +195,13 @@ void CommandScheduler::Run() { loopCache->Poll(); m_watchdog.AddEpoch("buttons.Run()"); - m_impl->inRunLoop = true; bool isDisabled = frc::RobotState::IsDisabled(); - // Run scheduled commands, remove finished commands. - for (Command* command : m_impl->scheduledCommands) { + // create a new set to avoid iterator invalidation. + for (Command* command : wpi::SmallSet(m_impl->scheduledCommands)) { + if (!IsScheduled(command)) { + continue; // skip as the normal scheduledCommands was modified + } + if (isDisabled && !command->RunsWhenDisabled()) { Cancel(command, std::nullopt); continue; @@ -224,14 +214,12 @@ void CommandScheduler::Run() { m_watchdog.AddEpoch(command->GetName() + ".Execute()"); if (command->IsFinished()) { - m_impl->endingCommands.insert(command); + m_impl->scheduledCommands.erase(command); command->End(false); for (auto&& action : m_impl->finishActions) { action(*command); } - m_impl->endingCommands.erase(command); - m_impl->scheduledCommands.erase(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); } @@ -241,19 +229,6 @@ void CommandScheduler::Run() { m_impl->ownedCommands.erase(command); } } - m_impl->inRunLoop = false; - - for (Command* command : m_impl->toSchedule) { - Schedule(command); - } - - for (size_t i = 0; i < m_impl->toCancelCommands.size(); i++) { - Cancel(m_impl->toCancelCommands[i], m_impl->toCancelInterruptors[i]); - } - - m_impl->toSchedule.clear(); - m_impl->toCancelCommands.clear(); - m_impl->toCancelInterruptors.clear(); // Add default commands for un-required registered subsystems. for (auto&& subsystem : m_impl->subsystems) { @@ -346,24 +321,14 @@ void CommandScheduler::Cancel(Command* command, if (!m_impl) { return; } - if (m_impl->endingCommands.contains(command)) { - return; - } - if (m_impl->inRunLoop) { - m_impl->toCancelCommands.emplace_back(command); - m_impl->toCancelInterruptors.emplace_back(interruptor); - return; - } if (!IsScheduled(command)) { return; } - m_impl->endingCommands.insert(command); + m_impl->scheduledCommands.erase(command); command->End(true); for (auto&& action : m_impl->interruptActions) { action(*command, interruptor); } - m_impl->endingCommands.erase(command); - m_impl->scheduledCommands.erase(command); for (auto&& requirement : m_impl->requirements) { if (requirement.second == command) { m_impl->requirements.erase(requirement.first); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp index a3b02d18f3a..00f179da084 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -15,159 +15,117 @@ using namespace frc2; Trigger::Trigger(const Trigger& other) = default; -Trigger Trigger::OnChange(Command* command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); +void Trigger::AddBinding(wpi::unique_function&& body) { + m_loop->Bind([condition = m_condition, previous = m_condition(), + body = std::move(body)]() mutable { + bool current = condition(); - if (previous != current) { - command->Schedule(); - } + body(previous, current); + + previous = current; + }); +} - previous = current; - }); +Trigger Trigger::OnChange(Command* command) { + AddBinding([command](bool previous, bool current) { + if (previous != current) { + command->Schedule(); + } + }); return *this; } Trigger Trigger::OnChange(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (previous != current) { command.Schedule(); } - - previous = current; }); return *this; } Trigger Trigger::OnTrue(Command* command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (!previous && current) { - command->Schedule(); - } - - previous = current; - }); + AddBinding([command](bool previous, bool current) { + if (!previous && current) { + command->Schedule(); + } + }); return *this; } Trigger Trigger::OnTrue(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (!previous && current) { command.Schedule(); } - - previous = current; }); return *this; } Trigger Trigger::OnFalse(Command* command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (previous && !current) { - command->Schedule(); - } - - previous = current; - }); + AddBinding([command](bool previous, bool current) { + if (previous && !current) { + command->Schedule(); + } + }); return *this; } Trigger Trigger::OnFalse(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (previous && !current) { command.Schedule(); } - - previous = current; }); return *this; } Trigger Trigger::WhileTrue(Command* command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (!previous && current) { - command->Schedule(); - } else if (previous && !current) { - command->Cancel(); - } - - previous = current; - }); + AddBinding([command](bool previous, bool current) { + if (!previous && current) { + command->Schedule(); + } else if (previous && !current) { + command->Cancel(); + } + }); return *this; } Trigger Trigger::WhileTrue(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (!previous && current) { command.Schedule(); } else if (previous && !current) { command.Cancel(); } - - previous = current; }); return *this; } Trigger Trigger::WhileFalse(Command* command) { - m_loop->Bind( - [condition = m_condition, previous = m_condition(), command]() mutable { - bool current = condition(); - - if (previous && !current) { - command->Schedule(); - } else if (!previous && current) { - command->Cancel(); - } - - previous = current; - }); + AddBinding([command](bool previous, bool current) { + if (previous && !current) { + command->Schedule(); + } else if (!previous && current) { + command->Cancel(); + } + }); return *this; } Trigger Trigger::WhileFalse(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (!previous && current) { command.Schedule(); } else if (previous && !current) { command.Cancel(); } - - previous = current; }); return *this; } Trigger Trigger::ToggleOnTrue(Command* command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = command]() mutable { - bool current = condition(); - + AddBinding([command](bool previous, bool current) { if (!previous && current) { if (command->IsScheduled()) { command->Cancel(); @@ -175,17 +133,12 @@ Trigger Trigger::ToggleOnTrue(Command* command) { command->Schedule(); } } - - previous = current; }); return *this; } Trigger Trigger::ToggleOnTrue(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (!previous && current) { if (command.IsScheduled()) { command.Cancel(); @@ -193,17 +146,12 @@ Trigger Trigger::ToggleOnTrue(CommandPtr&& command) { command.Schedule(); } } - - previous = current; }); return *this; } Trigger Trigger::ToggleOnFalse(Command* command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = command]() mutable { - bool current = condition(); - + AddBinding([command](bool previous, bool current) { if (previous && !current) { if (command->IsScheduled()) { command->Cancel(); @@ -211,17 +159,12 @@ Trigger Trigger::ToggleOnFalse(Command* command) { command->Schedule(); } } - - previous = current; }); return *this; } Trigger Trigger::ToggleOnFalse(CommandPtr&& command) { - m_loop->Bind([condition = m_condition, previous = m_condition(), - command = std::move(command)]() mutable { - bool current = condition(); - + AddBinding([command = std::move(command)](bool previous, bool current) { if (previous && !current) { if (command.IsScheduled()) { command.Cancel(); @@ -229,8 +172,6 @@ Trigger Trigger::ToggleOnFalse(CommandPtr&& command) { command.Schedule(); } } - - previous = current; }); return *this; } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index c4af1afe81b..4dea3eb1a5f 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -309,6 +309,16 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { [[nodiscard]] CommandPtr OnlyIf(std::function condition) &&; + /** + * Creates a new command that runs this command and the deadline in parallel, + * finishing (and interrupting this command) when the deadline finishes. + * + * @param deadline the deadline of the command group + * @return the decorated command + * @see DeadlineFor + */ + CommandPtr WithDeadline(CommandPtr&& deadline) &&; + /** * Decorates this command with a set of commands to run parallel to it, ending * when the calling command ends and interrupting all the others. Often more @@ -318,9 +328,11 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param parallel the commands to run in parallel. Note the parallel commands * will be interupted when the deadline command ends * @return the decorated command + * @see WithDeadline */ [[nodiscard]] CommandPtr DeadlineFor(CommandPtr&& parallel) &&; + /** * Decorates this command with a set of commands to run parallel to it, ending * when the last command ends. Often more convenient/less-verbose than diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h index e2f534f7547..81a255a9a73 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h @@ -182,6 +182,16 @@ class CommandPtr final { [[nodiscard]] CommandPtr OnlyIf(std::function condition) &&; + /** + * Creates a new command that runs this command and the deadline in parallel, + * finishing (and interrupting this command) when the deadline finishes. + * + * @param deadline the deadline of the command group + * @return the decorated command + * @see DeadlineFor + */ + CommandPtr WithDeadline(CommandPtr&& deadline) &&; + /** * Decorates this command with a set of commands to run parallel to it, ending * when the calling command ends and interrupting all the others. Often more diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h index 714427d1045..353effe9dca 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h @@ -41,12 +41,11 @@ class ProxyCommand : public CommandHelper { * confusing and opens potential footguns for users who do not fully * understand the semantics and implications of proxying, but who simply want * runtime construction. Users who do know what they are doing and need a - * supplier-constructed proxied command should instead proxy a DeferredCommand - * using the AsProxy decorator. + * supplier-constructed proxied command should instead defer a proxy command. * @see DeferredCommand */ WPI_IGNORE_DEPRECATED - [[deprecated("Proxy a DeferredCommand instead")]] + [[deprecated("Defer a proxy command instead.")]] explicit ProxyCommand(wpi::unique_function supplier); /** @@ -62,11 +61,10 @@ class ProxyCommand : public CommandHelper { * confusing and opens potential footguns for users who do not fully * understand the semantics and implications of proxying, but who simply want * runtime construction. Users who do know what they are doing and need a - * supplier-constructed proxied command should instead proxy a DeferredCommand - * using the AsProxy decorator. + * supplier-constructed proxied command should instead defer a proxy command. * @see DeferredCommand */ - [[deprecated("Proxy a DeferredCommand instead")]] + [[deprecated("Defer a proxy command instead.")]] explicit ProxyCommand(wpi::unique_function supplier); WPI_UNIGNORE_DEPRECATED diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index a2382631334..9e86b1b7004 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h @@ -11,6 +11,7 @@ #include #include #include +#include #include "frc2/command/Command.h" #include "frc2/command/CommandScheduler.h" @@ -291,6 +292,13 @@ class Trigger { bool Get() const; private: + /** + * Adds a binding to the EventLoop. + * + * @param body The body of the binding to add. + */ + void AddBinding(wpi::unique_function&& body); + frc::EventLoop* m_loop; std::function m_condition; }; diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java index 1f9605d010c..4d9ad2c06f9 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java @@ -271,6 +271,57 @@ void deadlineForOrderTest() { } } + @Test + void withDeadlineTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean finish = new AtomicBoolean(false); + + Command endsBeforeGroup = Commands.none().withDeadline(Commands.waitUntil(finish::get)); + scheduler.schedule(endsBeforeGroup); + scheduler.run(); + assertTrue(scheduler.isScheduled(endsBeforeGroup)); + finish.set(true); + scheduler.run(); + assertFalse(scheduler.isScheduled(endsBeforeGroup)); + finish.set(false); + + Command endsAfterGroup = Commands.idle().withDeadline(Commands.waitUntil(finish::get)); + scheduler.schedule(endsAfterGroup); + scheduler.run(); + assertTrue(scheduler.isScheduled(endsAfterGroup)); + finish.set(true); + scheduler.run(); + assertFalse(scheduler.isScheduled(endsAfterGroup)); + } + } + + @Test + void withDeadlineOrderTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean dictatorHasRun = new AtomicBoolean(false); + AtomicBoolean dictatorWasPolled = new AtomicBoolean(false); + Command dictator = + new FunctionalCommand( + () -> {}, + () -> dictatorHasRun.set(true), + interrupted -> {}, + () -> { + dictatorWasPolled.set(true); + return true; + }); + Command other = + Commands.run( + () -> + assertAll( + () -> assertTrue(dictatorHasRun.get()), + () -> assertTrue(dictatorWasPolled.get()))); + Command group = other.withDeadline(dictator); + scheduler.schedule(group); + scheduler.run(); + assertAll(() -> assertTrue(dictatorHasRun.get()), () -> assertTrue(dictatorWasPolled.get())); + } + } + @Test void alongWithTest() { try (CommandScheduler scheduler = new CommandScheduler()) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp index 3a31308a3f0..a0b7726f5d6 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -221,6 +221,27 @@ TEST_F(CommandDecoratorTest, DeadlineFor) { EXPECT_FALSE(scheduler.IsScheduled(group)); } +TEST_F(CommandDecoratorTest, WithDeadline) { + CommandScheduler scheduler = GetScheduler(); + + bool finish = false; + + auto dictator = WaitUntilCommand([&finish] { return finish; }); + auto endsAfter = WaitUntilCommand([] { return false; }); + + auto group = std::move(endsAfter).WithDeadline(std::move(dictator).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(group)); + + finish = true; + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(group)); +} + TEST_F(CommandDecoratorTest, AlongWith) { CommandScheduler scheduler = GetScheduler(); @@ -283,6 +304,33 @@ TEST_F(CommandDecoratorTest, DeadlineForOrder) { EXPECT_TRUE(dictatorWasPolled); } +TEST_F(CommandDecoratorTest, WithDeadlineOrder) { + CommandScheduler scheduler = GetScheduler(); + + bool dictatorHasRun = false; + bool dictatorWasPolled = false; + + auto dictator = + FunctionalCommand([] {}, [&dictatorHasRun] { dictatorHasRun = true; }, + [](bool interrupted) {}, + [&dictatorWasPolled] { + dictatorWasPolled = true; + return true; + }); + auto other = RunCommand([&dictatorHasRun, &dictatorWasPolled] { + EXPECT_TRUE(dictatorHasRun); + EXPECT_TRUE(dictatorWasPolled); + }); + + auto group = std::move(other).WithDeadline(std::move(dictator).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(dictatorHasRun); + EXPECT_TRUE(dictatorWasPolled); +} + TEST_F(CommandDecoratorTest, AlongWithOrder) { CommandScheduler scheduler = GetScheduler(); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index 91786770e09..840c05bb213 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -6,6 +6,9 @@ #include #include "CommandTestBase.h" +#include "frc2/command/FunctionalCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/RunCommand.h" using namespace frc2; class CommandScheduleTest : public CommandTestBase {}; @@ -95,6 +98,51 @@ TEST_F(CommandScheduleTest, SchedulerCancel) { EXPECT_FALSE(scheduler.IsScheduled(&command)); } +TEST_F(CommandScheduleTest, CommandKnowsWhenItEnded) { + CommandScheduler scheduler = GetScheduler(); + + frc2::FunctionalCommand* commandPtr = nullptr; + auto command = frc2::FunctionalCommand( + [] {}, [] {}, + [&](auto isForced) { + EXPECT_FALSE(scheduler.IsScheduled(commandPtr)) + << "Command shouldn't be scheduled when its end is called"; + }, + [] { return true; }); + commandPtr = &command; + + scheduler.Schedule(commandPtr); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(commandPtr)) + << "Command should be removed from scheduler when its isFinished() " + "returns true"; +} + +TEST_F(CommandScheduleTest, ScheduleCommandInCommand) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + frc2::InstantCommand commandToGetScheduled{[&counter] { counter++; }}; + + auto command = + frc2::RunCommand([&counter, &scheduler, &commandToGetScheduled] { + scheduler.Schedule(&commandToGetScheduled); + EXPECT_EQ(counter, 1) + << "Scheduled command's init was not run immediately " + "after getting scheduled"; + }); + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_EQ(counter, 1) << "Command 2 was not run when it should have been"; + EXPECT_TRUE(scheduler.IsScheduled(&commandToGetScheduled)) + << "Command 2 was not added to scheduler"; + + scheduler.Run(); + EXPECT_EQ(counter, 1) << "Command 2 was run when it shouldn't have been"; + EXPECT_FALSE(scheduler.IsScheduled(&commandToGetScheduled)) + << "Command 2 did not end when it should have"; +} + TEST_F(CommandScheduleTest, NotScheduledCancel) { CommandScheduler scheduler = GetScheduler(); MockCommand command; diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp index 52436b97ac6..0b54c589515 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp @@ -339,6 +339,45 @@ TEST_F(SchedulingRecursionTest, CancelDefaultCommandFromEnd) { EXPECT_TRUE(scheduler.IsScheduled(&other)); } +TEST_F(SchedulingRecursionTest, CancelNextCommandFromCommand) { + CommandScheduler scheduler = GetScheduler(); + + frc2::RunCommand* command1Ptr = nullptr; + frc2::RunCommand* command2Ptr = nullptr; + int counter = 0; + + auto command1 = frc2::RunCommand([&counter, &command2Ptr, &scheduler] { + scheduler.Cancel(command2Ptr); + counter++; + }); + auto command2 = frc2::RunCommand([&counter, &command1Ptr, &scheduler] { + scheduler.Cancel(command1Ptr); + counter++; + }); + + command1Ptr = &command1; + command2Ptr = &command2; + + scheduler.Schedule(&command1); + scheduler.Schedule(&command2); + scheduler.Run(); + + EXPECT_EQ(counter, 1) << "Second command was run when it shouldn't have been"; + + // only one of the commands should be canceled. + EXPECT_FALSE(scheduler.IsScheduled(&command1) && + scheduler.IsScheduled(&command2)) + << "Both commands are running when only one should be"; + // one of the commands shouldn't be canceled because the other one is canceled + // first + EXPECT_TRUE(scheduler.IsScheduled(&command1) || + scheduler.IsScheduled(&command2)) + << "Both commands are canceled when only one should be"; + + scheduler.Run(); + EXPECT_EQ(counter, 2); +} + INSTANTIATE_TEST_SUITE_P( SchedulingRecursionTests, SchedulingRecursionTest, testing::Values(Command::InterruptionBehavior::kCancelSelf, diff --git a/wpilibc/BUILD.bazel b/wpilibc/BUILD.bazel new file mode 100644 index 00000000000..c11298d3d57 --- /dev/null +++ b/wpilibc/BUILD.bazel @@ -0,0 +1,80 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") +load("//shared/bazel/rules/gen:gen-version-file.bzl", "generate_version_file") + +generate_version_file( + name = "generate-version", + output_file = "WPILibVersion.cpp", + template = "src/generate/WPILibVersion.cpp.in", + visibility = ["//wpilibc:__subpackages__"], +) + +cc_library( + name = "generated_cc_headers", + hdrs = glob(["src/generated/main/native/include/**"]), + includes = ["src/generated/main/native/include"], + strip_include_prefix = "src/generated/main/native/include", + visibility = ["//wpilibc:__subpackages__"], +) + +filegroup( + name = "generated_cc_source", + srcs = glob( + ["src/generated/main/native/cpp/**"], + exclude = ["src/generated/main/native/cpp/jni/**"], + ), + visibility = ["//wpilibc:__subpackages__"], +) + +cc_library( + name = "wpilibc.static", + srcs = [ + ":generate-version", + ] + glob([ + "src/main/native/cppcs/**", + "src/main/native/cpp/**", + ]) + [":generated_cc_source"], + hdrs = glob(["src/main/native/include/**"]), + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + ":generated_cc_headers", + "//cameraserver:cameraserver.static", + "//cscore:cscore.static", + "//hal:wpiHal.static", + "//ntcore:ntcore.static", + "//wpimath:wpimath.static", + "//wpinet:wpinet.static", + "//wpiutil:wpiutil.static", + ], +) + +cc_library( + name = "test-headers", + testonly = True, + hdrs = glob(["src/test/native/include/**"]), + includes = ["src/test/native/include"], +) + +cc_test( + name = "wpilibc-test", + size = "small", + srcs = glob(["src/test/native/cpp/**"]), + tags = [ + "no-asan", + "no-tsan", + "no-ubsan", + ], + deps = [ + ":test-headers", + ":wpilibc.static", + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + ":wpilibc.static", + ], +) diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index ea40670bb0e..8723e307cae 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -759,11 +759,11 @@ }, { "name": "SimpleDifferentialDriveSimulation", - "description": "Simulate a differential drivetrain and follow trajectories with RamseteController (non-command-based).", + "description": "Simulate a differential drivetrain and follow trajectories with LTVUnicycleController (non-command-based).", "tags": [ "Differential Drive", "State-Space", - "Ramsete", + "LTVUnicycleController", "Path Following", "Trajectory", "Encoder", diff --git a/wpilibcIntegrationTests/BUILD.bazel b/wpilibcIntegrationTests/BUILD.bazel new file mode 100644 index 00000000000..c67b2dd969b --- /dev/null +++ b/wpilibcIntegrationTests/BUILD.bazel @@ -0,0 +1,24 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library") + +ATHENA_SOURCES = glob(["src/main/native/cpp/**"]) + +NON_ATHENA_SOURCES = glob(["src/main/native/dt/**"]) + +cc_library( + name = "test_headers", + hdrs = glob(["src/main/native/include/**"]), + strip_include_prefix = "src/main/native/include", +) + +cc_binary( + name = "wpilibcIntegrationTests", + srcs = select({ + "@rules_bzlmodrio_toolchains//constraints/is_roborio:roborio": ATHENA_SOURCES, + "//conditions:default": NON_ATHENA_SOURCES, + }), + deps = [ + ":test_headers", + "//thirdparty/googletest:googletest.static", + "//wpilibc:wpilibc.static", + ], +) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java index dedd790b45f..127fb94f4f0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java @@ -96,8 +96,7 @@ public void removeRef() { private static DataStore getForModule(int module) { synchronized (m_handleLock) { - Integer moduleBoxed = module; - DataStore pcm = m_handleMap.get(moduleBoxed); + DataStore pcm = m_handleMap.get(module); if (pcm == null) { pcm = new DataStore(module); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java index 4f1d70c5161..c075c46a755 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java @@ -47,8 +47,7 @@ public void removeRef() { private static DataStore getForModule(int module) { synchronized (m_handleLock) { - Integer moduleBoxed = module; - DataStore pcm = m_handleMap.get(moduleBoxed); + DataStore pcm = m_handleMap.get(module); if (pcm == null) { pcm = new DataStore(module); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index b351d169db4..adc52e2ef67 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.differentialdrivebot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -82,10 +79,8 @@ public Drivetrain() { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 7df9f3b4496..fbf6e90b9c9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.ComputerVisionUtil; @@ -142,10 +139,8 @@ public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index 67fea4c8ae0..b8947ce6da9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; @@ -99,18 +96,12 @@ public void setDriveStates( m_leftLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentLeft.position, - m_feedforward - .calculate( - MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) - .in(Volts) + m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); m_rightLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentRight.position, - m_feedforward - .calculate( - MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) - .in(Volts) + m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java index 83958230ec6..373485923c5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.ExponentialProfile; import edu.wpi.first.wpilibj.Joystick; @@ -48,7 +45,7 @@ public void teleopPeriodic() { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, - m_feedforward.calculate(RadiansPerSecond.of(next.velocity)).in(Volts) / 12.0); + m_feedforward.calculate(next.velocity) / 12.0); m_setpoint = next; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java index e1ae800421d..2ab9560dcb8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Joystick; @@ -48,6 +45,6 @@ public void teleopPeriodic() { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position, - m_feedforward.calculate(MetersPerSecond.of(m_setpoint.velocity)).in(Volts) / 12.0); + m_feedforward.calculate(m_setpoint.velocity) / 12.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java index e75edfe3d19..f9646e282a3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.eventloop; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.Encoder; @@ -67,7 +64,7 @@ public Robot() { () -> { m_shooter.setVoltage( m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY) - + m_ff.calculate(RPM.of(SHOT_VELOCITY)).in(Volts)); + + m_ff.calculate(SHOT_VELOCITY)); }); // if not, stop diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 3ed52e1bef9..1abd22855bf 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -650,11 +650,11 @@ }, { "name": "SimpleDifferentialDriveSimulation", - "description": "Simulate a differential drivetrain and follow trajectories with RamseteController (non-command-based).", + "description": "Simulate a differential drivetrain and follow trajectories with LTVUnicycleController (non-command-based).", "tags": [ "Differential Drive", "State-Space", - "Ramsete", + "LTVUnicycleController", "Path Following", "Trajectory", "Encoder", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index abba8a7365c..76fc4317594 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; @@ -89,8 +86,7 @@ public void teleopPeriodic() { // Controls a motor with the output of the BangBang controller and a // feedforward. The feedforward is reduced slightly to avoid overspeeding // the shooter. - m_flywheelMotor.setVoltage( - bangOutput + 0.9 * m_feedforward.calculate(RadiansPerSecond.of(setpoint)).in(Volts)); + m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint)); } /** Update our simulation. This should be run every robot loop in simulation. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index addedc0a9a7..c64c9b3958a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.mecanumbot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Translation2d; @@ -98,14 +95,10 @@ public MecanumDriveWheelPositions getCurrentDistances() { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts); - final double frontRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts); - final double backLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts); - final double backRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); final double frontLeftOutput = m_frontLeftPIDController.calculate( @@ -136,12 +129,14 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if (fieldRelative) { - chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d()); - } - chassisSpeeds.discretize(periodSeconds); - var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + var mecanumDriveWheelSpeeds = + m_kinematics.toWheelSpeeds( + ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds)); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); setSpeeds(mecanumDriveWheelSpeeds); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index b3f9b3462ab..d6ff1eb734f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -5,7 +5,6 @@ package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; import edu.wpi.first.math.kinematics.MecanumDriveOdometry; import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; @@ -125,11 +124,15 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ } /** Sets the front left drive MotorController to a voltage. */ - public void setDriveMotorControllersVolts(MecanumDriveMotorVoltages volts) { - m_frontLeft.setVoltage(volts.frontLeftVoltage); - m_rearLeft.setVoltage(volts.rearLeftVoltage); - m_frontRight.setVoltage(volts.frontRightVoltage); - m_rearRight.setVoltage(volts.rearRightVoltage); + public void setDriveMotorControllersVolts( + double frontLeftVoltage, + double frontRightVoltage, + double rearLeftVoltage, + double rearRightVoltage) { + m_frontLeft.setVoltage(frontLeftVoltage); + m_rearLeft.setVoltage(rearLeftVoltage); + m_frontRight.setVoltage(frontRightVoltage); + m_rearRight.setVoltage(rearRightVoltage); } /** Resets the drive encoders to currently read a position of 0. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index ada2b6d83e0..256824c03b6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -110,14 +107,10 @@ public MecanumDriveWheelPositions getCurrentDistances() { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts); - final double frontRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts); - final double backLeftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts); - final double backRightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); final double frontLeftOutput = m_frontLeftPIDController.calculate( @@ -148,12 +141,14 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if (fieldRelative) { - chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation()); - } - chassisSpeeds.discretize(periodSeconds); - var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); + var mecanumDriveWheelSpeeds = + m_kinematics.toWheelSpeeds( + ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds)); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); setSpeeds(mecanumDriveWheelSpeeds); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index 7e9e8e0370a..dbb234a8d0a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -143,9 +140,7 @@ public Command turnToAngleCommand(double angleDeg) { 0, m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg) // Divide feedforward voltage by battery voltage to normalize it to [-1, 1] - + m_feedforward - .calculate(DegreesPerSecond.of(m_controller.getSetpoint().velocity)) - .in(Volts) + + m_feedforward.calculate(m_controller.getSetpoint().velocity) / RobotController.getBatteryVoltage())) .until(m_controller::atGoal) .finallyDo(() -> m_drive.arcadeDrive(0, 0)); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java index a10f6f91436..39023c2e8ef 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.wpilibj2.command.Commands.parallel; import static edu.wpi.first.wpilibj2.command.Commands.waitUntil; @@ -60,9 +58,7 @@ public Command shootCommand(double setpointRotationsPerSecond) { run( () -> { m_shooterMotor.set( - m_shooterFeedforward - .calculate(RotationsPerSecond.of(setpointRotationsPerSecond)) - .in(Volts) + m_shooterFeedforward.calculate(setpointRotationsPerSecond) + m_shooterFeedback.calculate( m_shooterEncoder.getRate(), setpointRotationsPerSecond)); }), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index eb911054a58..d0d1ed4dcdc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -4,9 +4,6 @@ package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; @@ -97,10 +94,8 @@ public Drivetrain() { /** Sets speeds to the drivetrain motors. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts); - final double rightFeedforward = - m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts); + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); double rightOutput = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index f622259b494..958e5a3e617 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -57,12 +57,14 @@ public Drivetrain() { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if (fieldRelative) { - chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d()); - } - chassisSpeeds.discretize(periodSeconds); - var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds); + var swerveModuleStates = + m_kinematics.toSwerveModuleStates( + ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds)); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index eb95c4c52ca..b57ba6e5eae 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -4,10 +4,6 @@ package edu.wpi.first.wpilibj.examples.swervebot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -127,10 +123,7 @@ public void setDesiredState(SwerveModuleState desiredState) { final double driveOutput = m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); - final double driveFeedforward = - m_driveFeedforward - .calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)) - .in(Volts); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); // Calculate the turning motor output from the turning PID controller. final double turnOutput = @@ -138,9 +131,7 @@ public void setDesiredState(SwerveModuleState desiredState) { m_turningEncoder.getDistance(), desiredState.angle.getRadians()); final double turnFeedforward = - m_turnFeedforward - .calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity)) - .in(Volts); + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); m_driveMotor.setVoltage(driveOutput + driveFeedforward); m_turningMotor.setVoltage(turnOutput + turnFeedforward); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index c950e4c7c53..04ee2998df6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -118,12 +118,14 @@ public void resetOdometry(Pose2d pose) { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if (fieldRelative) { - chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d()); - } - chassisSpeeds.discretize(DriveConstants.kDrivePeriod); - var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds); + var swerveModuleStates = + DriveConstants.kDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + DriveConstants.kDrivePeriod)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); m_frontLeft.setDesiredState(swerveModuleStates[0]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index a28dfcacc28..c232bb19a7b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -36,11 +36,8 @@ public class Drivetrain { new SwerveDriveKinematics( m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); - /* - * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. - * The numbers used - * below are robot specific, and should be tuned. - */ + /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used + below are robot specific, and should be tuned. */ private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( m_kinematics, @@ -69,12 +66,14 @@ public Drivetrain() { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if (fieldRelative) { - chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation()); - } - chassisSpeeds.discretize(periodSeconds); - var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds); + var swerveModuleStates = + m_kinematics.toSwerveModuleStates( + ChassisSpeeds.discretize( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) + : new ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds)); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); @@ -93,8 +92,7 @@ public void updateOdometry() { m_backRight.getPosition() }); - // Also apply vision measurements. We use 0.3 seconds in the past as an example - // -- on + // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on // a real robot, this must be calculated based either on latency or timestamps. m_poseEstimator.addVisionMeasurement( ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index f6a9b46bdd8..e8af157d37b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -4,10 +4,6 @@ package edu.wpi.first.wpilibj.examples.swervedriveposeestimator; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -126,10 +122,7 @@ public void setDesiredState(SwerveModuleState desiredState) { final double driveOutput = m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); - final double driveFeedforward = - m_driveFeedforward - .calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond)) - .in(Volts); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); // Calculate the turning motor output from the turning PID controller. final double turnOutput = @@ -137,9 +130,7 @@ public void setDesiredState(SwerveModuleState desiredState) { m_turningEncoder.getDistance(), desiredState.angle.getRadians()); final double turnFeedforward = - m_turnFeedforward - .calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity)) - .in(Volts); + m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity); m_driveMotor.setVoltage(driveOutput + driveFeedforward); m_turningMotor.setVoltage(turnOutput + turnFeedforward); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java index cc7fbac2fe4..affceda1443 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java @@ -95,9 +95,7 @@ public Command runShooter(DoubleSupplier shooterSpeed) { return run(() -> { m_shooterMotor.setVoltage( m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble()) - + m_shooterFeedforward - .calculate(RotationsPerSecond.of(shooterSpeed.getAsDouble())) - .in(Volts)); + + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble())); m_feederMotor.set(ShooterConstants.kFeederSpeed); }) .finallyDo( diff --git a/wpilibjIntegrationTests/build.gradle b/wpilibjIntegrationTests/build.gradle index b79337aff8c..b9167369648 100644 --- a/wpilibjIntegrationTests/build.gradle +++ b/wpilibjIntegrationTests/build.gradle @@ -15,7 +15,7 @@ application { mainClass = 'edu.wpi.first.wpilibj.test.AntJunitLauncher' } -apply plugin: 'com.github.johnrengelman.shadow' +apply plugin: 'com.gradleup.shadow' repositories { maven { diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java index 793c7e939e2..9fa5e0aae77 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java @@ -26,8 +26,8 @@ public class CounterTest extends AbstractComsSetup { private static FakeCounterFixture counter = null; private static final Logger logger = Logger.getLogger(CounterTest.class.getName()); - Integer m_input; - Integer m_output; + int m_input; + int m_output; @Override protected Logger getClassLogger() { @@ -40,10 +40,7 @@ protected Logger getClassLogger() { * @param input The input Port * @param output The output Port */ - public CounterTest(Integer input, Integer output) { - assert input != null; - assert output != null; - + public CounterTest(int input, int output) { m_input = input; m_output = output; // System.out.println("Counter Test: Input: " + input + " Output: " + diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java index 97405ce5cab..9a3ee5c8017 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java @@ -39,7 +39,7 @@ protected Logger getClassLogger() { * @param input The port for the input wire * @param output The port for the output wire */ - public DIOCrossConnectTest(Integer input, Integer output) { + public DIOCrossConnectTest(int input, int output) { if (dio != null) { dio.teardown(); } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java index d668d16d5a0..eb7fda51794 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java @@ -51,7 +51,7 @@ public static void tearDownAfterClass() { * @param mef Motor encoder fixture. * @param expectedCurrentDraw Expected current draw in Amps. */ - public PDPTest(MotorEncoderFixture mef, Double expectedCurrentDraw) { + public PDPTest(MotorEncoderFixture mef, double expectedCurrentDraw) { logger.fine("Constructor with: " + mef.getType()); if (me != null && !me.equals(mef)) { me.teardown(); diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java index 909194556c8..90c41eef97d 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java @@ -41,9 +41,9 @@ public class PIDTest extends AbstractComsSetup { private PIDController m_controller = null; private static MotorEncoderFixture me = null; - private final Double m_p; - private final Double m_i; - private final Double m_d; + private final double m_p; + private final double m_i; + private final double m_d; @Override protected Logger getClassLogger() { @@ -58,7 +58,7 @@ protected Logger getClassLogger() { * @param d D gain. * @param mef Motor encoder fixture. */ - public PIDTest(Double p, Double i, Double d, MotorEncoderFixture mef) { + public PIDTest(double p, double i, double d, MotorEncoderFixture mef) { logger.fine("Constructor with: " + mef.getType()); if (PIDTest.me != null && !PIDTest.me.equals(mef)) { PIDTest.me.teardown(); diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java index 78880d84a71..03aacb6a7da 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java @@ -37,10 +37,8 @@ public DIOCrossConnectFixture(DigitalInput input, DigitalOutput output) { * @param input The port of the {@link DigitalInput} * @param output The port of the {@link DigitalOutput} */ - public DIOCrossConnectFixture(Integer input, Integer output) { - assert input != null; - assert output != null; - assert !input.equals(output); + public DIOCrossConnectFixture(int input, int output) { + assert input != output; m_input = new DigitalInput(input); m_output = new DigitalOutput(output); m_allocated = true; diff --git a/wpimath/BUILD.bazel b/wpimath/BUILD.bazel index 2f650293fdb..7979a8ba1cb 100644 --- a/wpimath/BUILD.bazel +++ b/wpimath/BUILD.bazel @@ -1,3 +1,4 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") load("@rules_java//java:defs.bzl", "java_binary", "java_library") load("@rules_python//python:defs.bzl", "py_binary") @@ -7,6 +8,76 @@ filegroup( visibility = ["//wpimath:__subpackages__"], ) +cc_library( + name = "eigen-headers", + hdrs = glob([ + "src/main/native/thirdparty/eigen/include/**", + ]), + includes = ["src/main/native/thirdparty/eigen/include"], + strip_include_prefix = "src/main/native/thirdparty/eigen/include", + visibility = ["//wpimath:__subpackages__"], +) + +cc_library( + name = "gcem", + hdrs = glob([ + "src/main/native/thirdparty/gcem/include/**", + ]), + includes = ["src/main/native/thirdparty/gcem/include"], + strip_include_prefix = "src/main/native/thirdparty/gcem/include", + visibility = ["//wpimath:__subpackages__"], +) + +cc_library( + name = "sleipnir-headers", + hdrs = glob([ + "src/main/native/thirdparty/sleipnir/include/**/*.hpp", + ]), + includes = ["src/main/native/thirdparty/sleipnir/include"], + strip_include_prefix = "src/main/native/thirdparty/sleipnir/include", + visibility = ["//wpimath:__subpackages__"], +) + +filegroup( + name = "sleipnir-srcs", + srcs = glob(["src/main/native/thirdparty/sleipnir/src/**"]), + visibility = ["//wpimath:__subpackages__"], +) + +cc_library( + name = "nanopb-generated-headers", + hdrs = glob(["src/generated/main/native/cpp/**/*.h"]), + includes = ["src/generated/main/native/cpp"], + strip_include_prefix = "src/generated/main/native/cpp", + visibility = ["//wpiutil:__subpackages__"], +) + +cc_library( + name = "wpimath.static", + srcs = glob( + [ + "src/main/native/cpp/**", + "src/generated/main/native/cpp/**/*.cpp", + ], + exclude = ["src/main/native/cpp/jni/**"], + ) + [":sleipnir-srcs"], + hdrs = glob(["src/main/native/include/**"]), + defines = ["WPILIB_EXPORTS"], + includes = [ + "src/main/native/include", + "src/main/native/thirdparty/sleipnir/src", + ], + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + ":eigen-headers", + ":gcem", + ":nanopb-generated-headers", + ":sleipnir-headers", + "//wpiutil:wpiutil.static", + ], +) + java_library( name = "wpimath-java", srcs = [":generated_java"] + glob(["src/main/java/**/*.java"]), @@ -24,6 +95,40 @@ java_library( ], ) +cc_library( + name = "test_headers", + hdrs = glob([ + "src/test/native/include/**", + ]), + strip_include_prefix = "src/test/native/include", +) + +cc_test( + name = "wpimath-cpp-test", + size = "small", + srcs = glob([ + "src/test/native/cpp/**/*.cpp", + "src/test/native/cpp/**/*.h", + ]), + tags = [ + "no-bullseye", + "no-raspi", + ], + deps = [ + ":test_headers", + ":wpimath.static", + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + ":wpimath.static", + ], +) + java_binary( name = "DevMain-Java", srcs = ["src/dev/java/edu/wpi/first/math/DevMain.java"], diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index 5494f0b6752..8db878799f9 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,7 +19,7 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3427, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3021, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIkYKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh8KC3RyYWNrX3dpZHRo" + @@ -31,53 +31,46 @@ public final class Kinematics { "GAIgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFy" + "X2xlZnQYAyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/Cgpy" + "ZWFyX3JpZ2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0" + - "Ip8BCiFQcm90b2J1Zk1lY2FudW1Ecml2ZU1vdG9yVm9sdGFnZXMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" + - "ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" + - "AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0IqABCiJQcm90b2J1Zk1lY2Fu" + - "dW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9u" + - "dF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJl" + - "YXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJpdmVXaGVlbFNwZWVk" + - "cxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQYAiABKAFSCmZyb250" + - "UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0GAQgASgBUglyZWFy" + - "UmlnaHQiWwodUHJvdG9idWZTd2VydmVEcml2ZUtpbmVtYXRpY3MSOgoHbW9kdWxlcxgBIAMoCzIgLndw", - "aS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSB21vZHVsZXMibwocUHJvdG9idWZTd2VydmVNb2R1" + - "bGVQb3NpdGlvbhIaCghkaXN0YW5jZRgBIAEoAVIIZGlzdGFuY2USMwoFYW5nbGUYAiABKAsyHS53cGku" + - "cHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZSJmChlQcm90b2J1ZlN3ZXJ2ZU1vZHVsZVN0YXRl" + - "EhQKBXNwZWVkGAEgASgBUgVzcGVlZBIzCgVhbmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJv" + - "dGF0aW9uMmRSBWFuZ2xlQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0qNDwoGEgQAAEQBCggKAQwS" + - "AwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIEABIECAAMAQoK" + - "CgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkLCgwKBQQAAgAD" + - "EgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJCwoMCgUEAAIBAxIDCg4P" + - "CgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4KDAoFBAACAgMSAwsREgoKCgIE" + - "ARIEDgAQAQoKCgMEAQESAw4IKwoLCgQEAQIAEgMPAhkKDAoFBAECAAUSAw8CCAoMCgUEAQIAARIDDwkU" + - "CgwKBQQBAgADEgMPFxgKCgoCBAISBBIAFQEKCgoDBAIBEgMSCCwKCwoEBAICABIDEwISCgwKBQQCAgAF" + - "EgMTAggKDAoFBAICAAESAxMJDQoMCgUEAgIAAxIDExARCgsKBAQCAgESAxQCEwoMCgUEAgIBBRIDFAII" + - "CgwKBQQCAgEBEgMUCQ4KDAoFBAICAQMSAxQREgoKCgIEAxIEFwAaAQoKCgMEAwESAxcILwoLCgQEAwIA" + - "EgMYAhIKDAoFBAMCAAUSAxgCCAoMCgUEAwIAARIDGAkNCgwKBQQDAgADEgMYEBEKCwoEBAMCARIDGQIT" + - "CgwKBQQDAgEFEgMZAggKDAoFBAMCAQESAxkJDgoMCgUEAwIBAxIDGRESCgoKAgQEEgQcACEBCgoKAwQE" + - "ARIDHAgmCgsKBAQEAgASAx0CJwoMCgUEBAIABhIDHQIXCgwKBQQEAgABEgMdGCIKDAoFBAQCAAMSAx0l" + - "JgoLCgQEBAIBEgMeAigKDAoFBAQCAQYSAx4CFwoMCgUEBAIBARIDHhgjCgwKBQQEAgEDEgMeJicKCwoE" + - "BAQCAhIDHwImCgwKBQQEAgIGEgMfAhcKDAoFBAQCAgESAx8YIQoMCgUEBAICAxIDHyQlCgsKBAQEAgMS" + - "AyACJwoMCgUEBAIDBhIDIAIXCgwKBQQEAgMBEgMgGCIKDAoFBAQCAwMSAyAlJgoKCgIEBRIEIwAoAQoK" + - "CgMEBQESAyMIKQoLCgQEBQIAEgMkAhgKDAoFBAUCAAUSAyQCCAoMCgUEBQIAARIDJAkTCgwKBQQFAgAD", - "EgMkFhcKCwoEBAUCARIDJQIZCgwKBQQFAgEFEgMlAggKDAoFBAUCAQESAyUJFAoMCgUEBQIBAxIDJRcY" + - "CgsKBAQFAgISAyYCFwoMCgUEBQICBRIDJgIICgwKBQQFAgIBEgMmCRIKDAoFBAUCAgMSAyYVFgoLCgQE" + - "BQIDEgMnAhgKDAoFBAUCAwUSAycCCAoMCgUEBQIDARIDJwkTCgwKBQQFAgMDEgMnFhcKCgoCBAYSBCoA" + - "LwEKCgoDBAYBEgMqCCoKCwoEBAYCABIDKwIYCgwKBQQGAgAFEgMrAggKDAoFBAYCAAESAysJEwoMCgUE" + - "BgIAAxIDKxYXCgsKBAQGAgESAywCGQoMCgUEBgIBBRIDLAIICgwKBQQGAgEBEgMsCRQKDAoFBAYCAQMS" + - "AywXGAoLCgQEBgICEgMtAhcKDAoFBAYCAgUSAy0CCAoMCgUEBgICARIDLQkSCgwKBQQGAgIDEgMtFRYK" + - "CwoEBAYCAxIDLgIYCgwKBQQGAgMFEgMuAggKDAoFBAYCAwESAy4JEwoMCgUEBgIDAxIDLhYXCgoKAgQH" + - "EgQxADYBCgoKAwQHARIDMQgnCgsKBAQHAgASAzICGAoMCgUEBwIABRIDMgIICgwKBQQHAgABEgMyCRMK" + - "DAoFBAcCAAMSAzIWFwoLCgQEBwIBEgMzAhkKDAoFBAcCAQUSAzMCCAoMCgUEBwIBARIDMwkUCgwKBQQH" + - "AgEDEgMzFxgKCwoEBAcCAhIDNAIXCgwKBQQHAgIFEgM0AggKDAoFBAcCAgESAzQJEgoMCgUEBwICAxID" + - "NBUWCgsKBAQHAgMSAzUCGAoMCgUEBwIDBRIDNQIICgwKBQQHAgMBEgM1CRMKDAoFBAcCAwMSAzUWFwoK" + - "CgIECBIEOAA6AQoKCgMECAESAzgIJQoLCgQECAIAEgM5Ai0KDAoFBAgCAAQSAzkCCgoMCgUECAIABhID" + - "OQsgCgwKBQQIAgABEgM5ISgKDAoFBAgCAAMSAzkrLAoKCgIECRIEPAA/AQoKCgMECQESAzwIJAoLCgQE" + - "CQIAEgM9AhYKDAoFBAkCAAUSAz0CCAoMCgUECQIAARIDPQkRCgwKBQQJAgADEgM9FBUKCwoEBAkCARID" + - "PgIfCgwKBQQJAgEGEgM+AhQKDAoFBAkCAQESAz4VGgoMCgUECQIBAxIDPh0eCgoKAgQKEgRBAEQBCgoK" + - "AwQKARIDQQghCgsKBAQKAgASA0ICEwoMCgUECgIABRIDQgIICgwKBQQKAgABEgNCCQ4KDAoFBAoCAAMS" + - "A0IREgoLCgQECgIBEgNDAh8KDAoFBAoCAQYSA0MCFAoMCgUECgIBARIDQxUaCgwKBQQKAgEDEgNDHR5i" + - "BnByb3RvMw=="); + "IqABCiJQcm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFS" + + "CWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyAB" + + "KAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNh" + + "bnVtRHJpdmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRf" + + "cmlnaHQYAiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFy" + + "X3JpZ2h0GAQgASgBUglyZWFyUmlnaHQiWwodUHJvdG9idWZTd2VydmVEcml2ZUtpbmVtYXRpY3MSOgoH" + + "bW9kdWxlcxgBIAMoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSB21vZHVsZXMibwoc" + + "UHJvdG9idWZTd2VydmVNb2R1bGVQb3NpdGlvbhIaCghkaXN0YW5jZRgBIAEoAVIIZGlzdGFuY2USMwoF" + + "YW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZSJmChlQcm90b2J1", + "ZlN3ZXJ2ZU1vZHVsZVN0YXRlEhQKBXNwZWVkGAEgASgBUgVzcGVlZBIzCgVhbmdsZRgCIAEoCzIdLndw" + + "aS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90" + + "b0qZDQoGEgQAAD0BCggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAES" + + "AwYAMQoKCgIEABIECAAMAQoKCgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUE" + + "AAIAARIDCQkLCgwKBQQAAgADEgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQES" + + "AwoJCwoMCgUEAAIBAxIDCg4PCgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4K" + + "DAoFBAACAgMSAwsREgoKCgIEARIEDgAQAQoKCgMEAQESAw4IKwoLCgQEAQIAEgMPAhkKDAoFBAECAAUS" + + "Aw8CCAoMCgUEAQIAARIDDwkUCgwKBQQBAgADEgMPFxgKCgoCBAISBBIAFQEKCgoDBAIBEgMSCCwKCwoE" + + "BAICABIDEwISCgwKBQQCAgAFEgMTAggKDAoFBAICAAESAxMJDQoMCgUEAgIAAxIDExARCgsKBAQCAgES" + + "AxQCEwoMCgUEAgIBBRIDFAIICgwKBQQCAgEBEgMUCQ4KDAoFBAICAQMSAxQREgoKCgIEAxIEFwAaAQoK" + + "CgMEAwESAxcILwoLCgQEAwIAEgMYAhIKDAoFBAMCAAUSAxgCCAoMCgUEAwIAARIDGAkNCgwKBQQDAgAD" + + "EgMYEBEKCwoEBAMCARIDGQITCgwKBQQDAgEFEgMZAggKDAoFBAMCAQESAxkJDgoMCgUEAwIBAxIDGRES" + + "CgoKAgQEEgQcACEBCgoKAwQEARIDHAgmCgsKBAQEAgASAx0CJwoMCgUEBAIABhIDHQIXCgwKBQQEAgAB" + + "EgMdGCIKDAoFBAQCAAMSAx0lJgoLCgQEBAIBEgMeAigKDAoFBAQCAQYSAx4CFwoMCgUEBAIBARIDHhgj" + + "CgwKBQQEAgEDEgMeJicKCwoEBAQCAhIDHwImCgwKBQQEAgIGEgMfAhcKDAoFBAQCAgESAx8YIQoMCgUE" + + "BAICAxIDHyQlCgsKBAQEAgMSAyACJwoMCgUEBAIDBhIDIAIXCgwKBQQEAgMBEgMgGCIKDAoFBAQCAwMS" + + "AyAlJgoKCgIEBRIEIwAoAQoKCgMEBQESAyMIKgoLCgQEBQIAEgMkAhgKDAoFBAUCAAUSAyQCCAoMCgUE" + + "BQIAARIDJAkTCgwKBQQFAgADEgMkFhcKCwoEBAUCARIDJQIZCgwKBQQFAgEFEgMlAggKDAoFBAUCAQES" + + "AyUJFAoMCgUEBQIBAxIDJRcYCgsKBAQFAgISAyYCFwoMCgUEBQICBRIDJgIICgwKBQQFAgIBEgMmCRIK" + + "DAoFBAUCAgMSAyYVFgoLCgQEBQIDEgMnAhgKDAoFBAUCAwUSAycCCAoMCgUEBQIDARIDJwkTCgwKBQQF", + "AgMDEgMnFhcKCgoCBAYSBCoALwEKCgoDBAYBEgMqCCcKCwoEBAYCABIDKwIYCgwKBQQGAgAFEgMrAggK" + + "DAoFBAYCAAESAysJEwoMCgUEBgIAAxIDKxYXCgsKBAQGAgESAywCGQoMCgUEBgIBBRIDLAIICgwKBQQG" + + "AgEBEgMsCRQKDAoFBAYCAQMSAywXGAoLCgQEBgICEgMtAhcKDAoFBAYCAgUSAy0CCAoMCgUEBgICARID" + + "LQkSCgwKBQQGAgIDEgMtFRYKCwoEBAYCAxIDLgIYCgwKBQQGAgMFEgMuAggKDAoFBAYCAwESAy4JEwoM" + + "CgUEBgIDAxIDLhYXCgoKAgQHEgQxADMBCgoKAwQHARIDMQglCgsKBAQHAgASAzICLQoMCgUEBwIABBID" + + "MgIKCgwKBQQHAgAGEgMyCyAKDAoFBAcCAAESAzIhKAoMCgUEBwIAAxIDMissCgoKAgQIEgQ1ADgBCgoK" + + "AwQIARIDNQgkCgsKBAQIAgASAzYCFgoMCgUECAIABRIDNgIICgwKBQQIAgABEgM2CREKDAoFBAgCAAMS" + + "AzYUFQoLCgQECAIBEgM3Ah8KDAoFBAgCAQYSAzcCFAoMCgUECAIBARIDNxUaCgwKBQQIAgEDEgM3HR4K" + + "CgoCBAkSBDoAPQEKCgoDBAkBEgM6CCEKCwoEBAkCABIDOwITCgwKBQQJAgAFEgM7AggKDAoFBAkCAAES" + + "AzsJDgoMCgUECQIAAxIDOxESCgsKBAQJAgESAzwCHwoMCgUECQIBBhIDPAIUCgwKBQQJAgEBEgM8FRoK" + + "DAoFBAkCAQMSAzwdHmIGcHJvdG8z"); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -91,17 +84,15 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(368, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor = descriptor.internalContainedType(663, 159, "ProtobufMecanumDriveMotorVoltages", "wpi.proto.ProtobufMecanumDriveMotorVoltages"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(663, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(825, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(826, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(988, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(985, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1147, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1078, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1240, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); - - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1353, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1191, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); /** * @return this proto file's descriptor. @@ -2043,506 +2034,6 @@ static class FieldNames { } } - /** - * Protobuf type {@code ProtobufMecanumDriveMotorVoltages} - */ - public static final class ProtobufMecanumDriveMotorVoltages extends ProtoMessage implements Cloneable { - private static final long serialVersionUID = 0L; - - /** - * optional double front_left = 1; - */ - private double frontLeft; - - /** - * optional double front_right = 2; - */ - private double frontRight; - - /** - * optional double rear_left = 3; - */ - private double rearLeft; - - /** - * optional double rear_right = 4; - */ - private double rearRight; - - private ProtobufMecanumDriveMotorVoltages() { - } - - /** - * @return a new empty instance of {@code ProtobufMecanumDriveMotorVoltages} - */ - public static ProtobufMecanumDriveMotorVoltages newInstance() { - return new ProtobufMecanumDriveMotorVoltages(); - } - - /** - * optional double front_left = 1; - * @return whether the frontLeft field is set - */ - public boolean hasFrontLeft() { - return (bitField0_ & 0x00000001) != 0; - } - - /** - * optional double front_left = 1; - * @return this - */ - public ProtobufMecanumDriveMotorVoltages clearFrontLeft() { - bitField0_ &= ~0x00000001; - frontLeft = 0D; - return this; - } - - /** - * optional double front_left = 1; - * @return the frontLeft - */ - public double getFrontLeft() { - return frontLeft; - } - - /** - * optional double front_left = 1; - * @param value the frontLeft to set - * @return this - */ - public ProtobufMecanumDriveMotorVoltages setFrontLeft(final double value) { - bitField0_ |= 0x00000001; - frontLeft = value; - return this; - } - - /** - * optional double front_right = 2; - * @return whether the frontRight field is set - */ - public boolean hasFrontRight() { - return (bitField0_ & 0x00000002) != 0; - } - - /** - * optional double front_right = 2; - * @return this - */ - public ProtobufMecanumDriveMotorVoltages clearFrontRight() { - bitField0_ &= ~0x00000002; - frontRight = 0D; - return this; - } - - /** - * optional double front_right = 2; - * @return the frontRight - */ - public double getFrontRight() { - return frontRight; - } - - /** - * optional double front_right = 2; - * @param value the frontRight to set - * @return this - */ - public ProtobufMecanumDriveMotorVoltages setFrontRight(final double value) { - bitField0_ |= 0x00000002; - frontRight = value; - return this; - } - - /** - * optional double rear_left = 3; - * @return whether the rearLeft field is set - */ - public boolean hasRearLeft() { - return (bitField0_ & 0x00000004) != 0; - } - - /** - * optional double rear_left = 3; - * @return this - */ - public ProtobufMecanumDriveMotorVoltages clearRearLeft() { - bitField0_ &= ~0x00000004; - rearLeft = 0D; - return this; - } - - /** - * optional double rear_left = 3; - * @return the rearLeft - */ - public double getRearLeft() { - return rearLeft; - } - - /** - * optional double rear_left = 3; - * @param value the rearLeft to set - * @return this - */ - public ProtobufMecanumDriveMotorVoltages setRearLeft(final double value) { - bitField0_ |= 0x00000004; - rearLeft = value; - return this; - } - - /** - * optional double rear_right = 4; - * @return whether the rearRight field is set - */ - public boolean hasRearRight() { - return (bitField0_ & 0x00000008) != 0; - } - - /** - * optional double rear_right = 4; - * @return this - */ - public ProtobufMecanumDriveMotorVoltages clearRearRight() { - bitField0_ &= ~0x00000008; - rearRight = 0D; - return this; - } - - /** - * optional double rear_right = 4; - * @return the rearRight - */ - public double getRearRight() { - return rearRight; - } - - /** - * optional double rear_right = 4; - * @param value the rearRight to set - * @return this - */ - public ProtobufMecanumDriveMotorVoltages setRearRight(final double value) { - bitField0_ |= 0x00000008; - rearRight = value; - return this; - } - - @Override - public ProtobufMecanumDriveMotorVoltages copyFrom( - final ProtobufMecanumDriveMotorVoltages other) { - cachedSize = other.cachedSize; - if ((bitField0_ | other.bitField0_) != 0) { - bitField0_ = other.bitField0_; - frontLeft = other.frontLeft; - frontRight = other.frontRight; - rearLeft = other.rearLeft; - rearRight = other.rearRight; - } - return this; - } - - @Override - public ProtobufMecanumDriveMotorVoltages mergeFrom( - final ProtobufMecanumDriveMotorVoltages other) { - if (other.isEmpty()) { - return this; - } - cachedSize = -1; - if (other.hasFrontLeft()) { - setFrontLeft(other.frontLeft); - } - if (other.hasFrontRight()) { - setFrontRight(other.frontRight); - } - if (other.hasRearLeft()) { - setRearLeft(other.rearLeft); - } - if (other.hasRearRight()) { - setRearRight(other.rearRight); - } - return this; - } - - @Override - public ProtobufMecanumDriveMotorVoltages clear() { - if (isEmpty()) { - return this; - } - cachedSize = -1; - bitField0_ = 0; - frontLeft = 0D; - frontRight = 0D; - rearLeft = 0D; - rearRight = 0D; - return this; - } - - @Override - public ProtobufMecanumDriveMotorVoltages clearQuick() { - if (isEmpty()) { - return this; - } - cachedSize = -1; - bitField0_ = 0; - return this; - } - - @Override - public boolean equals(Object o) { - if (o == this) { - return true; - } - if (!(o instanceof ProtobufMecanumDriveMotorVoltages)) { - return false; - } - ProtobufMecanumDriveMotorVoltages other = (ProtobufMecanumDriveMotorVoltages) o; - return bitField0_ == other.bitField0_ - && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft)) - && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight)) - && (!hasRearLeft() || ProtoUtil.isEqual(rearLeft, other.rearLeft)) - && (!hasRearRight() || ProtoUtil.isEqual(rearRight, other.rearRight)); - } - - @Override - public void writeTo(final ProtoSink output) throws IOException { - if ((bitField0_ & 0x00000001) != 0) { - output.writeRawByte((byte) 9); - output.writeDoubleNoTag(frontLeft); - } - if ((bitField0_ & 0x00000002) != 0) { - output.writeRawByte((byte) 17); - output.writeDoubleNoTag(frontRight); - } - if ((bitField0_ & 0x00000004) != 0) { - output.writeRawByte((byte) 25); - output.writeDoubleNoTag(rearLeft); - } - if ((bitField0_ & 0x00000008) != 0) { - output.writeRawByte((byte) 33); - output.writeDoubleNoTag(rearRight); - } - } - - @Override - protected int computeSerializedSize() { - int size = 0; - if ((bitField0_ & 0x00000001) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000002) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000004) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000008) != 0) { - size += 9; - } - return size; - } - - @Override - @SuppressWarnings("fallthrough") - public ProtobufMecanumDriveMotorVoltages mergeFrom(final ProtoSource input) throws IOException { - // Enabled Fall-Through Optimization (QuickBuffers) - int tag = input.readTag(); - while (true) { - switch (tag) { - case 9: { - // frontLeft - frontLeft = input.readDouble(); - bitField0_ |= 0x00000001; - tag = input.readTag(); - if (tag != 17) { - break; - } - } - case 17: { - // frontRight - frontRight = input.readDouble(); - bitField0_ |= 0x00000002; - tag = input.readTag(); - if (tag != 25) { - break; - } - } - case 25: { - // rearLeft - rearLeft = input.readDouble(); - bitField0_ |= 0x00000004; - tag = input.readTag(); - if (tag != 33) { - break; - } - } - case 33: { - // rearRight - rearRight = input.readDouble(); - bitField0_ |= 0x00000008; - tag = input.readTag(); - if (tag != 0) { - break; - } - } - case 0: { - return this; - } - default: { - if (!input.skipField(tag)) { - return this; - } - tag = input.readTag(); - break; - } - } - } - } - - @Override - public void writeTo(final JsonSink output) throws IOException { - output.beginObject(); - if ((bitField0_ & 0x00000001) != 0) { - output.writeDouble(FieldNames.frontLeft, frontLeft); - } - if ((bitField0_ & 0x00000002) != 0) { - output.writeDouble(FieldNames.frontRight, frontRight); - } - if ((bitField0_ & 0x00000004) != 0) { - output.writeDouble(FieldNames.rearLeft, rearLeft); - } - if ((bitField0_ & 0x00000008) != 0) { - output.writeDouble(FieldNames.rearRight, rearRight); - } - output.endObject(); - } - - @Override - public ProtobufMecanumDriveMotorVoltages mergeFrom(final JsonSource input) throws IOException { - if (!input.beginObject()) { - return this; - } - while (!input.isAtEnd()) { - switch (input.readFieldHash()) { - case 127514064: - case -324277155: { - if (input.isAtField(FieldNames.frontLeft)) { - if (!input.trySkipNullValue()) { - frontLeft = input.readDouble(); - bitField0_ |= 0x00000001; - } - } else { - input.skipUnknownField(); - } - break; - } - case -336370317: - case -1456996218: { - if (input.isAtField(FieldNames.frontRight)) { - if (!input.trySkipNullValue()) { - frontRight = input.readDouble(); - bitField0_ |= 0x00000002; - } - } else { - input.skipUnknownField(); - } - break; - } - case -854852661: - case -712874558: { - if (input.isAtField(FieldNames.rearLeft)) { - if (!input.trySkipNullValue()) { - rearLeft = input.readDouble(); - bitField0_ |= 0x00000004; - } - } else { - input.skipUnknownField(); - } - break; - } - case -724967720: - case -618613823: { - if (input.isAtField(FieldNames.rearRight)) { - if (!input.trySkipNullValue()) { - rearRight = input.readDouble(); - bitField0_ |= 0x00000008; - } - } else { - input.skipUnknownField(); - } - break; - } - default: { - input.skipUnknownField(); - break; - } - } - } - input.endObject(); - return this; - } - - @Override - public ProtobufMecanumDriveMotorVoltages clone() { - return new ProtobufMecanumDriveMotorVoltages().copyFrom(this); - } - - @Override - public boolean isEmpty() { - return ((bitField0_) == 0); - } - - public static ProtobufMecanumDriveMotorVoltages parseFrom(final byte[] data) throws - InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveMotorVoltages(), data).checkInitialized(); - } - - public static ProtobufMecanumDriveMotorVoltages parseFrom(final ProtoSource input) throws - IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveMotorVoltages(), input).checkInitialized(); - } - - public static ProtobufMecanumDriveMotorVoltages parseFrom(final JsonSource input) throws - IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveMotorVoltages(), input).checkInitialized(); - } - - /** - * @return factory for creating ProtobufMecanumDriveMotorVoltages messages - */ - public static MessageFactory getFactory() { - return ProtobufMecanumDriveMotorVoltagesFactory.INSTANCE; - } - - /** - * @return this type's descriptor. - */ - public static Descriptors.Descriptor getDescriptor() { - return Kinematics.wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor; - } - - private enum ProtobufMecanumDriveMotorVoltagesFactory implements MessageFactory { - INSTANCE; - - @Override - public ProtobufMecanumDriveMotorVoltages create() { - return ProtobufMecanumDriveMotorVoltages.newInstance(); - } - } - - /** - * Contains name constants used for serializing JSON - */ - static class FieldNames { - static final FieldName frontLeft = FieldName.forField("frontLeft", "front_left"); - - static final FieldName frontRight = FieldName.forField("frontRight", "front_right"); - - static final FieldName rearLeft = FieldName.forField("rearLeft", "rear_left"); - - static final FieldName rearRight = FieldName.forField("rearRight", "rear_right"); - } - } - /** * Protobuf type {@code ProtobufMecanumDriveWheelPositions} */ diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index 65087413758..ad404212b9c 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -78,283 +78,243 @@ static const uint8_t file_descriptor[] { 0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e, 0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52, 0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67,0x68,0x74, -0x22,0x9f,0x01,0x0a,0x21,0x50,0x72,0x6f,0x74,0x6f, +0x22,0xa0,0x01,0x0a,0x22,0x50,0x72,0x6f,0x74,0x6f, 0x62,0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d, -0x44,0x72,0x69,0x76,0x65,0x4d,0x6f,0x74,0x6f,0x72, -0x56,0x6f,0x6c,0x74,0x61,0x67,0x65,0x73,0x12,0x1d, -0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c,0x65, -0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x09, -0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12, -0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72, -0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01, -0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67, -0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61,0x72, -0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28, -0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66, -0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f, -0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28, -0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67, -0x68,0x74,0x22,0xa0,0x01,0x0a,0x22,0x50,0x72,0x6f, -0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,0x61,0x6e, -0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65, -0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e, -0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74, -0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28, -0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65, -0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e, -0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20, -0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74, -0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72, -0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03, -0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72, -0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65, -0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04, -0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72, -0x52,0x69,0x67,0x68,0x74,0x22,0x9d,0x01,0x0a,0x1f, -0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65, -0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65, -0x57,0x68,0x65,0x65,0x6c,0x53,0x70,0x65,0x65,0x64, -0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74, -0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28, -0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65, -0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e, -0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20, -0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74, -0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72, -0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03, -0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72, -0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65, -0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04, -0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72, -0x52,0x69,0x67,0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50, -0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65, -0x72,0x76,0x65,0x44,0x72,0x69,0x76,0x65,0x4b,0x69, -0x6e,0x65,0x6d,0x61,0x74,0x69,0x63,0x73,0x12,0x3a, -0x0a,0x07,0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x18, -0x01,0x20,0x03,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70, -0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, -0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e, -0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52, -0x07,0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x22,0x6f, -0x0a,0x1c,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, -0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75, -0x6c,0x65,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e, -0x12,0x1a,0x0a,0x08,0x64,0x69,0x73,0x74,0x61,0x6e, -0x63,0x65,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x08, -0x64,0x69,0x73,0x74,0x61,0x6e,0x63,0x65,0x12,0x33, -0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20, -0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e, +0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c, +0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x73,0x12, +0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c, +0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52, +0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74, +0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f, +0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28, +0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69, +0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61, +0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01, +0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65, +0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72, +0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01, +0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69, +0x67,0x68,0x74,0x22,0x9d,0x01,0x0a,0x1f,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,0x61, +0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,0x68, +0x65,0x65,0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12, +0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c, +0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52, +0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74, +0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f, +0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28, +0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69, +0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61, +0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01, +0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65, +0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72, +0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01, +0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69, +0x67,0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50,0x72,0x6f, +0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,0x76, +0x65,0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65, +0x6d,0x61,0x74,0x69,0x63,0x73,0x12,0x3a,0x0a,0x07, +0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x18,0x01,0x20, +0x03,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e, 0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74, -0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69, -0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c, -0x65,0x22,0x66,0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f, -0x62,0x75,0x66,0x53,0x77,0x65,0x72,0x76,0x65,0x4d, -0x6f,0x64,0x75,0x6c,0x65,0x53,0x74,0x61,0x74,0x65, -0x12,0x14,0x0a,0x05,0x73,0x70,0x65,0x65,0x64,0x18, -0x01,0x20,0x01,0x28,0x01,0x52,0x05,0x73,0x70,0x65, -0x65,0x64,0x12,0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c, -0x65,0x18,0x02,0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e, -0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e, -0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f, -0x74,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05, -0x61,0x6e,0x67,0x6c,0x65,0x42,0x1a,0x0a,0x18,0x65, -0x64,0x75,0x2e,0x77,0x70,0x69,0x2e,0x66,0x69,0x72, -0x73,0x74,0x2e,0x6d,0x61,0x74,0x68,0x2e,0x70,0x72, -0x6f,0x74,0x6f,0x4a,0x8d,0x0f,0x0a,0x06,0x12,0x04, -0x00,0x00,0x44,0x01,0x0a,0x08,0x0a,0x01,0x0c,0x12, -0x03,0x00,0x00,0x12,0x0a,0x08,0x0a,0x01,0x02,0x12, -0x03,0x02,0x00,0x12,0x0a,0x09,0x0a,0x02,0x03,0x00, -0x12,0x03,0x04,0x00,0x1a,0x0a,0x08,0x0a,0x01,0x08, -0x12,0x03,0x06,0x00,0x31,0x0a,0x09,0x0a,0x02,0x08, -0x01,0x12,0x03,0x06,0x00,0x31,0x0a,0x0a,0x0a,0x02, -0x04,0x00,0x12,0x04,0x08,0x00,0x0c,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x00,0x01,0x12,0x03,0x08,0x08,0x1d, -0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x00,0x12,0x03, -0x09,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x00,0x05,0x12,0x03,0x09,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x00,0x01,0x12,0x03,0x09,0x09, -0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x03, -0x12,0x03,0x09,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04, -0x00,0x02,0x01,0x12,0x03,0x0a,0x02,0x10,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x01,0x05,0x12,0x03,0x0a, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01, -0x01,0x12,0x03,0x0a,0x09,0x0b,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x01,0x03,0x12,0x03,0x0a,0x0e,0x0f, -0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x02,0x12,0x03, -0x0b,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x02,0x05,0x12,0x03,0x0b,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x02,0x01,0x12,0x03,0x0b,0x09, -0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x03, -0x12,0x03,0x0b,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04, -0x01,0x12,0x04,0x0e,0x00,0x10,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x01,0x01,0x12,0x03,0x0e,0x08,0x2b,0x0a, -0x0b,0x0a,0x04,0x04,0x01,0x02,0x00,0x12,0x03,0x0f, -0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00, -0x05,0x12,0x03,0x0f,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x01,0x02,0x00,0x01,0x12,0x03,0x0f,0x09,0x14, -0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x03,0x12, -0x03,0x0f,0x17,0x18,0x0a,0x0a,0x0a,0x02,0x04,0x02, -0x12,0x04,0x12,0x00,0x15,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x02,0x01,0x12,0x03,0x12,0x08,0x2c,0x0a,0x0b, -0x0a,0x04,0x04,0x02,0x02,0x00,0x12,0x03,0x13,0x02, -0x12,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x05, -0x12,0x03,0x13,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x02,0x02,0x00,0x01,0x12,0x03,0x13,0x09,0x0d,0x0a, -0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x03,0x12,0x03, -0x13,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02, -0x01,0x12,0x03,0x14,0x02,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x02,0x02,0x01,0x05,0x12,0x03,0x14,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01,0x01,0x12, -0x03,0x14,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x02, -0x02,0x01,0x03,0x12,0x03,0x14,0x11,0x12,0x0a,0x0a, -0x0a,0x02,0x04,0x03,0x12,0x04,0x17,0x00,0x1a,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x17, -0x08,0x2f,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00, -0x12,0x03,0x18,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04, -0x03,0x02,0x00,0x05,0x12,0x03,0x18,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03, -0x18,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, -0x00,0x03,0x12,0x03,0x18,0x10,0x11,0x0a,0x0b,0x0a, -0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x19,0x02,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12, -0x03,0x19,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03, -0x02,0x01,0x01,0x12,0x03,0x19,0x09,0x0e,0x0a,0x0c, -0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x19, -0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04, -0x1c,0x00,0x21,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04, -0x01,0x12,0x03,0x1c,0x08,0x26,0x0a,0x0b,0x0a,0x04, -0x04,0x04,0x02,0x00,0x12,0x03,0x1d,0x02,0x27,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x06,0x12,0x03, -0x1d,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, -0x00,0x01,0x12,0x03,0x1d,0x18,0x22,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x1d,0x25, -0x26,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12, -0x03,0x1e,0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x01,0x06,0x12,0x03,0x1e,0x02,0x17,0x0a,0x0c, -0x0a,0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1e, -0x18,0x23,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, -0x03,0x12,0x03,0x1e,0x26,0x27,0x0a,0x0b,0x0a,0x04, -0x04,0x04,0x02,0x02,0x12,0x03,0x1f,0x02,0x26,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x02,0x06,0x12,0x03, -0x1f,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, -0x02,0x01,0x12,0x03,0x1f,0x18,0x21,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x02,0x03,0x12,0x03,0x1f,0x24, -0x25,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x03,0x12, -0x03,0x20,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x03,0x06,0x12,0x03,0x20,0x02,0x17,0x0a,0x0c, -0x0a,0x05,0x04,0x04,0x02,0x03,0x01,0x12,0x03,0x20, -0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03, -0x03,0x12,0x03,0x20,0x25,0x26,0x0a,0x0a,0x0a,0x02, -0x04,0x05,0x12,0x04,0x23,0x00,0x28,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x05,0x01,0x12,0x03,0x23,0x08,0x29, -0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03, -0x24,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, -0x00,0x05,0x12,0x03,0x24,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x24,0x09, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03, -0x12,0x03,0x24,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04, -0x05,0x02,0x01,0x12,0x03,0x25,0x02,0x19,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x25, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01, -0x01,0x12,0x03,0x25,0x09,0x14,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x01,0x03,0x12,0x03,0x25,0x17,0x18, -0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x02,0x12,0x03, -0x26,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, -0x02,0x05,0x12,0x03,0x26,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x05,0x02,0x02,0x01,0x12,0x03,0x26,0x09, -0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x03, -0x12,0x03,0x26,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04, -0x05,0x02,0x03,0x12,0x03,0x27,0x02,0x18,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x03,0x05,0x12,0x03,0x27, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x03, -0x01,0x12,0x03,0x27,0x09,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x03,0x03,0x12,0x03,0x27,0x16,0x17, -0x0a,0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x2a,0x00, -0x2f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12, -0x03,0x2a,0x08,0x2a,0x0a,0x0b,0x0a,0x04,0x04,0x06, -0x02,0x00,0x12,0x03,0x2b,0x02,0x18,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x00,0x05,0x12,0x03,0x2b,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01, -0x12,0x03,0x2b,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x00,0x03,0x12,0x03,0x2b,0x16,0x17,0x0a, -0x0b,0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x2c, -0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01, -0x05,0x12,0x03,0x2c,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x01,0x01,0x12,0x03,0x2c,0x09,0x14, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12, -0x03,0x2c,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x06, -0x02,0x02,0x12,0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x02,0x05,0x12,0x03,0x2d,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01, -0x12,0x03,0x2d,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x02,0x03,0x12,0x03,0x2d,0x15,0x16,0x0a, -0x0b,0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2e, -0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03, -0x05,0x12,0x03,0x2e,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x03,0x01,0x12,0x03,0x2e,0x09,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12, -0x03,0x2e,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x07, -0x12,0x04,0x31,0x00,0x36,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x07,0x01,0x12,0x03,0x31,0x08,0x27,0x0a,0x0b, -0x0a,0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x32,0x02, -0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05, -0x12,0x03,0x32,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x07,0x02,0x00,0x01,0x12,0x03,0x32,0x09,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03, -0x32,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02, -0x01,0x12,0x03,0x33,0x02,0x19,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x01,0x05,0x12,0x03,0x33,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12, -0x03,0x33,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x01,0x03,0x12,0x03,0x33,0x17,0x18,0x0a,0x0b, -0x0a,0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x34,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05, -0x12,0x03,0x34,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x07,0x02,0x02,0x01,0x12,0x03,0x34,0x09,0x12,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03, -0x34,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02, -0x03,0x12,0x03,0x35,0x02,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x03,0x05,0x12,0x03,0x35,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12, -0x03,0x35,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x03,0x03,0x12,0x03,0x35,0x16,0x17,0x0a,0x0a, -0x0a,0x02,0x04,0x08,0x12,0x04,0x38,0x00,0x3a,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x38, -0x08,0x25,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00, -0x12,0x03,0x39,0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x00,0x04,0x12,0x03,0x39,0x02,0x0a,0x0a, -0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x06,0x12,0x03, -0x39,0x0b,0x20,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x00,0x01,0x12,0x03,0x39,0x21,0x28,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x00,0x03,0x12,0x03,0x39,0x2b, -0x2c,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c, -0x00,0x3f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01, -0x12,0x03,0x3c,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04, -0x09,0x02,0x00,0x12,0x03,0x3d,0x02,0x16,0x0a,0x0c, -0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00, -0x01,0x12,0x03,0x3d,0x09,0x11,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x14,0x15, -0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03, -0x3e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, -0x01,0x06,0x12,0x03,0x3e,0x02,0x14,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x15, -0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03, -0x12,0x03,0x3e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04, -0x0a,0x12,0x04,0x41,0x00,0x44,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x0a,0x01,0x12,0x03,0x41,0x08,0x21,0x0a, -0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x42, -0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00, -0x05,0x12,0x03,0x42,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x0a,0x02,0x00,0x01,0x12,0x03,0x42,0x09,0x0e, -0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x03,0x12, -0x03,0x42,0x11,0x12,0x0a,0x0b,0x0a,0x04,0x04,0x0a, -0x02,0x01,0x12,0x03,0x43,0x02,0x1f,0x0a,0x0c,0x0a, -0x05,0x04,0x0a,0x02,0x01,0x06,0x12,0x03,0x43,0x02, -0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x01,0x01, -0x12,0x03,0x43,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04, -0x0a,0x02,0x01,0x03,0x12,0x03,0x43,0x1d,0x1e,0x62, -0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, +0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c, +0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x07,0x6d, +0x6f,0x64,0x75,0x6c,0x65,0x73,0x22,0x6f,0x0a,0x1c, +0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77, +0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65, +0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x12,0x1a, +0x0a,0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63,0x65, +0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x08,0x64,0x69, +0x73,0x74,0x61,0x6e,0x63,0x65,0x12,0x33,0x0a,0x05, +0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,0x28, +0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72, +0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,0x6e, +0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x22, +0x66,0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, +0x66,0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64, +0x75,0x6c,0x65,0x53,0x74,0x61,0x74,0x65,0x12,0x14, +0x0a,0x05,0x73,0x70,0x65,0x65,0x64,0x18,0x01,0x20, +0x01,0x28,0x01,0x52,0x05,0x73,0x70,0x65,0x65,0x64, +0x12,0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,0x18, +0x02,0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70, +0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,0x61, +0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,0x6e, +0x67,0x6c,0x65,0x42,0x1a,0x0a,0x18,0x65,0x64,0x75, +0x2e,0x77,0x70,0x69,0x2e,0x66,0x69,0x72,0x73,0x74, +0x2e,0x6d,0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74, +0x6f,0x4a,0x99,0x0d,0x0a,0x06,0x12,0x04,0x00,0x00, +0x3d,0x01,0x0a,0x08,0x0a,0x01,0x0c,0x12,0x03,0x00, +0x00,0x12,0x0a,0x08,0x0a,0x01,0x02,0x12,0x03,0x02, +0x00,0x12,0x0a,0x09,0x0a,0x02,0x03,0x00,0x12,0x03, +0x04,0x00,0x1a,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03, +0x06,0x00,0x31,0x0a,0x09,0x0a,0x02,0x08,0x01,0x12, +0x03,0x06,0x00,0x31,0x0a,0x0a,0x0a,0x02,0x04,0x00, +0x12,0x04,0x08,0x00,0x0c,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x00,0x01,0x12,0x03,0x08,0x08,0x1d,0x0a,0x0b, +0x0a,0x04,0x04,0x00,0x02,0x00,0x12,0x03,0x09,0x02, +0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05, +0x12,0x03,0x09,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x00,0x01,0x12,0x03,0x09,0x09,0x0b,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03, +0x09,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, +0x01,0x12,0x03,0x0a,0x02,0x10,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x01,0x05,0x12,0x03,0x0a,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12, +0x03,0x0a,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x01,0x03,0x12,0x03,0x0a,0x0e,0x0f,0x0a,0x0b, +0x0a,0x04,0x04,0x00,0x02,0x02,0x12,0x03,0x0b,0x02, +0x13,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05, +0x12,0x03,0x0b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x02,0x01,0x12,0x03,0x0b,0x09,0x0e,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03, +0x0b,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12, +0x04,0x0e,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x01,0x01,0x12,0x03,0x0e,0x08,0x2b,0x0a,0x0b,0x0a, +0x04,0x04,0x01,0x02,0x00,0x12,0x03,0x0f,0x02,0x19, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x05,0x12, +0x03,0x0f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x00,0x01,0x12,0x03,0x0f,0x09,0x14,0x0a,0x0c, +0x0a,0x05,0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x0f, +0x17,0x18,0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04, +0x12,0x00,0x15,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02, +0x01,0x12,0x03,0x12,0x08,0x2c,0x0a,0x0b,0x0a,0x04, +0x04,0x02,0x02,0x00,0x12,0x03,0x13,0x02,0x12,0x0a, +0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03, +0x13,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, +0x00,0x01,0x12,0x03,0x13,0x09,0x0d,0x0a,0x0c,0x0a, +0x05,0x04,0x02,0x02,0x00,0x03,0x12,0x03,0x13,0x10, +0x11,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x01,0x12, +0x03,0x14,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02, +0x02,0x01,0x05,0x12,0x03,0x14,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x02,0x02,0x01,0x01,0x12,0x03,0x14, +0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01, +0x03,0x12,0x03,0x14,0x11,0x12,0x0a,0x0a,0x0a,0x02, +0x04,0x03,0x12,0x04,0x17,0x00,0x1a,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x17,0x08,0x2f, +0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03, +0x18,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, +0x00,0x05,0x12,0x03,0x18,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x18,0x09, +0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03, +0x12,0x03,0x18,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04, +0x03,0x02,0x01,0x12,0x03,0x19,0x02,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x19, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01, +0x01,0x12,0x03,0x19,0x09,0x0e,0x0a,0x0c,0x0a,0x05, +0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x19,0x11,0x12, +0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1c,0x00, +0x21,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12, +0x03,0x1c,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x04, +0x02,0x00,0x12,0x03,0x1d,0x02,0x27,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x00,0x06,0x12,0x03,0x1d,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01, +0x12,0x03,0x1d,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x00,0x03,0x12,0x03,0x1d,0x25,0x26,0x0a, +0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1e, +0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, +0x06,0x12,0x03,0x1e,0x02,0x17,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1e,0x18,0x23, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12, +0x03,0x1e,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x04, +0x02,0x02,0x12,0x03,0x1f,0x02,0x26,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x02,0x06,0x12,0x03,0x1f,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x02,0x01, +0x12,0x03,0x1f,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x02,0x03,0x12,0x03,0x1f,0x24,0x25,0x0a, +0x0b,0x0a,0x04,0x04,0x04,0x02,0x03,0x12,0x03,0x20, +0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03, +0x06,0x12,0x03,0x20,0x02,0x17,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x03,0x01,0x12,0x03,0x20,0x18,0x22, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x03,0x12, +0x03,0x20,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x05, +0x12,0x04,0x23,0x00,0x28,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x05,0x01,0x12,0x03,0x23,0x08,0x2a,0x0a,0x0b, +0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x24,0x02, +0x18,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05, +0x12,0x03,0x24,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x00,0x01,0x12,0x03,0x24,0x09,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03, +0x24,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02, +0x01,0x12,0x03,0x25,0x02,0x19,0x0a,0x0c,0x0a,0x05, +0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x25,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12, +0x03,0x25,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x01,0x03,0x12,0x03,0x25,0x17,0x18,0x0a,0x0b, +0x0a,0x04,0x04,0x05,0x02,0x02,0x12,0x03,0x26,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x05, +0x12,0x03,0x26,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x02,0x01,0x12,0x03,0x26,0x09,0x12,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x03,0x12,0x03, +0x26,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02, +0x03,0x12,0x03,0x27,0x02,0x18,0x0a,0x0c,0x0a,0x05, +0x04,0x05,0x02,0x03,0x05,0x12,0x03,0x27,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x03,0x01,0x12, +0x03,0x27,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x03,0x03,0x12,0x03,0x27,0x16,0x17,0x0a,0x0a, +0x0a,0x02,0x04,0x06,0x12,0x04,0x2a,0x00,0x2f,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x2a, +0x08,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00, +0x12,0x03,0x2b,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x00,0x05,0x12,0x03,0x2b,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03, +0x2b,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x00,0x03,0x12,0x03,0x2b,0x16,0x17,0x0a,0x0b,0x0a, +0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x2c,0x02,0x19, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x05,0x12, +0x03,0x2c,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x01,0x01,0x12,0x03,0x2c,0x09,0x14,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x2c, +0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02, +0x12,0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x02,0x05,0x12,0x03,0x2d,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03, +0x2d,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x02,0x03,0x12,0x03,0x2d,0x15,0x16,0x0a,0x0b,0x0a, +0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2e,0x02,0x18, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x05,0x12, +0x03,0x2e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x03,0x01,0x12,0x03,0x2e,0x09,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2e, +0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04, +0x31,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07, +0x01,0x12,0x03,0x31,0x08,0x25,0x0a,0x0b,0x0a,0x04, +0x04,0x07,0x02,0x00,0x12,0x03,0x32,0x02,0x2d,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x04,0x12,0x03, +0x32,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x00,0x06,0x12,0x03,0x32,0x0b,0x20,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x00,0x01,0x12,0x03,0x32,0x21, +0x28,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03, +0x12,0x03,0x32,0x2b,0x2c,0x0a,0x0a,0x0a,0x02,0x04, +0x08,0x12,0x04,0x35,0x00,0x38,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x24,0x0a, +0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36, +0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00, +0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x11, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12, +0x03,0x36,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04,0x08, +0x02,0x01,0x12,0x03,0x37,0x02,0x1f,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x01,0x06,0x12,0x03,0x37,0x02, +0x14,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01, +0x12,0x03,0x37,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x1d,0x1e,0x0a, +0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3a,0x00,0x3d, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03, +0x3a,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, +0x00,0x12,0x03,0x3b,0x02,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3b,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12, +0x03,0x3b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x00,0x03,0x12,0x03,0x3b,0x11,0x12,0x0a,0x0b, +0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3c,0x02, +0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x06, +0x12,0x03,0x3c,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x01,0x01,0x12,0x03,0x3c,0x15,0x1a,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03, +0x3c,0x1d,0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f, +0x33, }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; @@ -387,12 +347,6 @@ pb_filedesc_t wpi_proto_ProtobufMecanumDriveKinematics::file_descriptor(void) no PB_BIND(wpi_proto_ProtobufMecanumDriveKinematics, wpi_proto_ProtobufMecanumDriveKinematics, AUTO) -static const char wpi_proto_ProtobufMecanumDriveMotorVoltages_name[] = "wpi.proto.ProtobufMecanumDriveMotorVoltages"; -std::string_view wpi_proto_ProtobufMecanumDriveMotorVoltages::msg_name(void) noexcept { return wpi_proto_ProtobufMecanumDriveMotorVoltages_name; } -pb_filedesc_t wpi_proto_ProtobufMecanumDriveMotorVoltages::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } -PB_BIND(wpi_proto_ProtobufMecanumDriveMotorVoltages, wpi_proto_ProtobufMecanumDriveMotorVoltages, AUTO) - - static const char wpi_proto_ProtobufMecanumDriveWheelPositions_name[] = "wpi.proto.ProtobufMecanumDriveWheelPositions"; std::string_view wpi_proto_ProtobufMecanumDriveWheelPositions::msg_name(void) noexcept { return wpi_proto_ProtobufMecanumDriveWheelPositions_name; } pb_filedesc_t wpi_proto_ProtobufMecanumDriveWheelPositions::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h index e55103ffb80..cbb27afc08c 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h @@ -63,17 +63,6 @@ typedef struct _wpi_proto_ProtobufMecanumDriveKinematics { pb_callback_t rear_right; } wpi_proto_ProtobufMecanumDriveKinematics; -typedef struct _wpi_proto_ProtobufMecanumDriveMotorVoltages { - static const pb_msgdesc_t* msg_descriptor(void) noexcept; - static std::string_view msg_name(void) noexcept; - static pb_filedesc_t file_descriptor(void) noexcept; - - double front_left; - double front_right; - double rear_left; - double rear_right; -} wpi_proto_ProtobufMecanumDriveMotorVoltages; - typedef struct _wpi_proto_ProtobufMecanumDriveWheelPositions { static const pb_msgdesc_t* msg_descriptor(void) noexcept; static std::string_view msg_name(void) noexcept; @@ -129,7 +118,6 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_init_default {0, 0} #define wpi_proto_ProtobufDifferentialDriveWheelPositions_init_default {0, 0} #define wpi_proto_ProtobufMecanumDriveKinematics_init_default {{{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_init_default {0, 0, 0, 0} #define wpi_proto_ProtobufMecanumDriveWheelPositions_init_default {0, 0, 0, 0} #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_init_default {0, 0, 0, 0} #define wpi_proto_ProtobufSwerveDriveKinematics_init_default {{{NULL}, NULL}} @@ -140,7 +128,6 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_init_zero {0, 0} #define wpi_proto_ProtobufDifferentialDriveWheelPositions_init_zero {0, 0} #define wpi_proto_ProtobufMecanumDriveKinematics_init_zero {{{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_init_zero {0, 0, 0, 0} #define wpi_proto_ProtobufMecanumDriveWheelPositions_init_zero {0, 0, 0, 0} #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_init_zero {0, 0, 0, 0} #define wpi_proto_ProtobufSwerveDriveKinematics_init_zero {{{NULL}, NULL}} @@ -160,10 +147,6 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { #define wpi_proto_ProtobufMecanumDriveKinematics_front_right_tag 2 #define wpi_proto_ProtobufMecanumDriveKinematics_rear_left_tag 3 #define wpi_proto_ProtobufMecanumDriveKinematics_rear_right_tag 4 -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_front_left_tag 1 -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_front_right_tag 2 -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_rear_left_tag 3 -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_rear_right_tag 4 #define wpi_proto_ProtobufMecanumDriveWheelPositions_front_left_tag 1 #define wpi_proto_ProtobufMecanumDriveWheelPositions_front_right_tag 2 #define wpi_proto_ProtobufMecanumDriveWheelPositions_rear_left_tag 3 @@ -215,14 +198,6 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, rear_right, 4) #define wpi_proto_ProtobufMecanumDriveKinematics_rear_left_MSGTYPE wpi_proto_ProtobufTranslation2d #define wpi_proto_ProtobufMecanumDriveKinematics_rear_right_MSGTYPE wpi_proto_ProtobufTranslation2d -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, DOUBLE, front_left, 1) \ -X(a, STATIC, SINGULAR, DOUBLE, front_right, 2) \ -X(a, STATIC, SINGULAR, DOUBLE, rear_left, 3) \ -X(a, STATIC, SINGULAR, DOUBLE, rear_right, 4) -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_CALLBACK NULL -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_DEFAULT NULL - #define wpi_proto_ProtobufMecanumDriveWheelPositions_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, DOUBLE, front_left, 1) \ X(a, STATIC, SINGULAR, DOUBLE, front_right, 2) \ @@ -264,12 +239,11 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) /* wpi_proto_ProtobufSwerveDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModulePosition_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModuleState_size depends on runtime parameters */ -#define WPI_PROTO_KINEMATICS_NPB_H_MAX_SIZE wpi_proto_ProtobufMecanumDriveMotorVoltages_size +#define WPI_PROTO_KINEMATICS_NPB_H_MAX_SIZE wpi_proto_ProtobufMecanumDriveWheelPositions_size #define wpi_proto_ProtobufChassisSpeeds_size 27 #define wpi_proto_ProtobufDifferentialDriveKinematics_size 9 #define wpi_proto_ProtobufDifferentialDriveWheelPositions_size 18 #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_size 18 -#define wpi_proto_ProtobufMecanumDriveMotorVoltages_size 36 #define wpi_proto_ProtobufMecanumDriveWheelPositions_size 36 #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_size 36 diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index 461d9a6f080..eb494308926 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -131,7 +131,10 @@ public static boolean isDetectable( * * @param pose A pose to convert to a vector. * @return The given pose in vector form, with the third element, theta, in radians. + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseToVector(Pose2d pose) { return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians()); } @@ -180,7 +183,10 @@ public static Matrix desaturateInputVector( * * @param pose A pose to convert to a vector. * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta). + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseTo4dVector(Pose2d pose) { return VecBuilder.fill( pose.getTranslation().getX(), @@ -194,7 +200,10 @@ public static Matrix poseTo4dVector(Pose2d pose) { * * @param pose A pose to convert to a vector. * @return The given pose in vector form, with the third element, theta, in radians. + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseTo3dVector(Pose2d pose) { return VecBuilder.fill( pose.getTranslation().getX(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index c1c30d42d21..a243a52d859 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -102,10 +102,9 @@ public ChassisSpeeds calculate( m_poseError = trajectoryPose.relativeTo(currentPose); m_rotationError = desiredHeading.minus(currentPose.getRotation()); - ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF); + if (!m_enabled) { - speeds.toRobotRelativeSpeeds(currentPose.getRotation()); - return speeds; + return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation()); } // Calculate feedback velocities (based on position error). @@ -113,10 +112,8 @@ public ChassisSpeeds calculate( double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY()); // Return next output. - speeds.vxMetersPerSecond += xFeedback; - speeds.vyMetersPerSecond += yFeedback; - speeds.toRobotRelativeSpeeds(currentPose.getRotation()); - return speeds; + return ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation()); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 9356481ffa0..b7603a6bfbe 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -4,15 +4,8 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.controller.proto.SimpleMotorFeedforwardProto; import edu.wpi.first.math.controller.struct.SimpleMotorFeedforwardStruct; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.PerUnit; -import edu.wpi.first.units.TimeUnit; -import edu.wpi.first.units.Unit; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -136,7 +129,7 @@ public double getDt() { * @param velocity The velocity setpoint. * @param acceleration The acceleration setpoint. * @return The computed feedforward. - * @deprecated Use the current/next velocity overload instead. + * @deprecated Use {@link #calculateWithVelocities(double, double)} instead. */ @SuppressWarnings("removal") @Deprecated(forRemoval = true, since = "2025") @@ -150,50 +143,30 @@ public double calculate(double velocity, double acceleration) { * * @param velocity The velocity setpoint. * @return The computed feedforward. - * @deprecated Use the current/next velocity overload instead. */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") public double calculate(double velocity) { return calculate(velocity, 0); } - /** - * Calculates the feedforward from the gains and setpoints assuming discrete control when the - * setpoint does not change. - * - * @param The velocity parameter either as distance or angle. - * @param setpoint The velocity setpoint. - * @return The computed feedforward. - */ - public Voltage calculate(Measure> setpoint) { - return calculate(setpoint, setpoint); - } - /** * Calculates the feedforward from the gains and setpoints assuming discrete control. * *

Note this method is inaccurate when the velocity crosses 0. * - * @param The velocity parameter either as distance or angle. * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. * @return The computed feedforward. */ - public Voltage calculate( - Measure> currentVelocity, - Measure> nextVelocity) { + public double calculateWithVelocities(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Simple_motor_feedforward for derivation if (ka == 0.0) { - return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude()); + return ks * Math.signum(nextVelocity) + kv * nextVelocity; } else { double A = -kv / ka; double B = 1.0 / ka; double A_d = Math.exp(A * m_dt); double B_d = 1.0 / A * (A_d - 1.0) * B; - return Volts.of( - ks * Math.signum(currentVelocity.magnitude()) - + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude())); + return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index a11aa6d7e9f..ee9c3027630 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -199,7 +199,7 @@ public boolean contains(Translation2d point) { * @return The distance (0, if the point is contained by the ellipse) */ public double getDistance(Translation2d point) { - return findNearestPoint(point).getDistance(point); + return nearest(point).getDistance(point); } /** @@ -218,7 +218,7 @@ public Distance getMeasureDistance(Translation2d point) { * @param point The point that this will find the nearest point to. * @return A new point that is nearest to {@code point} and contained in the ellipse. */ - public Translation2d findNearestPoint(Translation2d point) { + public Translation2d nearest(Translation2d point) { // Check if already in ellipse if (contains(point)) { return point; @@ -226,7 +226,7 @@ public Translation2d findNearestPoint(Translation2d point) { // Find nearest point var nearestPoint = new double[2]; - Ellipse2dJNI.findNearestPoint( + Ellipse2dJNI.nearest( m_center.getX(), m_center.getY(), m_center.getRotation().getRadians(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 89c404fddbd..0705d61ea7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -10,9 +10,13 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Pose2dProto; import edu.wpi.first.math.geometry.struct.Pose2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -79,6 +83,20 @@ public Pose2d(Distance x, Distance y, Rotation2d rotation) { this(x.in(Meters), y.in(Meters), rotation); } + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Pose2d(Matrix matrix) { + m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2)); + m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0)); + if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** * Transforms the pose by the given transformation and returns the new transformed pose. * @@ -295,6 +313,28 @@ public Twist2d log(Pose2d end) { return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); } + /** + * Returns an affine transformation matrix representation of this pose. + * + * @return An affine transformation matrix representation of this pose. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + mat.get(0, 0), + mat.get(0, 1), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + vec.get(1), + 0.0, + 0.0, + 1.0); + } + /** * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same * distance from this pose, return the one with the closest rotation component. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index ada3e9059db..02c2b9ba4eb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -10,10 +10,14 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Pose3dProto; import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.jni.Pose3dJNI; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -79,6 +83,23 @@ public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) { this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); } + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Pose3d(Matrix matrix) { + m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); + m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); + if (matrix.get(3, 0) != 0.0 + || matrix.get(3, 1) != 0.0 + || matrix.get(3, 2) != 0.0 + || matrix.get(3, 3) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -326,6 +347,35 @@ public Twist3d log(Pose3d end) { resultArray[5]); } + /** + * Returns an affine transformation matrix representation of this pose. + * + * @return An affine transformation matrix representation of this pose. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N4(), + Nat.N4(), + mat.get(0, 0), + mat.get(0, 1), + mat.get(0, 2), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + mat.get(1, 2), + vec.get(1), + mat.get(2, 0), + mat.get(2, 1), + mat.get(2, 2), + vec.get(2), + 0.0, + 0.0, + 0.0, + 1.0); + } + /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 3b9455e47b2..4e252b7f57a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -186,7 +186,7 @@ public boolean contains(Translation2d point) { * @return The distance (0, if the point is contained by the rectangle) */ public double getDistance(Translation2d point) { - return findNearestPoint(point).getDistance(point); + return nearest(point).getDistance(point); } /** @@ -205,7 +205,7 @@ public Distance getMeasureDistance(Translation2d point) { * @param point The point that this will find the nearest point to. * @return A new point that is nearest to {@code point} and contained in the rectangle. */ - public Translation2d findNearestPoint(Translation2d point) { + public Translation2d nearest(Translation2d point) { // Check if already in rectangle if (contains(point)) { return point; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 49cf4c3c7d0..693a7f2bfc2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -10,11 +10,15 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Rotation2dProto; import edu.wpi.first.math.geometry.struct.Rotation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -133,6 +137,39 @@ public Rotation2d(Angle angle) { this(angle.in(Radians)); } + /** + * Constructs a Rotation2d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal. + */ + public Rotation2d(Matrix rotationMatrix) { + final var R = rotationMatrix; + + // Require that the rotation matrix is special orthogonal. This is true if + // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). + if (R.times(R.transpose()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) { + var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n'; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + if (Math.abs(R.det() - 1.0) > 1e-9) { + var msg = + "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n" + + R.getStorage().toString() + + '\n'; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + + // R = [cosθ −sinθ] + // [sinθ cosθ] + m_cos = R.get(0, 0); + m_sin = R.get(1, 0); + + m_value = Math.atan2(m_sin, m_cos); + } + /** * Constructs and returns a Rotation2d with the given radian value. * @@ -238,6 +275,17 @@ public Rotation2d rotateBy(Rotation2d other) { m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); } + /** + * Returns matrix representation of this rotation. + * + * @return Matrix representation of this rotation. + */ + public Matrix toMatrix() { + // R = [cosθ −sinθ] + // [sinθ cosθ] + return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos); + } + /** * Returns the measure of the Rotation2d. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 8883e665389..7d134774957 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -10,6 +10,7 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; @@ -458,6 +459,32 @@ public Angle getMeasureZ() { return Radians.of(getZ()); } + /** + * Returns rotation matrix representation of this rotation. + * + * @return Rotation matrix representation of this rotation. + */ + public Matrix toMatrix() { + double w = m_q.getW(); + double x = m_q.getX(); + double y = m_q.getY(); + double z = m_q.getZ(); + + // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y), + 2.0 * (x * y + w * z), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x), + 2.0 * (x * z - w * y), + 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y)); + } + /** * Returns the axis in the axis-angle representation of this rotation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 79dc16939e9..7a4cf8f4f9a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -6,8 +6,12 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Transform2dProto; import edu.wpi.first.math.geometry.struct.Transform2dStruct; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -78,6 +82,20 @@ public Transform2d(Distance x, Distance y, Rotation2d rotation) { this(x.in(Meters), y.in(Meters), rotation); } + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Transform2d(Matrix matrix) { + m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2)); + m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0)); + if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { m_translation = Translation2d.kZero; @@ -160,6 +178,28 @@ public Distance getMeasureY() { return m_translation.getMeasureY(); } + /** + * Returns an affine transformation matrix representation of this transformation. + * + * @return An affine transformation matrix representation of this transformation. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + mat.get(0, 0), + mat.get(0, 1), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + vec.get(1), + 0.0, + 0.0, + 1.0); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 8332b607c20..d993d861001 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -6,8 +6,12 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Transform3dProto; import edu.wpi.first.math.geometry.struct.Transform3dStruct; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -80,6 +84,23 @@ public Transform3d(Distance x, Distance y, Distance z, Rotation3d rotation) { this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); } + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Transform3d(Matrix matrix) { + m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); + m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); + if (matrix.get(3, 0) != 0.0 + || matrix.get(3, 1) != 0.0 + || matrix.get(3, 2) != 0.0 + || matrix.get(3, 3) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform3d() { m_translation = Translation3d.kZero; @@ -192,6 +213,35 @@ public Distance getMeasureZ() { return m_translation.getMeasureZ(); } + /** + * Returns an affine transformation matrix representation of this transformation. + * + * @return An affine transformation matrix representation of this transformation. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N4(), + Nat.N4(), + mat.get(0, 0), + mat.get(0, 1), + mat.get(0, 2), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + mat.get(1, 2), + vec.get(1), + mat.get(2, 0), + mat.get(2, 1), + mat.get(2, 2), + vec.get(2), + 0.0, + 0.0, + 0.0, + 1.0); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index e3857eb14e6..9b4af815c72 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -88,10 +88,10 @@ public Translation2d(Distance x, Distance y) { } /** - * Constructs a Translation2d from the provided translation vector's X and Y components. The - * values are assumed to be in meters. + * Constructs a Translation2d from a 2D translation vector. The values are assumed to be in + * meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ public Translation2d(Vector vector) { this(vector.get(0), vector.get(1)); @@ -148,9 +148,9 @@ public Distance getMeasureY() { } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ public Vector toVector() { return VecBuilder.fill(m_x, m_y); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 48a70c7f45a..20cede3d3da 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -104,10 +104,10 @@ public Translation3d(Translation2d translation) { } /** - * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The - * values are assumed to be in meters. + * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in + * meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ public Translation3d(Vector vector) { this(vector.get(0), vector.get(1), vector.get(2)); @@ -184,9 +184,9 @@ public Distance getMeasureZ() { } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ public Vector toVector() { return VecBuilder.fill(m_x, m_y, m_z); diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java index 984ca504017..895caa2bc5b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java @@ -9,7 +9,7 @@ public final class Ellipse2dJNI extends WPIMathJNI { /** * Returns the nearest point that is contained within the ellipse. * - *

Constructs an Ellipse2d object and runs its FindNearestPoint() method. + *

Constructs an Ellipse2d object and runs its nearest() method. * * @param centerX The x coordinate of the center of the ellipse in meters. * @param centerY The y coordinate of the center of the ellipse in meters. @@ -20,7 +20,7 @@ public final class Ellipse2dJNI extends WPIMathJNI { * @param pointY The y coordinate of the point that this will find the nearest point to. * @param nearestPoint Array to store nearest point into. */ - public static native void findNearestPoint( + public static native void nearest( double centerX, double centerY, double centerHeading, diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index e0eb4077961..4c806a52cb7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -103,9 +103,7 @@ public Twist2d toTwist2d(double dtSeconds) { * @param omegaRadiansPerSecond Angular velocity. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. - * @deprecated Use instance method instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds discretize( double vxMetersPerSecond, double vyMetersPerSecond, @@ -143,9 +141,7 @@ public static ChassisSpeeds discretize( * @param omega Angular velocity. * @param dt The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. - * @deprecated Use instance method instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds discretize( LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) { return discretize( @@ -166,9 +162,7 @@ public static ChassisSpeeds discretize( * @param continuousSpeeds The continuous speeds. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. - * @deprecated Use instance method instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) { return discretize( continuousSpeeds.vxMetersPerSecond, @@ -177,36 +171,6 @@ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt dtSeconds); } - /** - * Discretizes a continuous-time chassis speed. - * - *

This function converts this continuous-time chassis speed into a discrete-time one such that - * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the - * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt - * along the y-axis, and omega * dt around the z-axis). - * - *

This is useful for compensating for translational skew when translating and rotating a - * swerve drivetrain. - * - * @param dtSeconds The duration of the timestep the speeds should be applied for. - */ - public void discretize(double dtSeconds) { - var desiredDeltaPose = - new Pose2d( - vxMetersPerSecond * dtSeconds, - vyMetersPerSecond * dtSeconds, - new Rotation2d(omegaRadiansPerSecond * dtSeconds)); - - // Find the chassis translation/rotation deltas in the robot frame that move the robot from its - // current pose to the desired pose - var twist = Pose2d.kZero.log(desiredDeltaPose); - - // Turn the chassis translation/rotation deltas into average velocities - vxMetersPerSecond = twist.dx / dtSeconds; - vyMetersPerSecond = twist.dy / dtSeconds; - omegaRadiansPerSecond = twist.dtheta / dtSeconds; - } - /** * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds * object. @@ -220,9 +184,7 @@ public void discretize(double dtSeconds) { * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. - * @deprecated Use toRobotRelativeSpeeds instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromFieldRelativeSpeeds( double vxMetersPerSecond, double vyMetersPerSecond, @@ -247,9 +209,7 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. - * @deprecated Use toRobotRelativeSpeeds instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromFieldRelativeSpeeds( LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) { return fromFieldRelativeSpeeds( @@ -267,9 +227,7 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. - * @deprecated Use toRobotRelativeSpeeds instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromFieldRelativeSpeeds( ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) { return fromFieldRelativeSpeeds( @@ -279,21 +237,6 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( robotAngle); } - /** - * Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object. - * - * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is - * considered to be zero when it is facing directly away from your alliance station wall. - * Remember that this should be CCW positive. - */ - public void toRobotRelativeSpeeds(Rotation2d robotAngle) { - // CW rotation into chassis frame - var rotated = - new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus()); - vxMetersPerSecond = rotated.getX(); - vyMetersPerSecond = rotated.getY(); - } - /** * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds * object. @@ -307,9 +250,7 @@ public void toRobotRelativeSpeeds(Rotation2d robotAngle) { * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. - * @deprecated Use toFieldRelativeSpeeds instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromRobotRelativeSpeeds( double vxMetersPerSecond, double vyMetersPerSecond, @@ -333,9 +274,7 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. - * @deprecated Use toFieldRelativeSpeeds instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromRobotRelativeSpeeds( LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) { return fromRobotRelativeSpeeds( @@ -353,9 +292,7 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. - * @deprecated Use toFieldRelativeSpeeds instead. */ - @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromRobotRelativeSpeeds( ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) { return fromRobotRelativeSpeeds( @@ -365,20 +302,6 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( robotAngle); } - /** - * Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object. - * - * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is - * considered to be zero when it is facing directly away from your alliance station wall. - * Remember that this should be CCW positive. - */ - public void toFieldRelativeSpeeds(Rotation2d robotAngle) { - // CCW rotation out of chassis frame - var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle); - vxMetersPerSecond = rotated.getX(); - vyMetersPerSecond = rotated.getY(); - } - /** * Adds two ChassisSpeeds and returns the sum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java index 8bc91b1d8e6..51249d4fc3a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java @@ -4,13 +4,9 @@ package edu.wpi.first.math.kinematics; -import edu.wpi.first.math.kinematics.proto.MecanumDriveMotorVoltagesProto; -import edu.wpi.first.math.kinematics.struct.MecanumDriveMotorVoltagesStruct; -import edu.wpi.first.util.protobuf.ProtobufSerializable; -import edu.wpi.first.util.struct.StructSerializable; - /** Represents the motor voltages for a mecanum drive drivetrain. */ -public class MecanumDriveMotorVoltages implements ProtobufSerializable, StructSerializable { +@Deprecated(since = "2025", forRemoval = true) +public class MecanumDriveMotorVoltages { /** Voltage of the front left motor. */ public double frontLeftVoltage; @@ -52,11 +48,4 @@ public String toString() { + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage); } - - /** MecanumDriveMotorVoltages struct for serialization. */ - public static final MecanumDriveMotorVoltagesStruct struct = - new MecanumDriveMotorVoltagesStruct(); - - /** MecanumDriveMotorVoltages protobuf for serialization. */ - public static final MecanumDriveMotorVoltagesProto proto = new MecanumDriveMotorVoltagesProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java deleted file mode 100644 index 0b5f0c2583b..00000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.kinematics.proto; - -import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; -import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages; -import edu.wpi.first.util.protobuf.Protobuf; -import us.hebi.quickbuf.Descriptors.Descriptor; - -public final class MecanumDriveMotorVoltagesProto - implements Protobuf { - @Override - public Class getTypeClass() { - return MecanumDriveMotorVoltages.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufMecanumDriveMotorVoltages.getDescriptor(); - } - - @Override - public ProtobufMecanumDriveMotorVoltages createMessage() { - return ProtobufMecanumDriveMotorVoltages.newInstance(); - } - - @Override - public MecanumDriveMotorVoltages unpack(ProtobufMecanumDriveMotorVoltages msg) { - return new MecanumDriveMotorVoltages( - msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); - } - - @Override - public void pack(ProtobufMecanumDriveMotorVoltages msg, MecanumDriveMotorVoltages value) { - msg.setFrontLeft(value.frontLeftVoltage); - msg.setFrontRight(value.frontRightVoltage); - msg.setRearLeft(value.rearLeftVoltage); - msg.setRearRight(value.rearRightVoltage); - } -} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java deleted file mode 100644 index 8078e0a7b16..00000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.kinematics.struct; - -import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; - -public final class MecanumDriveMotorVoltagesStruct implements Struct { - @Override - public Class getTypeClass() { - return MecanumDriveMotorVoltages.class; - } - - @Override - public String getTypeName() { - return "MecanumDriveMotorVoltages"; - } - - @Override - public int getSize() { - return kSizeDouble * 4; - } - - @Override - public String getSchema() { - return "double front_left;double front_right;double rear_left;double rear_right"; - } - - @Override - public MecanumDriveMotorVoltages unpack(ByteBuffer bb) { - double front_left = bb.getDouble(); - double front_right = bb.getDouble(); - double rear_left = bb.getDouble(); - double rear_right = bb.getDouble(); - return new MecanumDriveMotorVoltages(front_left, front_right, rear_left, rear_right); - } - - @Override - public void pack(ByteBuffer bb, MecanumDriveMotorVoltages value) { - bb.putDouble(value.frontLeftVoltage); - bb.putDouble(value.frontRightVoltage); - bb.putDouble(value.rearLeftVoltage); - bb.putDouble(value.rearRightVoltage); - } -} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java index 1dcddea8404..a54e2eae448 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java @@ -8,8 +8,9 @@ import edu.wpi.first.math.Num; import edu.wpi.first.math.numbers.N1; import java.util.function.BiFunction; -import java.util.function.DoubleFunction; -import java.util.function.Function; +import java.util.function.DoubleBinaryOperator; +import java.util.function.DoubleUnaryOperator; +import java.util.function.UnaryOperator; /** Numerical integration utilities. */ public final class NumericalIntegration { @@ -25,13 +26,12 @@ private NumericalIntegration() { * @param dtSeconds The time over which to integrate. * @return the integration of dx/dt = f(x) for dt. */ - @SuppressWarnings("overloads") - public static double rk4(DoubleFunction f, double x, double dtSeconds) { + public static double rk4(DoubleUnaryOperator f, double x, double dtSeconds) { final var h = dtSeconds; - final var k1 = f.apply(x); - final var k2 = f.apply(x + h * k1 * 0.5); - final var k3 = f.apply(x + h * k2 * 0.5); - final var k4 = f.apply(x + h * k3); + final var k1 = f.applyAsDouble(x); + final var k2 = f.applyAsDouble(x + h * k1 * 0.5); + final var k3 = f.applyAsDouble(x + h * k2 * 0.5); + final var k4 = f.applyAsDouble(x + h * k3); return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); } @@ -45,15 +45,13 @@ public static double rk4(DoubleFunction f, double x, double dtSeconds) { * @param dtSeconds The time over which to integrate. * @return The result of Runge Kutta integration (4th order). */ - @SuppressWarnings("overloads") - public static double rk4( - BiFunction f, double x, Double u, double dtSeconds) { + public static double rk4(DoubleBinaryOperator f, double x, double u, double dtSeconds) { final var h = dtSeconds; - final var k1 = f.apply(x, u); - final var k2 = f.apply(x + h * k1 * 0.5, u); - final var k3 = f.apply(x + h * k2 * 0.5, u); - final var k4 = f.apply(x + h * k3, u); + final var k1 = f.applyAsDouble(x, u); + final var k2 = f.applyAsDouble(x + h * k1 * 0.5, u); + final var k3 = f.applyAsDouble(x + h * k2 * 0.5, u); + final var k4 = f.applyAsDouble(x + h * k3, u); return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); } @@ -69,7 +67,6 @@ public static double rk4( * @param dtSeconds The time over which to integrate. * @return the integration of dx/dt = f(x, u) for dt. */ - @SuppressWarnings("overloads") public static Matrix rk4( BiFunction, Matrix, Matrix> f, Matrix x, @@ -94,9 +91,8 @@ public static Matrix rk4( * @param dtSeconds The time over which to integrate. * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt. */ - @SuppressWarnings("overloads") public static Matrix rk4( - Function, Matrix> f, Matrix x, double dtSeconds) { + UnaryOperator> f, Matrix x, double dtSeconds) { final var h = dtSeconds; Matrix k1 = f.apply(x); @@ -145,7 +141,6 @@ public static Matrix rk4( * @param dtSeconds The time over which to integrate. * @return the integration of dx/dt = f(x, u) for dt. */ - @SuppressWarnings("overloads") public static Matrix rkdp( BiFunction, Matrix, Matrix> f, Matrix x, @@ -166,7 +161,6 @@ public static Matrix rkdp( * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. * @return the integration of dx/dt = f(x, u) for dt. */ - @SuppressWarnings("overloads") public static Matrix rkdp( BiFunction, Matrix, Matrix> f, Matrix x, @@ -291,7 +285,6 @@ public static Matrix rkdp( * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. * @return the integration of dx/dt = f(x, u) for dt. */ - @SuppressWarnings("overloads") public static Matrix rkdp( BiFunction, Matrix> f, double t, diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index eef320cd128..17ef087c19e 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -8,7 +8,7 @@ using namespace frc; -Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const { +Translation2d Ellipse2d::Nearest(const Translation2d& point) const { // Check if already in ellipse if (Contains(point)) { return point; diff --git a/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp b/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp index cc53d693154..72538dc3f81 100644 --- a/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp @@ -16,11 +16,11 @@ extern "C" { /* * Class: edu_wpi_first_math_jni_Ellipse2dJNI - * Method: findNearestPoint + * Method: nearest * Signature: (DDDDDDD[D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_math_jni_Ellipse2dJNI_findNearestPoint +Java_edu_wpi_first_math_jni_Ellipse2dJNI_nearest (JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading, jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY, jdoubleArray nearestPoint) @@ -30,7 +30,7 @@ Java_edu_wpi_first_math_jni_Ellipse2dJNI_findNearestPoint frc::Pose2d{units::meter_t{centerX}, units::meter_t{centerY}, units::radian_t{centerHeading}}, units::meter_t{xSemiAxis}, units::meter_t{ySemiAxis}} - .FindNearestPoint({units::meter_t{pointX}, units::meter_t{pointY}}); + .Nearest({units::meter_t{pointX}, units::meter_t{pointY}}); wpi::array buf{point.X().value(), point.Y().value()}; env->SetDoubleArrayRegion(nearestPoint, 0, 2, buf.data()); diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 785255d6748..1e8ae2a1081 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -206,9 +206,12 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { return Eigen::Vector3d{{pose.Translation().X().value(), pose.Translation().Y().value(), pose.Rotation().Radians().value()}}; @@ -220,9 +223,12 @@ constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { return Eigen::Vector4d{{pose.Translation().X().value(), pose.Translation().Y().value(), pose.Rotation().Cos(), pose.Rotation().Sin()}}; @@ -311,9 +317,12 @@ bool IsDetectable(const Matrixd& A, * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { return Eigen::Vector3d{ {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}}; } diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 932172810df..87099047f5b 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -84,14 +84,15 @@ class SimpleMotorFeedforward { } /** - * Calculates the feedforward from the gains and setpoint assuming discrete - * control. Use this method when the setpoint does not change. + * Calculates the feedforward from the gains and velocity setpoint assuming + * discrete control. Use this method when the velocity setpoint does not + * change. * - * @param setpoint The velocity setpoint. + * @param velocity The velocity setpoint. * @return The computed feedforward, in volts. */ - constexpr units::volt_t Calculate(units::unit_t setpoint) const { - return Calculate(setpoint, setpoint); + constexpr units::volt_t Calculate(units::unit_t velocity) const { + return Calculate(velocity, velocity); } /** diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h index 7b6f9056e7c..ce339db276e 100644 --- a/wpimath/src/main/native/include/frc/ct_matrix.h +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -308,6 +308,23 @@ class ct_matrix { (*this)(0) * rhs(1) - rhs(0) * (*this)(1)}}; } + /** + * Constexpr version of Eigen's 2x2 matrix determinant member function. + * + * @return Determinant of matrix. + */ + constexpr Scalar determinant() const + requires(Rows == 2 && Cols == 2) + { + // |a b| + // |c d| = ad - bc + Scalar a = (*this)(0, 0); + Scalar b = (*this)(0, 1); + Scalar c = (*this)(1, 0); + Scalar d = (*this)(1, 1); + return a * d - b * c; + } + /** * Constexpr version of Eigen's 3x3 matrix determinant member function. * @@ -364,7 +381,9 @@ using ct_vector = ct_matrix; template using ct_row_vector = ct_matrix; +using ct_matrix2d = ct_matrix; using ct_matrix3d = ct_matrix; +using ct_vector2d = ct_vector; using ct_vector3d = ct_vector; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index 4cd59f55b3d..629faf5922d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -154,7 +154,7 @@ class WPILIB_DLLEXPORT Ellipse2d { * @return The distance (0, if the point is contained by the ellipse) */ units::meter_t Distance(const Translation2d& point) const { - return FindNearestPoint(point).Distance(point); + return Nearest(point).Distance(point); } /** @@ -164,7 +164,7 @@ class WPILIB_DLLEXPORT Ellipse2d { * @return A new point that is nearest to {@code point} and contained in the * ellipse. */ - Translation2d FindNearestPoint(const Translation2d& point) const; + Translation2d Nearest(const Translation2d& point) const; /** * Checks equality between this Ellipse2d and another object. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 016de906245..0664161d6d3 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -53,6 +53,21 @@ class WPILIB_DLLEXPORT Pose2d { constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation) : m_translation{x, y}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Pose2d(const Eigen::Matrix3d& matrix) + : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}}, + m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)}, + {matrix(1, 0), matrix(1, 1)}}} { + if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Transforms the pose by the given transformation and returns the new * transformed pose. @@ -202,6 +217,17 @@ class WPILIB_DLLEXPORT Pose2d { */ constexpr Twist2d Log(const Pose2d& end) const; + /** + * Returns an affine transformation matrix representation of this pose. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)}, + {mat(1, 0), mat(1, 1), vec(1)}, + {0.0, 0.0, 1.0}}; + } + /** * Returns the nearest Pose2d from a collection of poses * @param poses The collection of poses. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index dc6a7416fd0..1f377a8ce71 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -54,6 +55,25 @@ class WPILIB_DLLEXPORT Pose3d { Rotation3d rotation) : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Pose3d(const Eigen::Matrix4d& matrix) + : m_translation{Eigen::Vector3d{ + {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}}, + m_rotation{ + Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)}, + {matrix(1, 0), matrix(1, 1), matrix(1, 2)}, + {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} { + if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 || + matrix(3, 3) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -218,6 +238,18 @@ class WPILIB_DLLEXPORT Pose3d { */ constexpr Twist3d Log(const Pose3d& end) const; + /** + * Returns an affine transformation matrix representation of this pose. + */ + constexpr Eigen::Matrix4d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)}, + {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)}, + {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)}, + {0.0, 0.0, 0.0, 1.0}}; + } + /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h index f03ca98c448..744b374ba23 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -155,7 +155,7 @@ class WPILIB_DLLEXPORT Rectangle2d { * @return The distance (0, if the point is contained by the rectangle) */ constexpr units::meter_t Distance(const Translation2d& point) const { - return FindNearestPoint(point).Distance(point); + return Nearest(point).Distance(point); } /** @@ -165,7 +165,7 @@ class WPILIB_DLLEXPORT Rectangle2d { * @return A new point that is nearest to {@code point} and contained in the * rectangle. */ - constexpr Translation2d FindNearestPoint(const Translation2d& point) const { + constexpr Translation2d Nearest(const Translation2d& point) const { // Check if already in rectangle if (Contains(point)) { return point; diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index a8899304f23..4251cabe0ad 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -5,12 +5,16 @@ #pragma once #include +#include +#include +#include #include #include #include #include +#include "frc/ct_matrix.h" #include "units/angle.h" #include "wpimath/MathShared.h" @@ -66,6 +70,43 @@ class WPILIB_DLLEXPORT Rotation2d { m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; } + /** + * Constructs a Rotation2d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws std::domain_error if the rotation matrix isn't special orthogonal. + */ + constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) { + auto impl = + [](const Matrix2d& R) -> std::pair { + // Require that the rotation matrix is special orthogonal. This is true if + // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). + if ((R * R.transpose() - Matrix2d::Identity()).norm() > 1e-9) { + throw std::domain_error("Rotation matrix isn't orthogonal"); + } + if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + throw std::domain_error( + "Rotation matrix is orthogonal but not special orthogonal"); + } + + // R = [cosθ −sinθ] + // [sinθ cosθ] + return {R(0, 0), R(1, 0)}; + }; + + if (std::is_constant_evaluated()) { + auto cossin = impl(ct_matrix2d{rotationMatrix}); + m_cos = std::get<0>(cossin); + m_sin = std::get<1>(cossin); + } else { + auto cossin = impl(rotationMatrix); + m_cos = std::get<0>(cossin); + m_sin = std::get<1>(cossin); + } + + m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; + } + /** * Adds two rotations together, with the result being bounded between -π and * π. @@ -154,6 +195,15 @@ class WPILIB_DLLEXPORT Rotation2d { Cos() * other.Sin() + Sin() * other.Cos()}; } + /** + * Returns matrix representation of this rotation. + */ + constexpr Eigen::Matrix2d ToMatrix() const { + // R = [cosθ −sinθ] + // [sinθ cosθ] + return Eigen::Matrix2d{{m_cos, -m_sin}, {m_sin, m_cos}}; + } + /** * Returns the radian value of the rotation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index f27d1d94131..47aa165b7c4 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -403,6 +403,24 @@ class WPILIB_DLLEXPORT Rotation3d { return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())}; } + /** + * Returns rotation matrix representation of this rotation. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix + return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y)}, + {2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x)}, + {2.0 * (x * z - w * y), 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y)}}; + } + /** * Returns a Rotation2d representing this Rotation3d projected into the X-Y * plane. diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index abf5c1ff4de..88c00550d04 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -49,6 +49,21 @@ class WPILIB_DLLEXPORT Transform2d { constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation) : m_translation{x, y}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Transform2d(const Eigen::Matrix3d& matrix) + : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}}, + m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)}, + {matrix(1, 0), matrix(1, 1)}}} { + if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs the identity transform -- maps an initial pose to itself. */ @@ -75,6 +90,18 @@ class WPILIB_DLLEXPORT Transform2d { */ constexpr units::meter_t Y() const { return m_translation.Y(); } + /** + * Returns an affine transformation matrix representation of this + * transformation. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)}, + {mat(1, 0), mat(1, 1), vec(1)}, + {0.0, 0.0, 1.0}}; + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 4716b7181e5..bd6ca9c41e0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -51,6 +51,25 @@ class WPILIB_DLLEXPORT Transform3d { Rotation3d rotation) : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Transform3d(const Eigen::Matrix4d& matrix) + : m_translation{Eigen::Vector3d{ + {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}}, + m_rotation{ + Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)}, + {matrix(1, 0), matrix(1, 1), matrix(1, 2)}, + {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} { + if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 || + matrix(3, 3) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs the identity transform -- maps an initial pose to itself. */ @@ -95,6 +114,19 @@ class WPILIB_DLLEXPORT Transform3d { */ constexpr units::meter_t Z() const { return m_translation.Z(); } + /** + * Returns an affine transformation matrix representation of this + * transformation. + */ + constexpr Eigen::Matrix4d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)}, + {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)}, + {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)}, + {0.0, 0.0, 0.0, 1.0}}; + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index a8a26b8bff5..aa6c4753ef7 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -54,13 +54,13 @@ class WPILIB_DLLEXPORT Translation2d { : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {} /** - * Constructs a Translation2d from the provided translation vector's X and Y - * components. The values are assumed to be in meters. + * Constructs a Translation2d from a 2D translation vector. The values are + * assumed to be in meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ constexpr explicit Translation2d(const Eigen::Vector2d& vector) - : m_x{units::meter_t{vector(0)}}, m_y{units::meter_t{vector(1)}} {} + : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {} /** * Calculates the distance between two translations in 2D space. @@ -90,9 +90,9 @@ class WPILIB_DLLEXPORT Translation2d { constexpr units::meter_t Y() const { return m_y; } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ constexpr Eigen::Vector2d ToVector() const { return Eigen::Vector2d{{m_x.value(), m_y.value()}}; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index fb3194b8ced..c16fc56a5b8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -56,10 +56,10 @@ class WPILIB_DLLEXPORT Translation3d { } /** - * Constructs a Translation3d from the provided translation vector's X, Y, and - * Z components. The values are assumed to be in meters. + * Constructs a Translation3d from a 3D translation vector. The values are + * assumed to be in meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ constexpr explicit Translation3d(const Eigen::Vector3d& vector) : m_x{units::meter_t{vector.x()}}, @@ -114,9 +114,9 @@ class WPILIB_DLLEXPORT Translation3d { constexpr units::meter_t Z() const { return m_z; } /** - * Returns a vector representation of this translation. + * Returns a 3D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 3D translation vector representation of this translation. */ constexpr Eigen::Vector3d ToVector() const { return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp index bdbeb473022..ef9a15bf69d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp @@ -204,6 +204,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { // Return zero @@ -218,22 +220,20 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value * rhs->value); } // Evaluate expression type ExpressionType type; - if (lhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant) { type = rhs->type; - } else if (rhs->type == ExpressionType::kConstant) { + } else if (rhs->type == kConstant) { type = lhs->type; - } else if (lhs->type == ExpressionType::kLinear && - rhs->type == ExpressionType::kLinear) { - type = ExpressionType::kQuadratic; + } else if (lhs->type == kLinear && rhs->type == kLinear) { + type = kQuadratic; } else { - type = ExpressionType::kNonlinear; + type = kNonlinear; } return MakeExpressionPtr( @@ -259,6 +259,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { // Return zero @@ -268,17 +270,16 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value / rhs->value); } // Evaluate expression type ExpressionType type; - if (rhs->type == ExpressionType::kConstant) { + if (rhs->type == kConstant) { type = lhs->type; } else { - type = ExpressionType::kNonlinear; + type = kNonlinear; } return MakeExpressionPtr( @@ -306,6 +307,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs == nullptr || lhs->IsConstant(0.0)) { return rhs; @@ -314,8 +317,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value + rhs->value); } @@ -339,6 +341,8 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { if (rhs->IsConstant(0.0)) { @@ -352,8 +356,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant && - rhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant && rhs->type == kConstant) { return MakeExpressionPtr(lhs->value - rhs->value); } @@ -375,6 +378,8 @@ struct SLEIPNIR_DLLEXPORT Expression { * @param lhs Operand of unary minus. */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { + using enum ExpressionType; + // Prune expression if (lhs->IsConstant(0.0)) { // Return zero @@ -382,7 +387,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant - if (lhs->type == ExpressionType::kConstant) { + if (lhs->type == kConstant) { return MakeExpressionPtr(-lhs->value); } @@ -465,6 +470,8 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -472,12 +479,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::abs(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::abs(x); }, + kNonlinear, [](double x, double) { return std::abs(x); }, [](double x, double, double parentAdjoint) { if (x < 0.0) { return -parentAdjoint; @@ -508,18 +515,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(std::numbers::pi / 2.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::acos(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::acos(x); }, + kNonlinear, [](double x, double) { return std::acos(x); }, [](double x, double, double parentAdjoint) { return -parentAdjoint / std::sqrt(1.0 - x * x); }, @@ -538,6 +547,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -545,12 +556,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::asin(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::asin(x); }, + kNonlinear, [](double x, double) { return std::asin(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / std::sqrt(1.0 - x * x); }, @@ -569,6 +580,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -576,12 +589,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::atan(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::atan(x); }, + kNonlinear, [](double x, double) { return std::atan(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (1.0 + x * x); }, @@ -600,6 +613,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT const ExpressionPtr& y, const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (y->IsConstant(0.0)) { // Return zero @@ -609,14 +624,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT } // Evaluate constant - if (y->type == ExpressionType::kConstant && - x->type == ExpressionType::kConstant) { + if (y->type == kConstant && x->type == kConstant) { return MakeExpressionPtr(std::atan2(y->value, x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, - [](double y, double x) { return std::atan2(y, x); }, + kNonlinear, [](double y, double x) { return std::atan2(y, x); }, [](double y, double x, double parentAdjoint) { return parentAdjoint * x / (y * y + x * x); }, @@ -641,18 +654,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(1.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::cos(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::cos(x); }, + kNonlinear, [](double x, double) { return std::cos(x); }, [](double x, double, double parentAdjoint) { return -parentAdjoint * std::sin(x); }, @@ -670,18 +685,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(1.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::cosh(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::cosh(x); }, + kNonlinear, [](double x, double) { return std::cosh(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::sinh(x); }, @@ -699,6 +716,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -706,12 +725,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::erf(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::erf(x); }, + kNonlinear, [](double x, double) { return std::erf(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * std::exp(-x * x); @@ -732,18 +751,20 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return MakeExpressionPtr(1.0); } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::exp(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::exp(x); }, + kNonlinear, [](double x, double) { return std::exp(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::exp(x); }, @@ -762,6 +783,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT const ExpressionPtr& x, const ExpressionPtr& y) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { return y; @@ -770,14 +793,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant && - y->type == ExpressionType::kConstant) { + if (x->type == kConstant && y->type == kConstant) { return MakeExpressionPtr(std::hypot(x->value, y->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, - [](double x, double y) { return std::hypot(x, y); }, + kNonlinear, [](double x, double y) { return std::hypot(x, y); }, [](double x, double y, double parentAdjoint) { return parentAdjoint * x / std::hypot(x, y); }, @@ -802,6 +823,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -809,12 +832,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::log(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::log(x); }, + kNonlinear, [](double x, double) { return std::log(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, [](const ExpressionPtr& x, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, @@ -828,6 +851,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -835,13 +860,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::log10(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, - [](double x, double) { return std::log10(x); }, + kNonlinear, [](double x, double) { return std::log10(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (std::numbers::ln10 * x); }, @@ -860,6 +884,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT const ExpressionPtr& base, const ExpressionPtr& power) { + using enum ExpressionType; + // Prune expression if (base->IsConstant(0.0)) { // Return zero @@ -874,15 +900,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT } // Evaluate constant - if (base->type == ExpressionType::kConstant && - power->type == ExpressionType::kConstant) { + if (base->type == kConstant && power->type == kConstant) { return MakeExpressionPtr(std::pow(base->value, power->value)); } return MakeExpressionPtr( - base->type == ExpressionType::kLinear && power->IsConstant(2.0) - ? ExpressionType::kQuadratic - : ExpressionType::kNonlinear, + base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, [](double base, double power) { return std::pow(base, power); }, [](double base, double power, double parentAdjoint) { return parentAdjoint * std::pow(base, power - 1) * power; @@ -923,8 +946,10 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { + using enum ExpressionType; + // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { if (x->value < 0.0) { return MakeExpressionPtr(-1.0); } else if (x->value == 0.0) { @@ -936,7 +961,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { } return MakeExpressionPtr( - ExpressionType::kNonlinear, + kNonlinear, [](double x, double) { if (x < 0.0) { return -1.0; @@ -961,6 +986,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -968,12 +995,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::sin(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::sin(x); }, + kNonlinear, [](double x, double) { return std::sin(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::cos(x); }, @@ -990,6 +1017,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -997,12 +1026,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::sinh(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::sinh(x); }, + kNonlinear, [](double x, double) { return std::sinh(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint * std::cosh(x); }, @@ -1020,8 +1049,10 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { if (x->value == 0.0) { // Return zero return x; @@ -1033,7 +1064,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::sqrt(x); }, + kNonlinear, [](double x, double) { return std::sqrt(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (2.0 * std::sqrt(x)); }, @@ -1052,6 +1083,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -1059,12 +1092,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::tan(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::tan(x); }, + kNonlinear, [](double x, double) { return std::tan(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (std::cos(x) * std::cos(x)); }, @@ -1082,6 +1115,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { + using enum ExpressionType; + // Prune expression if (x->IsConstant(0.0)) { // Return zero @@ -1089,12 +1124,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { } // Evaluate constant - if (x->type == ExpressionType::kConstant) { + if (x->type == kConstant) { return MakeExpressionPtr(std::tanh(x->value)); } return MakeExpressionPtr( - ExpressionType::kNonlinear, [](double x, double) { return std::tanh(x); }, + kNonlinear, [](double x, double) { return std::tanh(x); }, [](double x, double, double parentAdjoint) { return parentAdjoint / (std::cosh(x) * std::cosh(x)); }, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp index e812fa5e345..66883fed98a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp @@ -20,6 +20,7 @@ #include "sleipnir/optimization/SolverIterationInfo.hpp" #include "sleipnir/optimization/SolverStatus.hpp" #include "sleipnir/optimization/solver/InteriorPoint.hpp" +#include "sleipnir/optimization/solver/SQP.hpp" #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" @@ -305,10 +306,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { } // Solve the optimization problem - Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size()); - InteriorPoint(m_decisionVariables, m_equalityConstraints, - m_inequalityConstraints, m_f.value(), m_callback, config, - false, x, s, &status); + if (m_inequalityConstraints.empty()) { + SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback, + config, x, &status); + } else { + Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size()); + InteriorPoint(m_decisionVariables, m_equalityConstraints, + m_inequalityConstraints, m_f.value(), m_callback, config, + false, x, s, &status); + } if (config.diagnostics) { sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition)); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp index 734cd3d1273..7d1445297e3 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp @@ -46,29 +46,31 @@ enum class SolverExitCondition : int8_t { */ SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( const SolverExitCondition& exitCondition) { + using enum SolverExitCondition; + switch (exitCondition) { - case SolverExitCondition::kSuccess: + case kSuccess: return "solved to desired tolerance"; - case SolverExitCondition::kSolvedToAcceptableTolerance: + case kSolvedToAcceptableTolerance: return "solved to acceptable tolerance"; - case SolverExitCondition::kCallbackRequestedStop: + case kCallbackRequestedStop: return "callback requested stop"; - case SolverExitCondition::kTooFewDOFs: + case kTooFewDOFs: return "problem has too few degrees of freedom"; - case SolverExitCondition::kLocallyInfeasible: + case kLocallyInfeasible: return "problem is locally infeasible"; - case SolverExitCondition::kFeasibilityRestorationFailed: + case kFeasibilityRestorationFailed: return "solver failed to reach the desired tolerance, and feasibility " "restoration failed to converge"; - case SolverExitCondition::kNonfiniteInitialCostOrConstraints: + case kNonfiniteInitialCostOrConstraints: return "solver encountered nonfinite initial cost or constraints and " "gave up"; - case SolverExitCondition::kDivergingIterates: + case kDivergingIterates: return "solver encountered diverging primal iterates xₖ and/or sₖ and " "gave up"; - case SolverExitCondition::kMaxIterationsExceeded: + case kMaxIterationsExceeded: return "solution returned after maximum iterations exceeded"; - case SolverExitCondition::kTimeout: + case kTimeout: return "solution returned after maximum wall clock time exceeded"; default: return "unknown"; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp new file mode 100644 index 00000000000..ed10ffedff2 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp @@ -0,0 +1,46 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/autodiff/Variable.hpp" +#include "sleipnir/optimization/SolverConfig.hpp" +#include "sleipnir/optimization/SolverIterationInfo.hpp" +#include "sleipnir/optimization/SolverStatus.hpp" +#include "sleipnir/util/FunctionRef.hpp" +#include "sleipnir/util/SymbolExports.hpp" + +namespace sleipnir { + +/** +Finds the optimal solution to a nonlinear program using Sequential Quadratic +Programming (SQP). + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 +@endverbatim + +where f(x) is the cost function and cₑ(x) are the equality constraints. + +@param[in] decisionVariables The list of decision variables. +@param[in] equalityConstraints The list of equality constraints. +@param[in] f The cost function. +@param[in] callback The user callback. +@param[in] config Configuration options for the solver. +@param[in,out] x The initial guess and output location for the decision + variables. +@param[out] status The solver status. +*/ +SLEIPNIR_DLLEXPORT void SQP( + std::span decisionVariables, + std::span equalityConstraints, Variable& f, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status); + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp index 892d2dd20f7..d3981c59d16 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp @@ -195,7 +195,7 @@ void InteriorPoint(std::span decisionVariables, // Fraction-to-the-boundary rule scale factor τ double τ = τ_min; - Filter filter{f, μ}; + Filter filter{f}; // This should be run when the error estimate is below a desired threshold for // the current barrier parameter @@ -222,7 +222,7 @@ void InteriorPoint(std::span decisionVariables, τ = std::max(τ_min, 1.0 - μ); // Reset the filter when the barrier parameter is updated - filter.Reset(μ); + filter.Reset(); }; // Kept outside the loop so its storage can be reused @@ -372,6 +372,9 @@ void InteriorPoint(std::span decisionVariables, rhs.segment(x.rows(), y.rows()) = -c_e; // Solve the Newton-KKT system + // + // [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] + // [ Aₑ 0 ][−pₖʸ] [ cₑ ] solver.Compute(lhs, equalityConstraints.size(), μ); Eigen::VectorXd step{x.rows() + y.rows()}; if (solver.Info() == Eigen::Success) { @@ -435,7 +438,7 @@ void InteriorPoint(std::span decisionVariables, } // Check whether filter accepts trial iterate - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i); + auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); if (filter.TryAdd(entry)) { // Accept step break; @@ -499,7 +502,7 @@ void InteriorPoint(std::span decisionVariables, trial_c_i = c_iAD.Value(); // Check whether filter accepts trial iterate - entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i); + entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); if (filter.TryAdd(entry)) { p_x = p_x_cor; p_y = p_y_soc; @@ -531,7 +534,7 @@ void InteriorPoint(std::span decisionVariables, if (fullStepRejectedCounter >= 4 && filter.maxConstraintViolation > entry.constraintViolation / 10.0) { filter.maxConstraintViolation *= 0.1; - filter.Reset(μ); + filter.Reset(); continue; } @@ -580,7 +583,7 @@ void InteriorPoint(std::span decisionVariables, return; } - auto initialEntry = filter.MakeEntry(s, c_e, c_i); + auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ); // Feasibility restoration phase Eigen::VectorXd fr_x = x; @@ -603,7 +606,7 @@ void InteriorPoint(std::span decisionVariables, // If current iterate is acceptable to normal filter and // constraint violation has sufficiently reduced, stop // feasibility restoration - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i); + auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); if (filter.IsAcceptable(entry) && entry.constraintViolation < 0.9 * initialEntry.constraintViolation) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp new file mode 100644 index 00000000000..662abc2fb6e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp @@ -0,0 +1,559 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/SQP.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include "optimization/RegularizedLDLT.hpp" +#include "optimization/solver/util/ErrorEstimate.hpp" +#include "optimization/solver/util/FeasibilityRestoration.hpp" +#include "optimization/solver/util/Filter.hpp" +#include "optimization/solver/util/IsLocallyInfeasible.hpp" +#include "optimization/solver/util/KKTError.hpp" +#include "sleipnir/autodiff/Gradient.hpp" +#include "sleipnir/autodiff/Hessian.hpp" +#include "sleipnir/autodiff/Jacobian.hpp" +#include "sleipnir/optimization/SolverExitCondition.hpp" +#include "sleipnir/util/Print.hpp" +#include "sleipnir/util/Spy.hpp" +#include "util/ScopeExit.hpp" +#include "util/ToMilliseconds.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace sleipnir { + +void SQP(std::span decisionVariables, + std::span equalityConstraints, Variable& f, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { + const auto solveStartTime = std::chrono::system_clock::now(); + + // Map decision variables and constraints to VariableMatrices for Lagrangian + VariableMatrix xAD{decisionVariables}; + xAD.SetValue(x); + VariableMatrix c_eAD{equalityConstraints}; + + // Create autodiff variables for y for Lagrangian + VariableMatrix yAD(equalityConstraints.size()); + for (auto& y : yAD) { + y.SetValue(0.0); + } + + // Lagrangian L + // + // L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) + auto L = f - (yAD.T() * c_eAD)(0); + + // Equality constraint Jacobian Aₑ + // + // [∇ᵀcₑ₁(xₖ)] + // Aₑ(x) = [∇ᵀcₑ₂(xₖ)] + // [ ⋮ ] + // [∇ᵀcₑₘ(xₖ)] + Jacobian jacobianCe{c_eAD, xAD}; + Eigen::SparseMatrix A_e = jacobianCe.Value(); + + // Gradient of f ∇f + Gradient gradientF{f, xAD}; + Eigen::SparseVector g = gradientF.Value(); + + // Hessian of the Lagrangian H + // + // Hₖ = ∇²ₓₓL(xₖ, yₖ) + Hessian hessianL{L, xAD}; + Eigen::SparseMatrix H = hessianL.Value(); + + Eigen::VectorXd y = yAD.Value(); + Eigen::VectorXd c_e = c_eAD.Value(); + + // Check for overconstrained problem + if (equalityConstraints.size() > decisionVariables.size()) { + if (config.diagnostics) { + sleipnir::println("The problem has too few degrees of freedom."); + sleipnir::println( + "Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e(row) < 0.0) { + sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); + } + } + } + + status->exitCondition = SolverExitCondition::kTooFewDOFs; + return; + } + + // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) + if (!std::isfinite(f.Value()) || !c_e.allFinite()) { + status->exitCondition = + SolverExitCondition::kNonfiniteInitialCostOrConstraints; + return; + } + + // Sparsity pattern files written when spy flag is set in SolverConfig + std::ofstream H_spy; + std::ofstream A_e_spy; + if (config.spy) { + A_e_spy.open("A_e.spy"); + H_spy.open("H.spy"); + } + + if (config.diagnostics) { + sleipnir::println("Error tolerance: {}\n", config.tolerance); + } + + std::chrono::system_clock::time_point iterationsStartTime; + + int iterations = 0; + + // Prints final diagnostics when the solver exits + scope_exit exit{[&] { + status->cost = f.Value(); + + if (config.diagnostics) { + auto solveEndTime = std::chrono::system_clock::now(); + + sleipnir::println("\nSolve time: {:.3f} ms", + ToMilliseconds(solveEndTime - solveStartTime)); + sleipnir::println(" ↳ {:.3f} ms (solver setup)", + ToMilliseconds(iterationsStartTime - solveStartTime)); + if (iterations > 0) { + sleipnir::println( + " ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)", + ToMilliseconds(solveEndTime - iterationsStartTime), iterations, + ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); + } + sleipnir::println(""); + + sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff", + "setup (ms)", "avg solve (ms)", "solves"); + sleipnir::println("{:=^47}", ""); + constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}"; + sleipnir::println(format, "∇f(x)", + gradientF.GetProfiler().SetupDuration(), + gradientF.GetProfiler().AverageSolveDuration(), + gradientF.GetProfiler().SolveMeasurements()); + sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), + hessianL.GetProfiler().AverageSolveDuration(), + hessianL.GetProfiler().SolveMeasurements()); + sleipnir::println(format, "∂cₑ/∂x", + jacobianCe.GetProfiler().SetupDuration(), + jacobianCe.GetProfiler().AverageSolveDuration(), + jacobianCe.GetProfiler().SolveMeasurements()); + sleipnir::println(""); + } + }}; + + Filter filter{f}; + + // Kept outside the loop so its storage can be reused + wpi::SmallVector> triplets; + + RegularizedLDLT solver; + + // Variables for determining when a step is acceptable + constexpr double α_red_factor = 0.5; + int acceptableIterCounter = 0; + + int fullStepRejectedCounter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + if (config.diagnostics) { + iterationsStartTime = std::chrono::system_clock::now(); + } + + while (E_0 > config.tolerance && + acceptableIterCounter < config.maxAcceptableIterations) { + std::chrono::system_clock::time_point innerIterStartTime; + if (config.diagnostics) { + innerIterStartTime = std::chrono::system_clock::now(); + } + + // Check for local equality constraint infeasibility + if (IsEqualityLocallyInfeasible(A_e, c_e)) { + if (config.diagnostics) { + sleipnir::println( + "The problem is locally infeasible due to violated equality " + "constraints."); + sleipnir::println( + "Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e(row) < 0.0) { + sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); + } + } + } + + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + return; + } + + // Check for diverging iterates + if (x.lpNorm() > 1e20 || !x.allFinite()) { + status->exitCondition = SolverExitCondition::kDivergingIterates; + return; + } + + // Write out spy file contents if that's enabled + if (config.spy) { + // Gap between sparsity patterns + if (iterations > 0) { + A_e_spy << "\n"; + H_spy << "\n"; + } + + Spy(H_spy, H); + Spy(A_e_spy, A_e); + } + + // Call user callback + if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e, + Eigen::SparseMatrix{}})) { + status->exitCondition = SolverExitCondition::kCallbackRequestedStop; + return; + } + + // lhs = [H Aₑᵀ] + // [Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + const Eigen::SparseMatrix topLeft = + H.triangularView(); + triplets.clear(); + triplets.reserve(topLeft.nonZeros() + A_e.nonZeros()); + for (int col = 0; col < H.cols(); ++col) { + // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{topLeft, col}; it; + ++it) { + triplets.emplace_back(it.row(), it.col(), it.value()); + } + // Append column of Aₑ in bottom-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix lhs( + decisionVariables.size() + equalityConstraints.size(), + decisionVariables.size() + equalityConstraints.size()); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), + [](const auto&, const auto& b) { return b; }); + + // rhs = −[∇f − Aₑᵀy] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y); + rhs.segment(x.rows(), y.rows()) = -c_e; + + // Solve the Newton-KKT system + // + // [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy] + // [Aₑ 0 ][−pₖʸ] [ cₑ ] + solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0); + Eigen::VectorXd step{x.rows() + y.rows()}; + if (solver.Info() == Eigen::Success) { + step = solver.Solve(rhs); + } else { + // The regularization procedure failed due to a rank-deficient equality + // constraint Jacobian with linearly dependent constraints + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + return; + } + + // step = [ pₖˣ] + // [−pₖʸ] + Eigen::VectorXd p_x = step.segment(0, x.rows()); + Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); + + constexpr double α_max = 1.0; + double α = α_max; + + // Loop until a step is accepted. If a step becomes acceptable, the loop + // will exit early. + while (1) { + Eigen::VectorXd trial_x = x + α * p_x; + Eigen::VectorXd trial_y = y + α * p_y; + + xAD.SetValue(trial_x); + + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size + // immediately + if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) { + // Reduce step size + α *= α_red_factor; + continue; + } + + // Check whether filter accepts trial iterate + auto entry = filter.MakeEntry(trial_c_e); + if (filter.TryAdd(entry)) { + // Accept step + break; + } + + double prevConstraintViolation = c_e.lpNorm<1>(); + double nextConstraintViolation = trial_c_e.lpNorm<1>(); + + // Second-order corrections + // + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (nextConstraintViolation >= prevConstraintViolation) { + // Apply second-order corrections. See section 2.4 of [2]. + Eigen::VectorXd p_x_cor = p_x; + Eigen::VectorXd p_y_soc = p_y; + + double α_soc = α; + Eigen::VectorXd c_e_soc = c_e; + + bool stepAcceptable = false; + for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; + ++soc_iteration) { + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + step = solver.Solve(rhs); + + p_x_cor = step.segment(0, x.rows()); + p_y_soc = -step.segment(x.rows(), y.rows()); + + trial_x = x + α_soc * p_x_cor; + trial_y = y + α_soc * p_y_soc; + + xAD.SetValue(trial_x); + + trial_c_e = c_eAD.Value(); + + // Check whether filter accepts trial iterate + entry = filter.MakeEntry(trial_c_e); + if (filter.TryAdd(entry)) { + p_x = p_x_cor; + p_y = p_y_soc; + α = α_soc; + stepAcceptable = true; + } + } + + if (stepAcceptable) { + // Accept step + break; + } + } + + // If we got here and α is the full step, the full step was rejected. + // Increment the full-step rejected counter to keep track of how many full + // steps have been rejected in a row. + if (α == α_max) { + ++fullStepRejectedCounter; + } + + // If the full step was rejected enough times in a row, reset the filter + // because it may be impeding progress. + // + // See section 3.2 case I of [2]. + if (fullStepRejectedCounter >= 4 && + filter.maxConstraintViolation > entry.constraintViolation / 10.0) { + filter.maxConstraintViolation *= 0.1; + filter.Reset(); + continue; + } + + // Reduce step size + α *= α_red_factor; + + // Safety factor for the minimal step size + constexpr double α_min_frac = 0.05; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report infeasible. + if (α < α_min_frac * Filter::γConstraint) { + double currentKKTError = KKTError(g, A_e, c_e, y); + + Eigen::VectorXd trial_x = x + α_max * p_x; + Eigen::VectorXd trial_y = y + α_max * p_y; + + // Upate autodiff + xAD.SetValue(trial_x); + yAD.SetValue(trial_y); + + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + double nextKKTError = + KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (nextKKTError <= 0.999 * currentKKTError) { + α = α_max; + + // Accept step + break; + } + + auto initialEntry = filter.MakeEntry(c_e); + + // Feasibility restoration phase + Eigen::VectorXd fr_x = x; + SolverStatus fr_status; + FeasibilityRestoration( + decisionVariables, equalityConstraints, + [&](const SolverIterationInfo& info) { + trial_x = info.x.segment(0, decisionVariables.size()); + xAD.SetValue(trial_x); + + trial_c_e = c_eAD.Value(); + + // If current iterate is acceptable to normal filter and + // constraint violation has sufficiently reduced, stop + // feasibility restoration + entry = filter.MakeEntry(trial_c_e); + if (filter.IsAcceptable(entry) && + entry.constraintViolation < + 0.9 * initialEntry.constraintViolation) { + return true; + } + + return false; + }, + config, fr_x, &fr_status); + + if (fr_status.exitCondition == + SolverExitCondition::kCallbackRequestedStop) { + p_x = fr_x - x; + + // Lagrange mutliplier estimates + // + // y = (AₑAₑᵀ)⁻¹Aₑ∇f + // + // See equation (19.37) of [1]. + { + xAD.SetValue(fr_x); + + A_e = jacobianCe.Value(); + g = gradientF.Value(); + + // lhs = AₑAₑᵀ + Eigen::SparseMatrix lhs = A_e * A_e.transpose(); + + // rhs = Aₑ∇f + Eigen::VectorXd rhs = A_e * g; + + Eigen::SimplicialLDLT> yEstimator{lhs}; + Eigen::VectorXd sol = yEstimator.solve(rhs); + + p_y = y - sol.block(0, 0, y.rows(), 1); + } + + α = 1.0; + + // Accept step + break; + } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) { + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + x = fr_x; + return; + } else { + status->exitCondition = + SolverExitCondition::kFeasibilityRestorationFailed; + x = fr_x; + return; + } + } + } + + // If full step was accepted, reset full-step rejected counter + if (α == α_max) { + fullStepRejectedCounter = 0; + } + + // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when + // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. + // + // See section 3.9 of [2]. + double maxStepScaled = 0.0; + for (int row = 0; row < x.rows(); ++row) { + maxStepScaled = std::max(maxStepScaled, + std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); + } + if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { + α = α_max; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // yₖ₊₁ = yₖ + αₖpₖʸ + x += α * p_x; + y += α * p_y; + + // Update autodiff for Jacobians and Hessian + xAD.SetValue(x); + yAD.SetValue(y); + A_e = jacobianCe.Value(); + g = gradientF.Value(); + H = hessianL.Value(); + + c_e = c_eAD.Value(); + + // Update the error estimate + E_0 = ErrorEstimate(g, A_e, c_e, y); + if (E_0 < config.acceptableTolerance) { + ++acceptableIterCounter; + } else { + acceptableIterCounter = 0; + } + + const auto innerIterEndTime = std::chrono::system_clock::now(); + + // Diagnostics for current iteration + if (config.diagnostics) { + if (iterations % 20 == 0) { + sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter", + "time (ms)", "error", "cost", "infeasibility"); + sleipnir::println("{:=^61}", ""); + } + + sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations, + ToMilliseconds(innerIterEndTime - innerIterStartTime), + E_0, f.Value(), c_e.lpNorm<1>()); + } + + ++iterations; + + // Check for max iterations + if (iterations >= config.maxIterations) { + status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; + return; + } + + // Check for max wall clock time + if (innerIterEndTime - solveStartTime > config.timeout) { + status->exitCondition = SolverExitCondition::kTimeout; + return; + } + + // Check for solve to acceptable tolerance + if (E_0 > config.tolerance && + acceptableIterCounter == config.maxAcceptableIterations) { + status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; + return; + } + } +} + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp index 6b757df3e6b..f1490028dd2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp @@ -11,6 +11,42 @@ namespace sleipnir { +/** + * Returns the error estimate using the KKT conditions for SQP. + * + * @param g Gradient of the cost function ∇f. + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param y Equality constraint dual variables. + */ +inline double ErrorEstimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::VectorXd& y) { + int numEqualityConstraints = A_e.rows(); + + // Update the error estimate using the KKT conditions from equations (19.5a) + // through (19.5d) of [1]. + // + // ∇f − Aₑᵀy = 0 + // cₑ = 0 + // + // The error tolerance is the max of the following infinity norms scaled by + // s_d (see equation (5) of [2]). + // + // ‖∇f − Aₑᵀy‖_∞ / s_d + // ‖cₑ‖_∞ + + // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ + constexpr double s_max = 100.0; + double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max; + + return std::max({(g - A_e.transpose() * y).lpNorm() / s_d, + c_e.lpNorm()}); +} + /** * Returns the error estimate using the KKT conditions for the interior-point * method. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp index 0f13676aea0..79b5d99ae27 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -20,6 +20,168 @@ namespace sleipnir { +/** + * Finds the iterate that minimizes the constraint violation while not deviating + * too far from the starting point. This is a fallback procedure when the normal + * Sequential Quadratic Programming method fails to converge to a feasible + * point. + * + * @param[in] decisionVariables The list of decision variables. + * @param[in] equalityConstraints The list of equality constraints. + * @param[in] callback The user callback. + * @param[in] config Configuration options for the solver. + * @param[in,out] x The current iterate from the normal solve. + * @param[out] status The solver status. + */ +inline void FeasibilityRestoration( + std::span decisionVariables, + std::span equalityConstraints, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { + // Feasibility restoration + // + // min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R) + // x + // pₑ,nₑ + // + // s.t. cₑ(x) - pₑ + nₑ = 0 + // pₑ ≥ 0 + // nₑ ≥ 0 + // + // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original + // iterate before feasibility restoration, and D_R is a scaling matrix defined + // by + // + // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) + + constexpr double ρ = 1000.0; + double μ = config.tolerance / 10.0; + + wpi::SmallVector fr_decisionVariables; + fr_decisionVariables.reserve(decisionVariables.size() + + 2 * equalityConstraints.size()); + + // Assign x + fr_decisionVariables.assign(decisionVariables.begin(), + decisionVariables.end()); + + // Allocate pₑ and nₑ + for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) { + fr_decisionVariables.emplace_back(); + } + + auto it = fr_decisionVariables.begin(); + + VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; + it += decisionVariables.size(); + + VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}}; + it += equalityConstraints.size(); + + VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}}; + it += equalityConstraints.size(); + + // Set initial values for pₑ and nₑ. + // + // + // From equation (33) of [2]: + // ______________________ + // μ − ρ c(x) /(μ − ρ c(x))² μ c(x) + // n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1) + // 2ρ √ ( 2ρ ) 2ρ + // + // The quadratic formula: + // ________ + // -b + √b² - 4ac + // x = −−−−−−−−−−−−−− (2) + // 2a + // + // Rearrange (1) to fit the quadratic formula better: + // _________________________ + // μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x) + // n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− + // 2ρ + // + // Solve for coefficients: + // + // a = ρ (3) + // b = ρ c(x) - μ (4) + // + // -4ac = μ c(x) 2ρ + // -4(ρ)c = 2ρ μ c(x) + // -4c = 2μ c(x) + // c = -μ c(x)/2 (5) + // + // p = c(x) + n (6) + for (int row = 0; row < p_e.Rows(); ++row) { + double c_e = equalityConstraints[row].Value(); + + constexpr double a = 2 * ρ; + double b = ρ * c_e - μ; + double c = -μ * c_e / 2.0; + + double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); + double p = c_e + n; + + p_e(row).SetValue(p); + n_e(row).SetValue(n); + } + + // cₑ(x) - pₑ + nₑ = 0 + wpi::SmallVector fr_equalityConstraints; + fr_equalityConstraints.assign(equalityConstraints.begin(), + equalityConstraints.end()); + for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { + auto& constraint = fr_equalityConstraints[row]; + constraint = constraint - p_e(row) + n_e(row); + } + + // cᵢ(x) - s - pᵢ + nᵢ = 0 + wpi::SmallVector fr_inequalityConstraints; + + // pₑ ≥ 0 + std::copy(p_e.begin(), p_e.end(), + std::back_inserter(fr_inequalityConstraints)); + + // nₑ ≥ 0 + std::copy(n_e.begin(), n_e.end(), + std::back_inserter(fr_inequalityConstraints)); + + Variable J = 0.0; + + // J += ρ Σ (pₑ + nₑ) + for (auto& elem : p_e) { + J += elem; + } + for (auto& elem : n_e) { + J += elem; + } + J *= ρ; + + // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) + Eigen::VectorXd D_R{x.rows()}; + for (int row = 0; row < D_R.rows(); ++row) { + D_R(row) = std::min(1.0, 1.0 / std::abs(x(row))); + } + + // J += ζ/2 (x - x_R)ᵀD_R(x - x_R) + for (int row = 0; row < x.rows(); ++row) { + J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2); + } + + Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value(); + + // Set up initial value for inequality constraint slack variables + Eigen::VectorXd fr_s{fr_inequalityConstraints.size()}; + fr_s.setOnes(); + + InteriorPoint(fr_decisionVariables, fr_equalityConstraints, + fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s, + status); + + x = fr_x.segment(0, decisionVariables.size()); +} + /** * Finds the iterate that minimizes the constraint violation while not deviating * too far from the starting point. This is a fallback procedure when the normal diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp index 02bdb5a8db5..0c14efd7b8a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp @@ -12,6 +12,8 @@ #include "sleipnir/autodiff/Variable.hpp" +// See docs/algorithms.md#Works_cited for citation definitions. + namespace sleipnir { /** @@ -32,26 +34,14 @@ struct FilterEntry { * @param cost The cost function's value. * @param constraintViolation The constraint violation. */ - FilterEntry(double cost, double constraintViolation) + constexpr FilterEntry(double cost, double constraintViolation) : cost{cost}, constraintViolation{constraintViolation} {} - - /** - * Constructs a FilterEntry. - * - * @param f The cost function. - * @param μ The barrier parameter. - * @param s The inequality constraint slack variables. - * @param c_e The equality constraint values (nonzero means violation). - * @param c_i The inequality constraint values (negative means violation). - */ - FilterEntry(Variable& f, double μ, const Eigen::VectorXd& s, - const Eigen::VectorXd& c_e, const Eigen::VectorXd& c_i) - : cost{f.Value() - μ * s.array().log().sum()}, - constraintViolation{c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {} }; /** - * Interior-point step filter. + * Step filter. + * + * See the section on filters in chapter 15 of [1]. */ class Filter { public: @@ -64,11 +54,9 @@ class Filter { * Construct an empty filter. * * @param f The cost function. - * @param μ The barrier parameter. */ - explicit Filter(Variable& f, double μ) { + explicit Filter(Variable& f) { m_f = &f; - m_μ = μ; // Initial filter entry rejects constraint violations above max m_filter.emplace_back(std::numeric_limits::infinity(), @@ -77,11 +65,8 @@ class Filter { /** * Reset the filter. - * - * @param μ The new barrier parameter. */ - void Reset(double μ) { - m_μ = μ; + void Reset() { m_filter.clear(); // Initial filter entry rejects constraint violations above max @@ -90,15 +75,26 @@ class Filter { } /** - * Creates a new filter entry. + * Creates a new Sequential Quadratic Programming filter entry. + * + * @param c_e The equality constraint values (nonzero means violation). + */ + FilterEntry MakeEntry(const Eigen::VectorXd& c_e) { + return FilterEntry{m_f->Value(), c_e.lpNorm<1>()}; + } + + /** + * Creates a new interior-point method filter entry. * * @param s The inequality constraint slack variables. * @param c_e The equality constraint values (nonzero means violation). * @param c_i The inequality constraint values (negative means violation). + * @param μ The barrier parameter. */ FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e, - const Eigen::VectorXd& c_i) { - return FilterEntry{*m_f, m_μ, s, c_e, c_i}; + const Eigen::VectorXd& c_i, double μ) { + return FilterEntry{m_f->Value() - μ * s.array().log().sum(), + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()}; } /** @@ -181,7 +177,6 @@ class Filter { private: Variable* m_f = nullptr; - double m_μ = 0.0; wpi::SmallVector m_filter; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp index 03fa112b6a6..3b603159d21 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp @@ -9,6 +9,28 @@ namespace sleipnir { +/** + * Returns the KKT error for Sequential Quadratic Programming. + * + * @param g Gradient of the cost function ∇f. + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param y Equality constraint dual variables. + */ +inline double KKTError(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) { + // Compute the KKT error as the 1-norm of the KKT conditions from equations + // (19.5a) through (19.5d) of [1]. + // + // ∇f − Aₑᵀy = 0 + // cₑ = 0 + + return (g - A_e.transpose() * y).lpNorm<1>() + c_e.lpNorm<1>(); +} + /** * Returns the KKT error for the interior-point method. * diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 4cb3b055a23..2a31b2d7c65 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -33,13 +33,6 @@ message ProtobufMecanumDriveKinematics { ProtobufTranslation2d rear_right = 4; } -message ProtobufMecanumDriveMotorVoltages { - double front_left = 1; - double front_right = 2; - double rear_left = 3; - double rear_right = 4; -} - message ProtobufMecanumDriveWheelPositions { double front_left = 1; double front_right = 2; diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java index 4883e6f5ce0..1715d93dc69 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java @@ -153,6 +153,7 @@ void testMatrixExp() { } @Test + @SuppressWarnings("removal") void testPoseToVector() { Pose2d pose = new Pose2d(1, 2, new Rotation2d(3)); var vector = StateSpaceUtil.poseToVector(pose); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index 1fd6f58d25f..3c0a74835a2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.controller; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -31,23 +30,23 @@ void testCalculate() { var r = VecBuilder.fill(2.0); var nextR = VecBuilder.fill(3.0); - var currentVelocity = RadiansPerSecond.mutable(2.0); - var nextVelocity = RadiansPerSecond.mutable(3.0); + double currentVelocity = 2.0; // rad/s + double nextVelocity = 3.0; // rad/s assertEquals( 37.52499583432516 + 0.5, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 0.002); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 0.002); // These won't match exactly. It's just an approximation to make sure they're // in the same ballpark. assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(), + simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), 2.0); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 28c9d41d015..016a1c70d5b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -30,7 +30,7 @@ void testGetFocalPoints() { } @Test - void testIntersectsPoint() { + void testIntersects() { var center = new Pose2d(1.0, 2.0, new Rotation2d()); var ellipse = new Ellipse2d(center, 2.0, 1.0); @@ -43,7 +43,7 @@ void testIntersectsPoint() { } @Test - void testContainsPoint() { + void testContains() { var center = new Pose2d(-1.0, -2.0, Rotation2d.fromDegrees(45.0)); var ellipse = new Ellipse2d(center, 2.0, 1.0); @@ -55,7 +55,7 @@ void testContainsPoint() { } @Test - void testDistanceToPoint() { + void testDistance() { var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0)); var ellipse = new Ellipse2d(center, 1.0, 2.0); @@ -73,30 +73,30 @@ void testDistanceToPoint() { } @Test - void testFindNearestPoint() { + void testNearest() { var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0)); var ellipse = new Ellipse2d(center, 1.0, 2.0); var point1 = new Translation2d(2.5, 2.0); - var nearestPoint1 = ellipse.findNearestPoint(point1); + var nearestPoint1 = ellipse.nearest(point1); assertAll( () -> assertEquals(2.5, nearestPoint1.getX(), kEpsilon), () -> assertEquals(2.0, nearestPoint1.getY(), kEpsilon)); var point2 = new Translation2d(1.0, 2.0); - var nearestPoint2 = ellipse.findNearestPoint(point2); + var nearestPoint2 = ellipse.nearest(point2); assertAll( () -> assertEquals(1.0, nearestPoint2.getX(), kEpsilon), () -> assertEquals(2.0, nearestPoint2.getY(), kEpsilon)); var point3 = new Translation2d(1.0, 1.0); - var nearestPoint3 = ellipse.findNearestPoint(point3); + var nearestPoint3 = ellipse.nearest(point3); assertAll( () -> assertEquals(1.0, nearestPoint3.getX(), kEpsilon), () -> assertEquals(1.0, nearestPoint3.getY(), kEpsilon)); var point4 = new Translation2d(-1.0, 2.5); - var nearestPoint4 = ellipse.findNearestPoint(point4); + var nearestPoint4 = ellipse.nearest(point4); assertAll( () -> assertEquals(-0.8512799937611617, nearestPoint4.getX(), kEpsilon), () -> assertEquals(2.378405333174535, nearestPoint4.getY(), kEpsilon)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 0844e1321e9..81f81509929 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -86,6 +86,14 @@ void testInequality() { assertNotEquals(one, two); } + @Test + void testToMatrix() { + var before = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(20.0)); + var after = new Pose2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testMinus() { var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index b333d24592f..00c63eae080 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -169,6 +169,22 @@ void testMinus() { () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon)); } + @Test + void testToMatrix() { + var before = + new Pose3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var after = new Pose3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testToPose2d() { var pose = diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 7ee86d6d7db..6705d9aecd7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -30,7 +30,7 @@ void testNewWithCorners() { } @Test - void testIntersectsPoint() { + void testIntersects() { var center = new Pose2d(4.0, 3.0, Rotation2d.fromDegrees(90.0)); var rect = new Rectangle2d(center, 2.0, 3.0); @@ -42,7 +42,7 @@ void testIntersectsPoint() { } @Test - void testContainsPoint() { + void testContains() { var center = new Pose2d(2.0, 3.0, Rotation2d.fromDegrees(45.0)); var rect = new Rectangle2d(center, 3.0, 1.0); @@ -53,7 +53,7 @@ void testContainsPoint() { } @Test - void testDistanceToPoint() { + void testDistance() { var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0)); var rect = new Rectangle2d(center, 1.0, 2.0); @@ -71,18 +71,18 @@ void testDistanceToPoint() { } @Test - void testFindNearestPoint() { + void testNearest() { var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0)); var rect = new Rectangle2d(center, 3.0, 4.0); var point1 = new Translation2d(1.0, 3.0); - var nearestPoint1 = rect.findNearestPoint(point1); + var nearestPoint1 = rect.nearest(point1); assertAll( () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon)); var point2 = new Translation2d(0.0, 0.0); - var nearestPoint2 = rect.findNearestPoint(point2); + var nearestPoint2 = rect.nearest(point2); assertAll( () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index b75182edb7e..6eaa2a3f98c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -100,6 +100,14 @@ void testInequality() { assertNotEquals(rot1, rot2); } + @Test + void testToMatrix() { + var before = Rotation2d.fromDegrees(20.0); + var after = new Rotation2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInterpolate() { // 50 + (70 - 50) * 0.5 = 60 diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java index 43b7e9b4c5b..3e96a1cb3df 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -344,6 +344,18 @@ void testInequality() { assertNotEquals(rot1, rot2); } + @Test + void testToMatrix() { + var before = + new Rotation3d( + Units.degreesToRadians(10.0), + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0)); + var after = new Rotation3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInterpolate() { final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java index 5817f4e2825..19d4b4f396f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java @@ -22,6 +22,14 @@ void testNewWithMeasures() { assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon); } + @Test + void testToMatrix() { + var before = new Transform2d(1.0, 2.0, Rotation2d.fromDegrees(20.0)); + var after = new Transform2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java index 51d5f071fc3..dc251512fd1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java @@ -11,6 +11,22 @@ import org.junit.jupiter.api.Test; class Transform3dTest { + @Test + void testToMatrix() { + var before = + new Transform3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var after = new Transform3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInverse() { var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 5ed2e7f22a1..8f7bb6fd123 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -20,11 +20,10 @@ class ChassisSpeedsTest { @Test void testDiscretize() { final var target = new ChassisSpeeds(1.0, 0.0, 0.5); - final var speeds = new ChassisSpeeds(1.0, 0.0, 0.5); final var duration = 1.0; final var dt = 0.01; - speeds.discretize(duration); + final var speeds = ChassisSpeeds.discretize(target, duration); final var twist = new Twist2d( speeds.vxMetersPerSecond * dt, @@ -62,8 +61,8 @@ void testMeasureConstructor() { @Test void testFromFieldRelativeSpeeds() { - final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5); - chassisSpeeds.toRobotRelativeSpeeds(Rotation2d.kCW_Pi_2); + final var chassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2); assertAll( () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), @@ -73,8 +72,8 @@ void testFromFieldRelativeSpeeds() { @Test void testFromRobotRelativeSpeeds() { - final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5); - chassisSpeeds.toFieldRelativeSpeeds(Rotation2d.fromDegrees(45.0)); + final var chassisSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0)); assertAll( () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon), diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java deleted file mode 100644 index 0aaac653f42..00000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.kinematics.proto; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; -import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages; -import edu.wpi.first.wpilibj.ProtoTestBase; - -@SuppressWarnings("PMD.TestClassWithoutTestCases") -class MecanumDriveMotorVoltagesProtoTest - extends ProtoTestBase { - MecanumDriveMotorVoltagesProtoTest() { - super(new MecanumDriveMotorVoltages(1.2, 3.1, 2.5, -0.1), MecanumDriveMotorVoltages.proto); - } - - @Override - public void checkEquals(MecanumDriveMotorVoltages testData, MecanumDriveMotorVoltages data) { - assertEquals(testData.frontLeftVoltage, data.frontLeftVoltage); - assertEquals(testData.frontRightVoltage, data.frontRightVoltage); - assertEquals(testData.rearLeftVoltage, data.rearLeftVoltage); - assertEquals(testData.rearRightVoltage, data.rearRightVoltage); - } -} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java deleted file mode 100644 index a53fbbadd4d..00000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.kinematics.struct; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; -import edu.wpi.first.wpilibj.StructTestBase; - -@SuppressWarnings("PMD.TestClassWithoutTestCases") -class MecanumDriveMotorVoltagesStructTest extends StructTestBase { - MecanumDriveMotorVoltagesStructTest() { - super(new MecanumDriveMotorVoltages(1.2, 3.1, 2.5, -0.1), MecanumDriveMotorVoltages.struct); - } - - @Override - public void checkEquals(MecanumDriveMotorVoltages testData, MecanumDriveMotorVoltages data) { - assertEquals(testData.frontLeftVoltage, data.frontLeftVoltage); - assertEquals(testData.frontRightVoltage, data.frontRightVoltage); - assertEquals(testData.rearLeftVoltage, data.rearLeftVoltage); - assertEquals(testData.rearRightVoltage, data.rearRightVoltage); - } -} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 1cedcae1de5..52af40e89b2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -4,8 +4,6 @@ package edu.wpi.first.math.trajectory; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -53,39 +51,27 @@ void testDifferentialDriveVoltageConstraint() { assertAll( () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.leftMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.leftMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.leftMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.leftMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.rightMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.rightMetersPerSecond, + wheelSpeeds.rightMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward - .calculate( - MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond), - MetersPerSecond.of( - wheelSpeeds.rightMetersPerSecond + dt * acceleration)) - .in(Volts) + feedforward.calculateWithVelocities( + wheelSpeeds.rightMetersPerSecond, + wheelSpeeds.rightMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java index 80d6abb41cb..2f4ffac2f2a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.trajectory; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; @@ -44,11 +43,9 @@ private static void assertNear( private static ExponentialProfile.State checkDynamics( ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) { var next = profile.calculate(kDt, current, goal); - var currentVelocity = RadiansPerSecond.mutable(current.velocity); - var nextVelocity = RadiansPerSecond.mutable(next.velocity); - var signal = feedforward.calculate(currentVelocity, nextVelocity); + var signal = feedforward.calculateWithVelocities(current.velocity, next.velocity); - assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9); + assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9); return next; } diff --git a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp index c51e50e4b61..1ee79e7ac6e 100644 --- a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp @@ -16,7 +16,7 @@ TEST(Ellipse2dTest, FocalPoints) { EXPECT_EQ(frc::Translation2d(4_m, 2_m), b); } -TEST(Ellipse2dTest, IntersectsPoint) { +TEST(Ellipse2dTest, Intersects) { constexpr frc::Pose2d center{1_m, 2_m, 0_deg}; constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m}; @@ -27,7 +27,7 @@ TEST(Ellipse2dTest, IntersectsPoint) { EXPECT_FALSE(ellipse.Intersects(pointB)); } -TEST(Ellipse2dTest, ContainsPoint) { +TEST(Ellipse2dTest, Contains) { constexpr frc::Pose2d center{-1_m, -2_m, 45_deg}; constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m}; @@ -38,7 +38,7 @@ TEST(Ellipse2dTest, ContainsPoint) { EXPECT_FALSE(ellipse.Contains(pointB)); } -TEST(Ellipse2dTest, DistanceToPoint) { +TEST(Ellipse2dTest, Distance) { constexpr double kEpsilon = 1E-9; constexpr frc::Pose2d center{1_m, 2_m, 270_deg}; @@ -57,29 +57,29 @@ TEST(Ellipse2dTest, DistanceToPoint) { EXPECT_NEAR(0.19210128384806818, ellipse.Distance(point4).value(), kEpsilon); } -TEST(Ellipse2dTest, FindNearestPoint) { +TEST(Ellipse2dTest, Nearest) { constexpr double kEpsilon = 1E-9; constexpr frc::Pose2d center{1_m, 2_m, 270_deg}; constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m}; constexpr frc::Translation2d point1{2.5_m, 2_m}; - auto nearestPoint1 = ellipse.FindNearestPoint(point1); + auto nearestPoint1 = ellipse.Nearest(point1); EXPECT_NEAR(2.5, nearestPoint1.X().value(), kEpsilon); EXPECT_NEAR(2.0, nearestPoint1.Y().value(), kEpsilon); constexpr frc::Translation2d point2{1_m, 2_m}; - auto nearestPoint2 = ellipse.FindNearestPoint(point2); + auto nearestPoint2 = ellipse.Nearest(point2); EXPECT_NEAR(1.0, nearestPoint2.X().value(), kEpsilon); EXPECT_NEAR(2.0, nearestPoint2.Y().value(), kEpsilon); constexpr frc::Translation2d point3{1_m, 1_m}; - auto nearestPoint3 = ellipse.FindNearestPoint(point3); + auto nearestPoint3 = ellipse.Nearest(point3); EXPECT_NEAR(1.0, nearestPoint3.X().value(), kEpsilon); EXPECT_NEAR(1.0, nearestPoint3.Y().value(), kEpsilon); constexpr frc::Translation2d point4{-1_m, 2.5_m}; - auto nearestPoint4 = ellipse.FindNearestPoint(point4); + auto nearestPoint4 = ellipse.Nearest(point4); EXPECT_NEAR(-0.8512799937611617, nearestPoint4.X().value(), kEpsilon); EXPECT_NEAR(2.378405333174535, nearestPoint4.Y().value(), kEpsilon); } diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index bd5a9aa8a28..9a038590b1a 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -141,6 +141,13 @@ TEST(Pose2dTest, Nearest) { .value()); } +TEST(Pose2dTest, ToMatrix) { + Pose2d before{1_m, 2_m, 20_deg}; + Pose2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Pose2dTest, Constexpr) { constexpr Pose2d defaultConstructed; constexpr Pose2d translationRotation{Translation2d{0_m, 1_m}, diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 88acb1fb897..f0168e95435 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -112,6 +112,13 @@ TEST(Pose3dTest, Minus) { EXPECT_NEAR(0.0, transform.Rotation().Z().value(), 1e-9); } +TEST(Pose3dTest, ToMatrix) { + Pose3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}}; + Pose3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Pose3dTest, ToPose2d) { Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}}; Pose2d expected{1_m, 2_m, 40_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp index f4d2d4350e2..90ac72f51ac 100644 --- a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp @@ -18,7 +18,7 @@ TEST(Rectangle2dTest, NewWithCorners) { EXPECT_EQ(4.0, rect.Center().Y().value()); } -TEST(Rectangle2dTest, IntersectsPoint) { +TEST(Rectangle2dTest, Intersects) { constexpr frc::Pose2d center{4_m, 3_m, 90_deg}; constexpr frc::Rectangle2d rect{center, 2_m, 3_m}; @@ -28,7 +28,7 @@ TEST(Rectangle2dTest, IntersectsPoint) { EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 3.5_m})); } -TEST(Rectangle2dTest, ContainsPoint) { +TEST(Rectangle2dTest, Contains) { constexpr frc::Pose2d center{2_m, 3_m, 45_deg}; constexpr frc::Rectangle2d rect{center, 3_m, 1_m}; @@ -37,7 +37,7 @@ TEST(Rectangle2dTest, ContainsPoint) { EXPECT_FALSE(rect.Contains(frc::Translation2d{3_m, 3_m})); } -TEST(Rectangle2dTest, DistanceToPoint) { +TEST(Rectangle2dTest, Distance) { constexpr double kEpsilon = 1E-9; constexpr frc::Pose2d center{1_m, 2_m, 270_deg}; @@ -56,19 +56,19 @@ TEST(Rectangle2dTest, DistanceToPoint) { EXPECT_NEAR(1, rect.Distance(point4).value(), kEpsilon); } -TEST(Rectangle2dTest, FindNearestPoint) { +TEST(Rectangle2dTest, Nearest) { constexpr double kEpsilon = 1E-9; constexpr frc::Pose2d center{1_m, 1_m, 90_deg}; constexpr frc::Rectangle2d rect{center, 3_m, 4_m}; constexpr frc::Translation2d point1{1_m, 3_m}; - auto nearestPoint1 = rect.FindNearestPoint(point1); + auto nearestPoint1 = rect.Nearest(point1); EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon); EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon); constexpr frc::Translation2d point2{0_m, 0_m}; - auto nearestPoint2 = rect.FindNearestPoint(point2); + auto nearestPoint2 = rect.Nearest(point2); EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon); EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 65d3016d18d..00065df514f 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -78,6 +78,13 @@ TEST(Rotation2dTest, Inequality) { EXPECT_NE(rot1, rot2); } +TEST(Rotation2dTest, ToMatrix) { + Rotation2d before{20_deg}; + Rotation2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Rotation2dTest, Constexpr) { constexpr Rotation2d defaultCtor; constexpr Rotation2d radianCtor{5_rad}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 16c033723d1..6088bdd7590 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -307,6 +307,13 @@ TEST(Rotation3dTest, Inequality) { EXPECT_NE(rot1, rot2); } +TEST(Rotation3dTest, ToMatrix) { + Rotation3d before{10_deg, 20_deg, 30_deg}; + Rotation3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Rotation3dTest, Interpolate) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 49c9466201c..5a562405cc6 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -13,6 +13,13 @@ using namespace frc; +TEST(Transform2dTest, ToMatrix) { + Transform2d before{1_m, 2_m, 20_deg}; + Transform2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Transform2dTest, Inverse) { const Pose2d initial{1_m, 2_m, 45_deg}; const Transform2d transform{{5_m, 0_m}, 5_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp index 8a6bbc039f0..9aa7eda331e 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp @@ -13,6 +13,13 @@ using namespace frc; +TEST(Transform3dTest, ToMatrix) { + Transform3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}}; + Transform3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Transform3dTest, Inverse) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; diff --git a/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java b/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java index 084f0220f54..a4bfadf2b36 100644 --- a/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java +++ b/wpinet/src/main/java/edu/wpi/first/net/WPINetJNI.java @@ -80,6 +80,22 @@ public static synchronized void forceLoad() throws IOException { */ public static native void removePortForwarder(int port); + /** + * Create a web server at the given port. Note that local ports less than 1024 won't work as a + * normal user. + * + * @param port local port number + * @param path local path to document root + */ + public static native void startWebServer(int port, String path); + + /** + * Stop web server running at the given port. + * + * @param port local port number + */ + public static native void stopWebServer(int port); + /** * Creates a MulticastServiceAnnouncer. * diff --git a/wpinet/src/main/java/edu/wpi/first/net/WebServer.java b/wpinet/src/main/java/edu/wpi/first/net/WebServer.java new file mode 100644 index 00000000000..e7a71a5e79a --- /dev/null +++ b/wpinet/src/main/java/edu/wpi/first/net/WebServer.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.net; + +/** A web server using the HTTP protocol. */ +public final class WebServer { + private WebServer() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Create a web server at the given port. Note that local ports less than 1024 won't work as a + * normal user. Also, many ports are blocked by the FRC robot radio; check the game manual for + * what is allowed through the radio firewall. + * + * @param port local port number + * @param path local path to document root + */ + public static void start(int port, String path) { + WPINetJNI.startWebServer(port, path); + } + + /** + * Stop web server running at the given port. + * + * @param port local port number + */ + public static void stop(int port) { + WPINetJNI.stopWebServer(port); + } +} diff --git a/wpinet/src/main/native/cpp/HttpUtil.cpp b/wpinet/src/main/native/cpp/HttpUtil.cpp index 92022ee7978..83d9ded426b 100644 --- a/wpinet/src/main/native/cpp/HttpUtil.cpp +++ b/wpinet/src/main/native/cpp/HttpUtil.cpp @@ -81,6 +81,22 @@ std::string_view EscapeURI(std::string_view str, SmallVectorImpl& buf, return {buf.data(), buf.size()}; } +std::string_view EscapeHTML(std::string_view str, SmallVectorImpl& buf) { + buf.clear(); + for (auto i = str.begin(), end = str.end(); i != end; ++i) { + if (*i == '&') { + buf.append({'&', 'a', 'm', 'p', ';'}); + } else if (*i == '<') { + buf.append({'&', 'l', 't', ';'}); + } else if (*i == '>') { + buf.append({'&', 'g', 't', ';'}); + } else { + buf.push_back(*i); + } + } + return {buf.data(), buf.size()}; +} + HttpQueryMap::HttpQueryMap(std::string_view query) { SmallVector queryElems; split(query, queryElems, '&', 100, false); diff --git a/wpinet/src/main/native/cpp/WebServer.cpp b/wpinet/src/main/native/cpp/WebServer.cpp new file mode 100644 index 00000000000..9efd76a1900 --- /dev/null +++ b/wpinet/src/main/native/cpp/WebServer.cpp @@ -0,0 +1,396 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpinet/WebServer.h" + +#ifndef _WIN32 +#include +#endif + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wpinet/EventLoopRunner.h" +#include "wpinet/HttpServerConnection.h" +#include "wpinet/HttpUtil.h" +#include "wpinet/UrlParser.h" +#include "wpinet/raw_uv_ostream.h" +#include "wpinet/uv/GetAddrInfo.h" +#include "wpinet/uv/Stream.h" +#include "wpinet/uv/Tcp.h" +#include "wpinet/uv/Timer.h" + +using namespace wpi; + +namespace { +class MyHttpConnection : public wpi::HttpServerConnection, + public std::enable_shared_from_this { + public: + explicit MyHttpConnection(std::shared_ptr stream, + std::string_view path) + : HttpServerConnection{std::move(stream)}, m_path{path} {} + + protected: + void ProcessRequest() override; + void SendFileResponse(int code, std::string_view codeText, + std::string_view contentType, fs::path filename, + std::string_view extraHeader = {}); + + std::string m_path; +}; +} // namespace + +#ifndef _WIN32 +namespace { +class SendfileReq : public uv::RequestImpl { + public: + SendfileReq(uv_file out, uv_file in, int64_t inOffset, size_t len) + : m_out(out), m_in(in), m_inOffset(inOffset), m_len(len) { + error = [this](uv::Error err) { GetLoop().error(err); }; + } + + uv::Loop& GetLoop() const { + return *static_cast(GetRaw()->loop->data); + } + + int Send(uv::Loop& loop) { + int err = uv_fs_sendfile(loop.GetRaw(), GetRaw(), m_out, m_in, m_inOffset, + m_len, [](uv_fs_t* req) { + auto& h = *static_cast(req->data); + if (req->result < 0) { + h.ReportError(req->result); + h.complete(); + h.Release(); + return; + } + + h.m_inOffset += req->result; + h.m_len -= req->result; + if (h.m_len == 0) { + // done + h.complete(); + h.Release(); // this is always a one-shot + return; + } + + // need to send more + h.Send(h.GetLoop()); + }); + if (err < 0) { + ReportError(err); + complete(); + } + return err; + } + + wpi::sig::Signal<> complete; + + private: + uv_file m_out; + uv_file m_in; + int64_t m_inOffset; + size_t m_len; +}; +} // namespace + +static void Sendfile(uv::Loop& loop, uv_file out, uv_file in, int64_t inOffset, + size_t len, std::function complete) { + auto req = std::make_shared(out, in, inOffset, len); + if (complete) { + req->complete.connect(complete); + } + int err = req->Send(loop); + if (err >= 0) { + req->Keep(); + } +} +#endif + +static std::string_view GetMimeType(std::string_view ext) { + static const wpi::StringMap map{ + {"css", "text/css"}, + {"csv", "text/csv"}, + {"gif", "image/gif"}, + {"htm", "text/html"}, + {"html", "text/html"}, + {"ico", "image/vnd.microsoft.icon"}, + {"jar", "application/java-archive"}, + {"jpeg", "image/jpeg"}, + {"jpg", "image/jpeg"}, + {"js", "text/javascript"}, + {"json", "text/json"}, + {"mjs", "text/javascript"}, + {"pdf", "application/pdf"}, + {"png", "image/png"}, + {"sh", "application/x-sh"}, + {"svg", "image/svg+xml"}, + {"txt", "text/plain"}, + {"webp", "image/webp"}, + {"xhtml", "application/xhtml+xml"}, + {"xml", "application/xml"}, + {"zip", "application/zip"}, + }; + auto it = map.find(ext); + if (it == map.end()) { + return "application/octet-stream"; + } + return it->second; +} + +void MyHttpConnection::SendFileResponse(int code, std::string_view codeText, + std::string_view contentType, + fs::path filename, + std::string_view extraHeader) { +#ifdef _WIN32 + auto membuf = wpi::MemoryBuffer::GetFile(filename.string()); + if (!membuf) { + SendError(404); + return; + } + + wpi::SmallVector toSend; + wpi::raw_uv_ostream os{toSend, 4096}; + BuildHeader(os, code, codeText, contentType, (*membuf)->size(), extraHeader); + SendData(os.bufs(), false); + auto buf = (*membuf)->GetBuffer(); + m_stream.Write( + {{buf}}, [closeAfter = !m_keepAlive, stream = &m_stream, + membuf = std::shared_ptr{std::move(*membuf)}](auto, uv::Error) { + if (closeAfter) { + stream->Close(); + } + }); +#else + // open file + std::error_code ec; + auto infile = fs::OpenFileForRead(filename, ec); + if (ec) { + SendError(404); + return; + } + int infd = fs::FileToFd(infile, ec, fs::OF_None); + if (ec) { + fs::CloseFile(infile); + SendError(404); + return; + } + + // get file size + auto size = fs::file_size(filename, ec); + if (ec) { + SendError(404); + ::close(infd); + return; + } + + uv_os_fd_t outfd; + int err = uv_fileno(m_stream.GetRawHandle(), &outfd); + if (err < 0) { + m_stream.GetLoopRef().ReportError(err); + SendError(404); + ::close(infd); + return; + } + + wpi::SmallVector toSend; + wpi::raw_uv_ostream os{toSend, 4096}; + BuildHeader(os, code, codeText, contentType, size, extraHeader); + SendData(os.bufs(), false); + + // close after write completes if we aren't keeping alive + // since we're using sendfile, set socket to blocking + m_stream.SetBlocking(true); + Sendfile(m_stream.GetLoopRef(), outfd, infd, 0, size, + [infd, closeAfter = !m_keepAlive, stream = &m_stream] { + ::close(infd); + if (closeAfter) { + stream->Close(); + } else { + stream->SetBlocking(false); + } + }); +#endif +} + +void MyHttpConnection::ProcessRequest() { + // fmt::print(stderr, "HTTP request: '{}'\n", m_request.GetUrl()); + wpi::UrlParser url{m_request.GetUrl(), + m_request.GetMethod() == wpi::HTTP_CONNECT}; + if (!url.IsValid()) { + // failed to parse URL + SendError(400); + return; + } + + std::string_view path; + if (url.HasPath()) { + path = url.GetPath(); + } + // fmt::print(stderr, "path: \"{}\"\n", path); + + wpi::SmallString<128> pathBuf; + bool error; + path = UnescapeURI(path, pathBuf, &error); + if (error) { + SendError(400); + return; + } + + std::string_view query; + if (url.HasQuery()) { + query = url.GetQuery(); + } + // fmt::print(stderr, "query: \"{}\"\n", query); + HttpQueryMap qmap{query}; + + const bool isGET = m_request.GetMethod() == wpi::HTTP_GET; + if (isGET && wpi::starts_with(path, '/') && !wpi::contains(path, "..")) { + fs::path fullpath = fmt::format("{}{}", m_path, path); + std::error_code ec; + bool isdir = fs::is_directory(fullpath, ec); + if (isdir) { + if (!wpi::ends_with(path, '/')) { + // redirect to trailing / location + SendResponse(301, "Moved Permanently", "text/plain", "", + fmt::format("Location: {}/\r\n\r\n", path)); + return; + } + // generate directory listing + wpi::SmallString<64> formatBuf; + if (qmap.Get("format", formatBuf).value_or("") == "json") { + wpi::json dirs = wpi::json::array(); + wpi::json files = wpi::json::array(); + for (auto&& entry : fs::directory_iterator{fullpath}) { + bool subdir = entry.is_directory(ec); + std::string name = entry.path().filename().string(); + if (subdir) { + dirs.emplace_back(wpi::json{{"name", std::move(name)}}); + } else { + files.emplace_back( + wpi::json{{"name", std::move(name)}, + {"size", subdir ? 0 : entry.file_size(ec)}}); + } + } + SendResponse( + 200, "OK", "text/json", + wpi::json{{"dirs", std::move(dirs)}, {"files", std::move(files)}} + .dump()); + } else { + wpi::StringMap dirs; + wpi::StringMap files; + for (auto&& entry : fs::directory_iterator{fullpath}) { + bool subdir = entry.is_directory(ec); + std::string name = entry.path().filename().string(); + wpi::SmallString<128> nameUriBuf, nameHtmlBuf; + if (subdir) { + dirs.emplace( + name, fmt::format( + "{}/", + EscapeURI(name, nameUriBuf), + EscapeHTML(name, nameHtmlBuf))); + } else { + files.emplace( + name, fmt::format( + "{}{}", + EscapeURI(name, nameUriBuf), + EscapeHTML(name, nameHtmlBuf), entry.file_size(ec))); + } + } + + std::string html = fmt::format( + "{}" + "\n", + path); + for (auto&& str : dirs) { + html += str.second; + } + for (auto&& str : files) { + html += str.second; + } + html += "
NameSize
"; + SendResponse(200, "OK", "text/html", html); + } + } else { + wpi::SmallString<128> extraHeadersBuf; + wpi::raw_svector_ostream os{extraHeadersBuf}; + os << "Content-Disposition: filename=\""; + os.write_escaped(fullpath.filename().string()); + os << "\"\r\n"; + SendFileResponse(200, "OK", GetMimeType(wpi::rsplit(path, '.').second), + fullpath, os.str()); + } + } else { + SendError(404, "Resource not found"); + } +} + +struct WebServer::Impl { + public: + EventLoopRunner runner; + DenseMap> servers; +}; + +WebServer::WebServer() : m_impl{new Impl} {} + +WebServer& WebServer::GetInstance() { + static WebServer instance; + return instance; +} + +void WebServer::Start(unsigned int port, std::string_view path) { + m_impl->runner.ExecSync([&](uv::Loop& loop) { + auto server = uv::Tcp::Create(loop); + if (!server) { + wpi::print(stderr, "WebServer: Creating server failed\n"); + return; + } + + // bind to local port + server->Bind("", port); + + // when we get a connection, accept it + server->connection.connect( + [serverPtr = server.get(), path = std::string{path}] { + auto client = serverPtr->Accept(); + if (!client) { + wpi::print(stderr, "WebServer: Connecting to client failed\n"); + return; + } + + // close on error + client->error.connect([clientPtr = client.get()](uv::Error err) { + clientPtr->Close(); + }); + + auto conn = std::make_shared(client, path); + client->SetData(conn); + }); + + // start listening for incoming connections + server->Listen(); + + m_impl->servers[port] = server; + }); +} + +void WebServer::Stop(unsigned int port) { + m_impl->runner.ExecSync([&](uv::Loop& loop) { + if (auto server = m_impl->servers.lookup(port).lock()) { + server->Close(); + m_impl->servers.erase(port); + } + }); +} diff --git a/wpinet/src/main/native/cpp/jni/WPINetJNI.cpp b/wpinet/src/main/native/cpp/jni/WPINetJNI.cpp index fb59c4d9598..04efdab2044 100644 --- a/wpinet/src/main/native/cpp/jni/WPINetJNI.cpp +++ b/wpinet/src/main/native/cpp/jni/WPINetJNI.cpp @@ -16,6 +16,7 @@ #include "wpinet/MulticastServiceAnnouncer.h" #include "wpinet/MulticastServiceResolver.h" #include "wpinet/PortForwarder.h" +#include "wpinet/WebServer.h" using namespace wpi::java; @@ -80,6 +81,31 @@ Java_edu_wpi_first_net_WPINetJNI_removePortForwarder wpi::PortForwarder::GetInstance().Remove(port); } +/* + * Class: edu_wpi_first_net_WPINetJNI + * Method: startWebServer + * Signature: (ILjava/lang/String;)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_net_WPINetJNI_startWebServer + (JNIEnv* env, jclass, jint port, jstring path) +{ + wpi::WebServer::GetInstance().Start(static_cast(port), + JStringRef{env, path}.str()); +} + +/* + * Class: edu_wpi_first_net_WPINetJNI + * Method: stopWebServer + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_net_WPINetJNI_stopWebServer + (JNIEnv* env, jclass, jint port) +{ + wpi::WebServer::GetInstance().Stop(port); +} + /* * Class: edu_wpi_first_net_WPINetJNI * Method: createMulticastServiceAnnouncer diff --git a/wpinet/src/main/native/include/wpinet/HttpUtil.h b/wpinet/src/main/native/include/wpinet/HttpUtil.h index 1509a6f1a03..dca5225809e 100644 --- a/wpinet/src/main/native/include/wpinet/HttpUtil.h +++ b/wpinet/src/main/native/include/wpinet/HttpUtil.h @@ -39,6 +39,11 @@ std::string_view UnescapeURI(std::string_view str, SmallVectorImpl& buf, std::string_view EscapeURI(std::string_view str, SmallVectorImpl& buf, bool spacePlus = true); +// Escape a string for HTML output. +// @param buf Buffer for output +// @return Escaped string +std::string_view EscapeHTML(std::string_view str, SmallVectorImpl& buf); + // Parse a set of HTTP headers. Saves just the Content-Type and Content-Length // fields. // @param is Input stream diff --git a/wpinet/src/main/native/include/wpinet/WebServer.h b/wpinet/src/main/native/include/wpinet/WebServer.h new file mode 100644 index 00000000000..ebfb9a36b2b --- /dev/null +++ b/wpinet/src/main/native/include/wpinet/WebServer.h @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#ifndef WPINET_WEBSERVER_H_ +#define WPINET_WEBSERVER_H_ + +#pragma once + +#include +#include + +namespace wpi { + +/** + * A web server using the HTTP protocol. + */ +class WebServer { + public: + WebServer(const WebServer&) = delete; + WebServer& operator=(const WebServer&) = delete; + + /** + * Get an instance of the WebServer class. + * + * This is a singleton to guarantee that there is only a single instance + * regardless of how many times GetInstance is called. + */ + static WebServer& GetInstance(); + + /** + * Create a web server at the given port. + * Note that local ports less than 1024 won't work as a normal user. Also, + * many ports are blocked by the FRC robot radio; check the game manual for + * what is allowed through the radio firewall. + * + * @param port local port number + * @param path local path to document root + */ + void Start(unsigned int port, std::string_view path); + + /** + * Stop web server running at the given port. + * + * @param port local port number + */ + void Stop(unsigned int port); + + private: + WebServer(); + + struct Impl; + std::unique_ptr m_impl; +}; + +} // namespace wpi + +#endif // WPINET_WEBSERVER_H_ diff --git a/wpiunits/src/main/java/edu/wpi/first/units/MomentOfInertiaUnit.java b/wpiunits/src/main/java/edu/wpi/first/units/MomentOfInertiaUnit.java index 74362d9a9f1..ab92427001c 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/MomentOfInertiaUnit.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/MomentOfInertiaUnit.java @@ -28,7 +28,7 @@ public final class MomentOfInertiaUnit extends PerUnit baseUnit, + MomentOfInertiaUnit baseUnit, UnaryFunction toBaseConverter, UnaryFunction fromBaseConverter, String name, diff --git a/wpiutil/BUILD.bazel b/wpiutil/BUILD.bazel index 6e9318bedff..928fa4e8f3d 100644 --- a/wpiutil/BUILD.bazel +++ b/wpiutil/BUILD.bazel @@ -197,9 +197,9 @@ cc_library( ":llvm-srcs", ":memory-srcs", ":mpack-srcs", + ":nanopb-srcs", ":native-srcs", ":protobuf-srcs", - ":nanopb-srcs", ], hdrs = glob(["src/main/native/include/**/*"]), includes = ["src/main/native/include"], @@ -215,8 +215,8 @@ cc_library( ":llvm-headers", ":memory-headers", ":mpack-headers", - ":protobuf-headers", ":nanopb-headers", + ":protobuf-headers", ":sigslot-headers", ] + select({ "@rules_bzlmodrio_toolchains//constraints/is_roborio:roborio": ["@bzlmodrio-ni//libraries/cpp/ni:shared"], diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index 5191e17e56e..9846a49cc70 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -184,6 +184,12 @@ nativeUtils.exportsConfigs { } } +nativeUtils.platformConfigs.each { + if (it.name.contains('windows')) { + it.cppCompiler.args.add("/D_DISABLE_CONSTEXPR_MUTEX_CONSTRUCTOR") + } +} + cppHeadersZip { def thirdpartyIncDirs = [ 'src/main/native/thirdparty/argparse/include', diff --git a/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java b/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java index 60fb5d13d09..9257d9706f7 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java @@ -48,7 +48,7 @@ public void remove(int handle) { public void wakeup() { m_lock.lock(); try { - for (Integer eventHandle : m_events) { + for (int eventHandle : m_events) { WPIUtilJNI.setEvent(eventHandle); } } finally { diff --git a/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java b/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java index 452cacfe72a..5ceaa076ad2 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java @@ -16,17 +16,20 @@ public final class RuntimeLoader { * @return A load error message. */ private static String getLoadErrorMessage(String libraryName, UnsatisfiedLinkError ule) { + String jvmLocation = ProcessHandle.current().info().command().orElse("Unknown"); StringBuilder msg = new StringBuilder(512); msg.append(libraryName) .append(" could not be loaded from path.\n" + "\tattempted to load for platform ") .append(CombinedRuntimeLoader.getPlatformPath()) .append("\nLast Load Error: \n") .append(ule.getMessage()) - .append('\n'); + .append('\n') + .append(String.format("JVM Location: %s\n", jvmLocation)); if (System.getProperty("os.name").startsWith("Windows")) { msg.append( - "A common cause of this error is missing the C++ runtime.\n" - + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n"); + "A common cause of this error is using a JVM with an incorrect MSVC runtime.\n" + + "Ensure you are using the WPILib JVM (The current running JVM is listed above)\n" + + "See https://wpilib.org/jvmruntime for more information\n"); } return msg.toString(); } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFetcher.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFetcher.java new file mode 100644 index 00000000000..dbfef7cc08f --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFetcher.java @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import java.util.Optional; + +/** + * A utility class for fetching the assigned struct of existing classes. These are usually public, + * static, and final properties with the Struct type. + */ +public final class StructFetcher { + private StructFetcher() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the + * non-contractual nature of the marker this can fail. If the {@code struct} field could not be + * accessed for any reason, an empty {@link Optional} is returned. + * + * @param The type of the class. + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> fetchStruct( + Class clazz) { + try { + var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct")); + return possibleField.flatMap( + field -> { + if (Struct.class.isAssignableFrom(field.getType())) { + try { + return Optional.ofNullable((Struct) field.get(null)); + } catch (IllegalAccessException e) { + return Optional.empty(); + } + } else { + return Optional.empty(); + } + }); + } catch (NoSuchFieldException e) { + return Optional.empty(); + } + } + + /** + * Returns a {@link Struct} for the given class. This does not do compile time checking that the + * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link + * #fetchStruct(Class)}. + * + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> fetchStructDynamic(Class clazz) { + if (StructSerializable.class.isAssignableFrom(clazz)) { + return fetchStruct((Class) clazz).map(struct -> struct); + } else { + return Optional.empty(); + } + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/ProceduralStructGenerator.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructGenerator.java similarity index 88% rename from wpiutil/src/main/java/edu/wpi/first/util/struct/ProceduralStructGenerator.java rename to wpiutil/src/main/java/edu/wpi/first/util/struct/StructGenerator.java index 213a16289f0..24b2541e1c2 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/struct/ProceduralStructGenerator.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructGenerator.java @@ -12,11 +12,10 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.List; -import java.util.Optional; /** A utility class for procedurally generating {@link Struct}s from records and enums. */ -public final class ProceduralStructGenerator { - private ProceduralStructGenerator() { +public final class StructGenerator { + private StructGenerator() { throw new UnsupportedOperationException("This is a utility class!"); } @@ -123,54 +122,6 @@ public static void addCustomStruct(Class clazz, Struct struct, boolean } } - /** - * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the - * non-contractual nature of the marker this can fail. If the {@code struct} field could not be - * accessed for any reason, an empty {@link Optional} is returned. - * - * @param The type of the class. - * @param clazz The class object to extract the struct from. - * @return An optional containing the struct if it could be extracted. - */ - @SuppressWarnings("unchecked") - public static Optional> extractClassStruct( - Class clazz) { - try { - var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct")); - return possibleField.flatMap( - field -> { - if (Struct.class.isAssignableFrom(field.getType())) { - try { - return Optional.ofNullable((Struct) field.get(null)); - } catch (IllegalAccessException e) { - return Optional.empty(); - } - } else { - return Optional.empty(); - } - }); - } catch (NoSuchFieldException e) { - return Optional.empty(); - } - } - - /** - * Returns a {@link Struct} for the given class. This does not do compile time checking that the - * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link - * #extractClassStruct(Class)}. - * - * @param clazz The class object to extract the struct from. - * @return An optional containing the struct if it could be extracted. - */ - @SuppressWarnings("unchecked") - public static Optional> extractClassStructDynamic(Class clazz) { - if (StructSerializable.class.isAssignableFrom(clazz)) { - return extractClassStruct((Class) clazz).map(struct -> struct); - } else { - return Optional.empty(); - } - } - /** A utility for building schema syntax in a procedural manner. */ @SuppressWarnings("PMD.AvoidStringBufferField") public static class SchemaBuilder { @@ -303,7 +254,7 @@ public boolean isImmutable() { * @return The generated struct. */ @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) - static Struct genRecord(final Class recordClass) { + public static Struct genRecord(final Class recordClass) { final RecordComponent[] components = recordClass.getRecordComponents(); final SchemaBuilder schemaBuilder = new SchemaBuilder(); final ArrayList> nestedStructs = new ArrayList<>(); @@ -329,7 +280,7 @@ static Struct genRecord(final Class recordClass) { if (customStructTypeMap.containsKey(type)) { struct = customStructTypeMap.get(type); } else if (StructSerializable.class.isAssignableFrom(type)) { - var optStruct = extractClassStructDynamic(type); + var optStruct = StructFetcher.fetchStructDynamic(type); if (optStruct.isPresent()) { struct = optStruct.get(); } else { @@ -465,7 +416,7 @@ public boolean isImmutable() { * @return The generated struct. */ @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) - static > Struct genEnum(Class enumClass) { + public static > Struct genEnum(Class enumClass) { final E[] enumVariants = enumClass.getEnumConstants(); final Field[] allEnumFields = enumClass.getDeclaredFields(); final SchemaBuilder schemaBuilder = new SchemaBuilder(); @@ -516,7 +467,7 @@ static > Struct genEnum(Class enumClass) { if (customStructTypeMap.containsKey(type)) { struct = customStructTypeMap.get(type); } else if (StructSerializable.class.isAssignableFrom(type)) { - var optStruct = extractClassStructDynamic(type); + var optStruct = StructFetcher.fetchStructDynamic(type); if (optStruct.isPresent()) { struct = optStruct.get(); } else { diff --git a/wpiutil/src/test/java/edu/wpi/first/util/struct/ProceduralStructGeneratorTest.java b/wpiutil/src/test/java/edu/wpi/first/util/struct/StructGeneratorTest.java similarity index 90% rename from wpiutil/src/test/java/edu/wpi/first/util/struct/ProceduralStructGeneratorTest.java rename to wpiutil/src/test/java/edu/wpi/first/util/struct/StructGeneratorTest.java index ccdbca30564..c5a1eea25be 100644 --- a/wpiutil/src/test/java/edu/wpi/first/util/struct/ProceduralStructGeneratorTest.java +++ b/wpiutil/src/test/java/edu/wpi/first/util/struct/StructGeneratorTest.java @@ -4,15 +4,15 @@ package edu.wpi.first.util.struct; -import static edu.wpi.first.util.struct.ProceduralStructGenerator.genEnum; -import static edu.wpi.first.util.struct.ProceduralStructGenerator.genRecord; +import static edu.wpi.first.util.struct.StructGenerator.genEnum; +import static edu.wpi.first.util.struct.StructGenerator.genRecord; import static org.junit.jupiter.api.Assertions.assertEquals; import java.nio.ByteBuffer; import java.nio.ByteOrder; import org.junit.jupiter.api.Test; -class ProceduralStructGeneratorTest { +class StructGeneratorTest { public record CustomRecord(int int32, boolean bool, double float64, char character, short int16) implements StructSerializable { public static CustomRecord create() { @@ -95,8 +95,7 @@ public final int hashCode() { @SuppressWarnings("unchecked") private void testStructRoundTrip(S value) { - Struct struct = - ProceduralStructGenerator.extractClassStruct((Class) value.getClass()).get(); + Struct struct = StructFetcher.fetchStruct((Class) value.getClass()).get(); ByteBuffer buffer = ByteBuffer.allocate(struct.getSize()); buffer.order(ByteOrder.LITTLE_ENDIAN); struct.pack(buffer, value); @@ -108,8 +107,7 @@ private void testStructRoundTrip(S value) { @SuppressWarnings("unchecked") private void testStructDoublePack(S value) { - Struct struct = - ProceduralStructGenerator.extractClassStruct((Class) value.getClass()).get(); + Struct struct = StructFetcher.fetchStruct((Class) value.getClass()).get(); ByteBuffer buffer = ByteBuffer.allocate(struct.getSize()); buffer.order(ByteOrder.LITTLE_ENDIAN); struct.pack(buffer, value); @@ -123,8 +121,7 @@ private void testStructDoublePack(S value) { @SuppressWarnings("unchecked") private void testStructDoubleUnpack(S value) { - Struct struct = - ProceduralStructGenerator.extractClassStruct((Class) value.getClass()).get(); + Struct struct = StructFetcher.fetchStruct((Class) value.getClass()).get(); ByteBuffer buffer = ByteBuffer.allocate(struct.getSize()); buffer.order(ByteOrder.LITTLE_ENDIAN); struct.pack(buffer, value); diff --git a/xrpVendordep/BUILD.bazel b/xrpVendordep/BUILD.bazel new file mode 100644 index 00000000000..964ea19d70f --- /dev/null +++ b/xrpVendordep/BUILD.bazel @@ -0,0 +1,54 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") +load("@rules_java//java:defs.bzl", "java_binary", "java_library") + +cc_library( + name = "xrp-cpp", + srcs = glob([ + "src/main/native/cpp/**", + ]), + hdrs = glob(["src/main/native/include/**"]), + strip_include_prefix = "src/main/native/include", + visibility = ["//visibility:public"], + deps = [ + "//wpilibc:wpilibc.static", + ], +) + +java_library( + name = "xrp-java", + srcs = glob(["src/main/java/**/*.java"]), + visibility = ["//visibility:public"], + deps = [ + "//hal:hal-java", + "//wpilibj", + "//wpimath:wpimath-java", + ], +) + +cc_test( + name = "xrp-cpp-test", + size = "small", + srcs = glob(["src/test/native/cpp/**"]), + deps = [ + "//thirdparty/googletest:googletest.static", + ], +) + +cc_binary( + name = "DevMain-Cpp", + srcs = ["src/dev/native/cpp/main.cpp"], + deps = [ + "//wpiutil:wpiutil.static", + ], +) + +java_binary( + name = "DevMain-Java", + srcs = ["src/dev/java/edu/wpi/first/wpilibj/xrp/DevMain.java"], + main_class = "edu.wpi.first.wpilibj.xrp.DevMain", + deps = [ + "//hal:hal-java", + "//ntcore:networktables-java", + "//wpiutil:wpiutil-java", + ], +)