diff --git a/.github/workflows/cmake-android.yml b/.github/workflows/cmake-android.yml index 90ec646e48..dbaf7c5fc9 100644 --- a/.github/workflows/cmake-android.yml +++ b/.github/workflows/cmake-android.yml @@ -30,7 +30,7 @@ jobs: - uses: nttld/setup-ndk@v1 id: setup-ndk with: - ndk-version: r27c + ndk-version: r27d add-to-path: false - uses: actions/setup-java@v5 diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 97bb28ec00..ba8b64ec7c 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -56,8 +56,7 @@ jobs: run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV if: startsWith(github.ref, 'refs/tags/v2027') - name: Build with Gradle - # main on 2021-05-30, to include addnab/docker-run-action/pull/17 - uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609 + uses: wpilibsuite/docker-run-action@v4 with: image: ${{ matrix.container }} options: -v ${{ github.workspace }}:/work -w /work -e ARTIFACTORY_PUBLISH_USERNAME -e ARTIFACTORY_PUBLISH_PASSWORD -e GITHUB_REF -e CI diff --git a/.github/workflows/sentinel-build.yml b/.github/workflows/sentinel-build.yml index dd6c3d359b..1bf91a1792 100644 --- a/.github/workflows/sentinel-build.yml +++ b/.github/workflows/sentinel-build.yml @@ -50,8 +50,7 @@ jobs: with: fetch-depth: 0 - name: Build with Gradle - # main on 2021-05-30, to include addnab/docker-run-action/pull/17 - uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609 + uses: wpilibsuite/docker-run-action@v4 with: image: ${{ matrix.container }} options: -v ${{ github.workspace }}:/work -w /work -e GITHUB_REF -e CI diff --git a/CMakeLists.txt b/CMakeLists.txt index d2408b0c36..2943dc85ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -349,7 +349,9 @@ if(WITH_WPILIB) set(APRILTAG_DEP_REPLACE "find_dependency(apriltag)") set(COMMANDSV3_DEP_REPLACE "find_dependency(commandsv3)") set(WPILIBC_DEP_REPLACE "find_dependency(wpilibc)") - set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)") + if(WITH_JAVA) + set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)") + endif() set(COMMAND_DEP_REPLACE "find_dependency(commandsv2)") add_subdirectory(apriltag) add_subdirectory(wpilibj) diff --git a/LICENSE.md b/LICENSE.md index d744196fe9..eb3061b0d8 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2025 FIRST and other WPILib contributors +Copyright (c) 2009-2026 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java index d2fb0199cc..7b3641029c 100644 --- a/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java @@ -17,13 +17,17 @@ public enum AprilTagFields { /** 2025 Reefscape Welded (see TU 12). */ k2025ReefscapeWelded("2025-reefscape-welded.json"), /** 2025 Reefscape AndyMark (see TU 12). */ - k2025ReefscapeAndyMark("2025-reefscape-andymark.json"); + k2025ReefscapeAndyMark("2025-reefscape-andymark.json"), + /** 2026 Rebuilt Welded. */ + k2026RebuiltWelded("2026-rebuilt-welded.json"), + /** 2026 Rebuilt AndyMark. */ + k2026RebuiltAndymark("2026-rebuilt-andymark.json"); /** Base resource directory. */ public static final String kBaseResourceDir = "/org/wpilib/vision/apriltag/"; /** Alias to the current game. */ - public static final AprilTagFields kDefaultField = k2025ReefscapeWelded; + public static final AprilTagFields kDefaultField = k2026RebuiltWelded; /** Resource filename. */ public final String m_resourceFile; diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index 255321198a..b7830edcf8 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -141,6 +141,8 @@ std::string_view GetResource_2023_chargedup_json(); std::string_view GetResource_2024_crescendo_json(); std::string_view GetResource_2025_reefscape_welded_json(); std::string_view GetResource_2025_reefscape_andymark_json(); +std::string_view GetResource_2026_rebuilt_welded_json(); +std::string_view GetResource_2026_rebuilt_andymark_json(); } // namespace wpi::apriltag @@ -162,6 +164,12 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) { case AprilTagField::k2025ReefscapeAndyMark: fieldString = GetResource_2025_reefscape_andymark_json(); break; + case AprilTagField::k2026RebuiltWelded: + fieldString = GetResource_2026_rebuilt_welded_json(); + break; + case AprilTagField::k2026RebuiltAndyMark: + fieldString = GetResource_2026_rebuilt_andymark_json(); + break; case AprilTagField::kNumFields: throw std::invalid_argument("Invalid Field"); } diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp index 4d73d41de0..2647130cef 100644 --- a/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp @@ -24,8 +24,12 @@ enum class AprilTagField { k2025ReefscapeAndyMark, /// 2025 Reefscape Welded (see TU12). k2025ReefscapeWelded, + /// 2026 Rebuilt Andymark. + k2026RebuiltAndyMark, + /// 2026 Rebuilt Welded. + k2026RebuiltWelded, /// Alias to the current game. - kDefaultField = k2025ReefscapeWelded, + kDefaultField = k2026RebuiltWelded, // This is a placeholder for denoting the last supported field. This should // always be the last entry in the enum and should not be used by users diff --git a/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.csv b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.csv new file mode 100644 index 0000000000..da80d5fa95 --- /dev/null +++ b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.csv @@ -0,0 +1,33 @@ +ID,X,Y,Z,Z-Rotation,X-Rotation +1,467.085,291.791,35,180,0 +2,468.559,182.077,44.25,90,0 +3,444.797,172.321,44.25,180,0 +4,444.797,158.321,44.25,180,0 +5,468.559,134.565,44.25,270,0 +6,467.085,24.851,35,180,0 +7,470.034,24.851,35,0,0 +8,482.559,134.565,44.25,270,0 +9,492.329,144.321,44.25,0,0 +10,492.329,158.321,44.25,0,0 +11,482.559,182.077,44.25,90,0 +12,470.034,291.791,35,0,0 +13,649.58,291.02,21.75,180,0 +14,649.58,274.02,21.75,180,0 +15,649.566,169.783,21.75,180,0 +16,649.566,152.783,21.75,180,0 +17,183.034,24.851,35,0,0 +18,181.559,134.565,44.25,270,0 +19,205.321,144.321,44.25,0,0 +20,205.321,158.321,44.25,0,0 +21,181.559,182.077,44.25,90,0 +22,183.034,291.791,35,0,0 +23,180.085,291.791,35,180,0 +24,167.559,182.077,44.25,90,0 +25,157.79,172.321,44.25,180,0 +26,157.79,158.321,44.25,180,0 +27,167.559,134.565,44.25,270,0 +28,180.085,24.851,35,180,0 +29,0.539,25.621,21.75,0,0 +30,0.539,42.621,21.75,0,0 +31,0.553,146.858,21.75,0,0 +32,0.553,163.858,21.75,0,0 diff --git a/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.json new file mode 100644 index 0000000000..ecc03907da --- /dev/null +++ b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-andymark.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.csv b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.csv new file mode 100644 index 0000000000..39f20f6bae --- /dev/null +++ b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.csv @@ -0,0 +1,33 @@ +ID,X,Y,Z,Z-Rotation,X-Rotation +1,467.637,292.314,35,180,0 +2,469.111,182.6,44.25,90,0 +3,445.349,172.844,44.25,180,0 +4,445.349,158.844,44.25,180,0 +5,469.111,135.088,44.25,270,0 +6,467.637,25.374,35,180,0 +7,470.586,25.374,35,0,0 +8,483.111,135.088,44.25,270,0 +9,492.881,144.844,44.25,0,0 +10,492.881,158.844,44.25,0,0 +11,483.111,182.6,44.25,90,0 +12,470.586,292.314,35,0,0 +13,650.918,291.469,21.75,180,0 +14,650.918,274.469,21.75,180,0 +15,650.904,170.219,21.75,180,0 +16,650.904,153.219,21.75,180,0 +17,183.586,25.374,35,0,0 +18,182.111,135.088,44.25,270,0 +19,205.873,144.844,44.25,0,0 +20,205.873,158.844,44.25,0,0 +21,182.111,182.6,44.25,90,0 +22,183.586,292.314,35,0,0 +23,180.637,292.314,35,180,0 +24,168.111,182.6,44.25,90,0 +25,158.341,172.844,44.25,180,0 +26,158.341,158.844,44.25,180,0 +27,168.111,135.088,44.25,270,0 +28,180.637,25.374,35,180,0 +29,0.305,26.219,21.75,0,0 +30,0.305,43.219,21.75,0,0 +31,0.318,147.469,21.75,0,0 +32,0.318,164.469,21.75,0,0 diff --git a/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.json new file mode 100644 index 0000000000..8c5a52de87 --- /dev/null +++ b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2026-rebuilt-welded.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.8779798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9154194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9154194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.8779798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9528844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2710194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2710194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9528844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.5333172, + "y": 7.4033126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.5333172, + "y": 6.9715126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.5329616, + "y": 4.3235626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.5329616, + "y": 3.8917626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6630844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6256194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6256194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6630844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.5881798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2700194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2700194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.5881798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 0.6659626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 1.0977626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0080772, + "y": 3.7457125999999996, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0080772, + "y": 4.1775126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.069 + } +} diff --git a/commandsv2/src/main/java/org/wpilib/command2/button/Trigger.java b/commandsv2/src/main/java/org/wpilib/command2/button/Trigger.java index a2e98d41a1..410a90ed61 100644 --- a/commandsv2/src/main/java/org/wpilib/command2/button/Trigger.java +++ b/commandsv2/src/main/java/org/wpilib/command2/button/Trigger.java @@ -11,6 +11,7 @@ import org.wpilib.command2.Command; import org.wpilib.command2.CommandScheduler; import org.wpilib.event.EventLoop; import org.wpilib.math.filter.Debouncer; +import org.wpilib.math.filter.EdgeCounterFilter; /** * This class provides an easy way to link commands to conditions. @@ -287,4 +288,31 @@ public class Trigger implements BooleanSupplier { } }); } + + /** + * Creates a new multi-press trigger from this trigger - it will become active when this trigger + * has been activated the required number of times within the specified time window. + * + *

This is useful for implementing "double-click" style functionality. + * + *

Input for this must be stable, consider using a Debouncer before this to avoid counting + * noise as multiple presses. + * + * @param requiredPresses The number of presses required. + * @param windowTime The number of seconds in which the presses must occur. + * @return The multi-press trigger. + */ + public Trigger multiPress(int requiredPresses, double windowTime) { + return new Trigger( + m_loop, + new BooleanSupplier() { + final EdgeCounterFilter m_edgeCounterFilter = + new EdgeCounterFilter(requiredPresses, windowTime); + + @Override + public boolean getAsBoolean() { + return m_edgeCounterFilter.calculate(m_condition.getAsBoolean()); + } + }); + } } diff --git a/commandsv2/src/main/native/cpp/frc2/command/button/Trigger.cpp b/commandsv2/src/main/native/cpp/frc2/command/button/Trigger.cpp index 8382d40f75..cb36990849 100644 --- a/commandsv2/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/commandsv2/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -8,6 +8,7 @@ #include "wpi/commands2/CommandPtr.hpp" #include "wpi/math/filter/Debouncer.hpp" +#include "wpi/math/filter/EdgeCounterFilter.hpp" using namespace wpi; using namespace wpi::cmd; @@ -183,6 +184,14 @@ Trigger Trigger::Debounce(wpi::units::second_t debounceTime, }); } +Trigger Trigger::MultiPress(int requiredPresses, units::second_t windowTime) { + return Trigger(m_loop, [filter = wpi::math::EdgeCounterFilter(requiredPresses, + windowTime), + condition = m_condition]() mutable { + return filter.Calculate(condition()); + }); +} + bool Trigger::Get() const { return m_condition(); } diff --git a/commandsv2/src/main/native/include/wpi/commands2/button/Trigger.hpp b/commandsv2/src/main/native/include/wpi/commands2/button/Trigger.hpp index ac3ffe3bbf..c7b923962e 100644 --- a/commandsv2/src/main/native/include/wpi/commands2/button/Trigger.hpp +++ b/commandsv2/src/main/native/include/wpi/commands2/button/Trigger.hpp @@ -283,6 +283,22 @@ class Trigger { wpi::math::Debouncer::DebounceType type = wpi::math::Debouncer::DebounceType::kRising); + /** + * Creates a new multi-press trigger from this trigger - it will become active + * when this trigger has been activated the required number of times within + * the specified time window. + * + *

This is useful for implementing "double-click" style functionality. + * + *

Input for this must be stable, consider using a Debouncer before this to + * avoid counting noise as multiple presses. + * + * @param requiredPresses The number of presses required. + * @param windowTime The time in which the presses must occur. + * @return The multi-press trigger. + */ + Trigger MultiPress(int requiredPresses, units::second_t windowTime); + /** * Returns the current state of this trigger. * diff --git a/cscore/src/main/native/objcpp/UvcControlImpl.mm b/cscore/src/main/native/objcpp/UvcControlImpl.mm index 88699b0a83..c68b1797ec 100644 --- a/cscore/src/main/native/objcpp/UvcControlImpl.mm +++ b/cscore/src/main/native/objcpp/UvcControlImpl.mm @@ -484,7 +484,7 @@ const propertyInfo_t propertyInfo[] = UVCERROR("USB control interface not found"); break; default: - UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02Xh}:{:03Xh}:{:04Xh}", + UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02X}h:{:03X}h:{:04X}h", sys, sub, code); break; } diff --git a/fields/src/main/java/org/wpilib/fields/Fields.java b/fields/src/main/java/org/wpilib/fields/Fields.java index 1bd1532c8f..df804dff67 100644 --- a/fields/src/main/java/org/wpilib/fields/Fields.java +++ b/fields/src/main/java/org/wpilib/fields/Fields.java @@ -17,12 +17,13 @@ public enum Fields { k2022RapidReact("2022-rapidreact.json"), k2023ChargedUp("2023-chargedup.json"), k2024Crescendo("2024-crescendo.json"), - k2025Reefscape("2025-reefscape.json"); + k2025Reefscape("2025-reefscape.json"), + k2026Rebuilt("2026-rebuilt.json"); public static final String kBaseResourceDir = "/org/wpilib/fields/"; /** Alias to the current game. */ - public static final Fields kDefaultField = k2025Reefscape; + public static final Fields kDefaultField = k2026Rebuilt; public final String m_resourceFile; diff --git a/fields/src/main/native/cpp/fields.cpp b/fields/src/main/native/cpp/fields.cpp index 84f91cc468..f3606ccf1c 100644 --- a/fields/src/main/native/cpp/fields.cpp +++ b/fields/src/main/native/cpp/fields.cpp @@ -17,10 +17,12 @@ #include "wpi/fields/2023-chargedup.hpp" #include "wpi/fields/2024-crescendo.hpp" #include "wpi/fields/2025-reefscape.hpp" +#include "wpi/fields/2026-rebuilt.hpp" using namespace wpi::fields; static const Field kFields[] = { + {"2026 Rebuilt", GetResource_2026_rebuilt_json, GetResource_2026_field_png}, {"2025 Reefscape", GetResource_2025_reefscape_json, GetResource_2025_field_png}, {"2024 Crescendo", GetResource_2024_crescendo_json, diff --git a/fields/src/main/native/include/wpi/fields/2026-rebuilt.hpp b/fields/src/main/native/include/wpi/fields/2026-rebuilt.hpp new file mode 100644 index 0000000000..25a3ad1f45 --- /dev/null +++ b/fields/src/main/native/include/wpi/fields/2026-rebuilt.hpp @@ -0,0 +1,12 @@ +// 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. + +#pragma once + +#include + +namespace wpi::fields { +std::string_view GetResource_2026_rebuilt_json(); +std::string_view GetResource_2026_field_png(); +} // namespace wpi::fields diff --git a/fields/src/main/native/resources/org/wpilib/fields/2026-field.png b/fields/src/main/native/resources/org/wpilib/fields/2026-field.png new file mode 100644 index 0000000000..b4f0295e8e Binary files /dev/null and b/fields/src/main/native/resources/org/wpilib/fields/2026-field.png differ diff --git a/fields/src/main/native/resources/org/wpilib/fields/2026-rebuilt.json b/fields/src/main/native/resources/org/wpilib/fields/2026-rebuilt.json new file mode 100644 index 0000000000..bc8675c0d4 --- /dev/null +++ b/fields/src/main/native/resources/org/wpilib/fields/2026-rebuilt.json @@ -0,0 +1,19 @@ +{ + "game": "Rebuilt", + "field-image": "2026-field.png", + "field-corners": { + "top-left": [ + 245, + 118 + ], + "bottom-right": [ + 3942, + 1914 + ] + }, + "field-size": [ + 54.269, + 26.474 + ], + "field-unit": "foot" +} diff --git a/glass/src/lib/native/cpp/other/FMS.cpp b/glass/src/lib/native/cpp/other/FMS.cpp index 1fd6ef5701..965c6e1bb6 100644 --- a/glass/src/lib/native/cpp/other/FMS.cpp +++ b/glass/src/lib/native/cpp/other/FMS.cpp @@ -73,11 +73,11 @@ void wpi::glass::DisplayFMS(FMSModel* model, bool editableDsAttached) { } ImGui::SameLine(); if (ImGui::Button("Auto") && !enabled) { - model->SetMatchTime(15.0); + model->SetMatchTime(20.0); } ImGui::SameLine(); if (ImGui::Button("Teleop") && !enabled) { - model->SetMatchTime(135.0); + model->SetMatchTime(140.0); } } diff --git a/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp index 099647102c..87cc5dc538 100644 --- a/glass/src/lib/native/cpp/other/Field2D.cpp +++ b/glass/src/lib/native/cpp/other/Field2D.cpp @@ -348,7 +348,7 @@ static bool InputPose(wpi::math::Pose2d* pose) { } FieldInfo::FieldInfo(Storage& storage) - : m_builtin{storage.GetString("builtin", "2025 Reefscape")}, + : m_builtin{storage.GetString("builtin", "2026 Rebuilt")}, m_filename{storage.GetString("image")}, m_width{storage.GetFloat("width", kDefaultWidth.to())}, m_height{storage.GetFloat("height", kDefaultHeight.to())}, diff --git a/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java index a91ab76f3f..81fab9161b 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java @@ -241,16 +241,19 @@ public class DriverStationJNI extends JNIWrapper { public static native String getJoystickName(byte joystickNum); /** - * Returns the approximate match time. + * Return the approximate match time. The FMS does not send an official match time to the robots, + * but does send an approximate match time. The value will count down the time remaining in the + * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to + * dispute ref calls or guarantee that a function will trigger before the match ends). * - *

The FMS does not send an official match time to the robots, but does send an approximate - * match time. The value will count down the time remaining in the current period (auto or - * teleop). + *

When connected to the real field, this number only changes in full integer increments, and + * always counts down. * - *

Warning: This is not an official time (so it cannot be used to dispute ref calls or - * guarantee that a function will trigger before the match ends). + *

When the DS is in practice mode, this number is a floating point number, and counts down. * - *

The Practice Match function of the DS approximates the behavior seen on the field. + *

When the DS is in teleop or autonomous mode, this number returns -1.0. + * + *

Simulation matches DS behavior without an FMS connected. * * @return time remaining in current match period (auto or teleop) * @see "HAL_GetMatchTime" diff --git a/hal/src/main/native/include/wpi/hal/DriverStation.h b/hal/src/main/native/include/wpi/hal/DriverStation.h index 02c3ec0102..a244ee1c6d 100644 --- a/hal/src/main/native/include/wpi/hal/DriverStation.h +++ b/hal/src/main/native/include/wpi/hal/DriverStation.h @@ -240,8 +240,7 @@ int32_t HAL_SetJoystickLeds(int32_t joystickNum, int32_t leds); *

When the DS is in practice mode, this number is a floating point number, * and counts down. * - *

When the DS is in teleop or autonomous mode, this number is a floating - * point number, and counts up. + *

When the DS is in teleop or autonomous mode, this number returns -1.0. * *

Simulation matches DS behavior without an FMS connected. * diff --git a/tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp index 9e5b1d1b04..8e57451694 100644 --- a/tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp +++ b/tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp @@ -57,7 +57,8 @@ static double Lerp(wpi::units::second_t time, */ static std::vector ConvertToPrepared(const MotorData& data) { std::vector prepared; - // assume we've selected down to a single contiguous run by this point + + // Assume we've selected down to a single contiguous run by this point auto run = data.runs[0]; for (int i = 0; i < static_cast(run.voltage.size()) - 1; ++i) { @@ -101,7 +102,7 @@ static void CopyRawData(wpi::util::StringMap* dataset) { } /** - * Assigns the combines the various datasets into a single one for analysis. + * Combines the various datasets into a single one for analysis. * * @param slowForward The slow forward dataset * @param slowBackward The slow backward dataset @@ -127,6 +128,28 @@ void AnalysisManager::PrepareGeneralData() { // Convert data to PreparedData structs for (auto& it : m_data.motorData) { auto key = it.first; + + // Assume we've selected down to a single contiguous run by this point + auto run = m_data.motorData[key].runs[0]; + + // Ensure data has at least two samples in it or linear interpolation within + // ConvertToPrepared() will fail + if (run.voltage.size() < 2) { + throw sysid::InvalidDataError(fmt::format( + "{} data has {} voltage samples and at least 2 are required.", key, + run.voltage.size())); + } + if (run.position.size() < 2) { + throw sysid::InvalidDataError(fmt::format( + "{} data has {} position samples and at least 2 are required.", key, + run.position.size())); + } + if (run.velocity.size() < 2) { + throw sysid::InvalidDataError(fmt::format( + "{} data has {} velocity samples and at least 2 are required.", key, + run.velocity.size())); + } + preparedData[key] = ConvertToPrepared(m_data.motorData[key]); WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size()); } diff --git a/wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp b/wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp index e738ba3d43..05bd5c698e 100644 --- a/wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp +++ b/wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp @@ -11,6 +11,7 @@ #include #include +#include "wpi/hal/UsageReporting.h" #include "wpi/math/util/MathUtil.hpp" #include "wpi/util/MathExtras.hpp" #include "wpi/util/timestamp.h" @@ -21,7 +22,9 @@ LEDPattern::LEDPattern( std::function)> impl) - : m_impl(std::move(impl)) {} + : m_impl(std::move(impl)) { + HAL_ReportUsage("LEDPattern", ""); +} void LEDPattern::ApplyTo( LEDPattern::LEDReader reader, diff --git a/wpilibc/src/main/native/include/wpi/drive/DifferentialDrive.hpp b/wpilibc/src/main/native/include/wpi/drive/DifferentialDrive.hpp index 4446698dd4..ebc80f866d 100644 --- a/wpilibc/src/main/native/include/wpi/drive/DifferentialDrive.hpp +++ b/wpilibc/src/main/native/include/wpi/drive/DifferentialDrive.hpp @@ -76,7 +76,7 @@ class DifferentialDrive : public RobotDriveBase, * Construct a DifferentialDrive. * * To pass multiple motors per side, use CAN motor controller followers or - * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so + * PWMMotorController::AddFollower(). If a motor needs to be inverted, do so * before passing it in. * * @param leftMotor Left motor. @@ -90,7 +90,7 @@ class DifferentialDrive : public RobotDriveBase, * Construct a DifferentialDrive. * * To pass multiple motors per side, use CAN motor controller followers or - * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so + * PWMMotorController::AddFollower(). If a motor needs to be inverted, do so * before passing it in. * * @param leftMotor Left motor setter. diff --git a/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp b/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp index 7676dd07b8..309e7a3283 100644 --- a/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp +++ b/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp @@ -587,8 +587,7 @@ class DriverStation final { *

When the DS is in practice mode, this number is a floating point number, * and counts down. * - *

When the DS is in teleop or autonomous mode, this number is a floating - * point number, and counts up. + *

When the DS is in teleop or autonomous mode, this number returns -1.0. * *

Simulation matches DS behavior without an FMS connected. * diff --git a/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp index facc6e2025..55dca9c5d2 100644 --- a/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp @@ -64,7 +64,7 @@ class TimedRobot : public IterativeRobotBase { ~TimedRobot() override; /** - * Return the system clock time in micrseconds for the start of the current + * Return the system clock time in microseconds for the start of the current * periodic loop. This is in the same time base as Timer.GetFPGATimestamp(), * but is stable through a loop. It is updated at the beginning of every * periodic callback (including the normal periodic loop). diff --git a/wpilibc/src/main/native/include/wpi/system/Timer.hpp b/wpilibc/src/main/native/include/wpi/system/Timer.hpp index 03820002e1..e9264ddcc6 100644 --- a/wpilibc/src/main/native/include/wpi/system/Timer.hpp +++ b/wpilibc/src/main/native/include/wpi/system/Timer.hpp @@ -139,19 +139,23 @@ class Timer { static wpi::units::second_t GetFPGATimestamp(); /** - * Return the approximate match time. - * - * The FMS does not send an official match time to the robots, but does send - * an approximate match time. The value will count down the time remaining in - * the current period (auto or teleop). - * + * Return the approximate match time. The FMS does not send an official match + * time to the robots, but does send an approximate match time. The value will + * count down the time remaining in the current period (auto or teleop). * Warning: This is not an official time (so it cannot be used to dispute ref * calls or guarantee that a function will trigger before the match ends). * - * The Practice Match function of the DS approximates the behavior seen on the - * field. + *

When connected to the real field, this number only changes in full + * integer increments, and always counts down. * - * @return Time remaining in current match period (auto or teleop) + *

When the DS is in practice mode, this number is a floating point number, + * and counts down. + * + *

When the DS is in teleop or autonomous mode, this number returns -1.0. + * + *

Simulation matches DS behavior without an FMS connected. + * + * @return Time remaining in current match period (auto or teleop) in seconds */ static wpi::units::second_t GetMatchTime(); diff --git a/wpilibcExamples/src/main/cpp/examples/Xrptimed/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Xrptimed/cpp/Robot.cpp new file mode 100644 index 0000000000..5ea68bdcc2 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Xrptimed/cpp/Robot.cpp @@ -0,0 +1,56 @@ +// 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 "Robot.hpp" + +Robot::Robot() { + wpi::util::SendableRegistry::AddChild(&m_drive, &m_leftMotor); + wpi::util::SendableRegistry::AddChild(&m_drive, &m_rightMotor); + + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotor.SetInverted(true); +} + +/** + * This function is called every 20 ms, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() {} + +// This function is only once each time Autonomous is enabled +void Robot::AutonomousInit() { + m_timer.Restart(); +} + +// This function is called periodically during autonomous mode +void Robot::AutonomousPeriodic() { + // Drive for 2 seconds + if (m_timer.Get() < 2_s) { + // Drive forwards half speed, make sure to turn input squaring off + m_drive.ArcadeDrive(0.5, 0.0, false); + } else { + // Stop robot + m_drive.ArcadeDrive(0.0, 0.0, false); + } +} + +// This function is only once each time telop is enabled +void Robot::TeleopInit() {} + +// This function is called periodically during teleop mode +void Robot::TeleopPeriodic() { + m_drive.ArcadeDrive(-m_controller.GetRawAxis(2), -m_controller.GetRawAxis(1)); +} + +#ifndef RUNNING_FRC_TESTS +int main() { + return wpi::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/Xrptimed/include/Robot.hpp b/wpilibcExamples/src/main/cpp/examples/Xrptimed/include/Robot.hpp new file mode 100644 index 0000000000..b3d4e896d0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/Xrptimed/include/Robot.hpp @@ -0,0 +1,32 @@ +// 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. + +#pragma once + +#include "wpi/drive/DifferentialDrive.hpp" +#include "wpi/driverstation/Joystick.hpp" +#include "wpi/framework/TimedRobot.hpp" +#include "wpi/system/Timer.hpp" +#include "wpi/xrp/XRPMotor.hpp" + +class Robot : public wpi::TimedRobot { + public: + Robot(); + void RobotPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + + private: + wpi::xrp::XRPMotor m_leftMotor{0}; + wpi::xrp::XRPMotor m_rightMotor{1}; + // Assumes a gamepad plugged into channel 0 + wpi::Joystick m_controller{0}; + wpi::Timer m_timer; + + wpi::DifferentialDrive m_drive{ + [&](double output) { m_leftMotor.Set(output); }, + [&](double output) { m_rightMotor.Set(output); }}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index fd49884765..13ddf8df49 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -510,6 +510,22 @@ "xrp" ] }, + { + "name": "XRP Timed", + "description": "An very basic timed robot program that can be used with the XRP reference robot design.", + "tags": [ + "XRP", + "Differential Drive", + "Joystick" + ], + "foldername": "Xrptimed", + "gradlebase": "cppxrp", + "mainclass": "Main", + "commandversion": 2, + "extravendordeps": [ + "xrp" + ] + }, { "name": "StateSpaceFlywheel", "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.", diff --git a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java index 9ef82723ca..962257b9d0 100644 --- a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java +++ b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java @@ -1733,8 +1733,7 @@ public final class DriverStation { * *

When the DS is in practice mode, this number is a floating point number, and counts down. * - *

When the DS is in teleop or autonomous mode, this number is a floating point number, and - * counts up. + *

When the DS is in teleop or autonomous mode, this number returns -1.0. * *

Simulation matches DS behavior without an FMS connected. * diff --git a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java index dc6a75c3ab..bd4205f4b5 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java @@ -181,9 +181,9 @@ public class TimedRobot extends IterativeRobotBase { } /** - * Return the system clock time in micrseconds for the start of the current periodic loop. This is - * in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated - * at the beginning of every periodic callback (including the normal periodic loop). + * Return the system clock time in microseconds for the start of the current periodic loop. This + * is in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is + * updated at the beginning of every periodic callback (including the normal periodic loop). * * @return Robot running time in microseconds, as of the start of the current periodic function. */ diff --git a/wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java b/wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java index 51de594e64..a43204fabb 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java @@ -14,6 +14,7 @@ import java.util.Objects; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.HAL; import org.wpilib.system.RobotController; import org.wpilib.units.collections.LongToObjectHashMap; import org.wpilib.units.measure.Dimensionless; @@ -150,6 +151,7 @@ public interface LEDPattern { * @return the mapped pattern */ default LEDPattern mapIndex(IndexMapper indexMapper) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { int bufLen = reader.getLength(); applyTo( @@ -295,6 +297,7 @@ public interface LEDPattern { final long totalTimeMicros = (long) (onTime.in(Microseconds) + offTime.in(Microseconds)); final long onTimeMicros = (long) onTime.in(Microseconds); + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { if (RobotController.getTime() % totalTimeMicros < onTimeMicros) { applyTo(reader, writer); @@ -324,6 +327,7 @@ public interface LEDPattern { * @return the blinking pattern */ default LEDPattern synchronizedBlink(BooleanSupplier signal) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { if (signal.getAsBoolean()) { applyTo(reader, writer); @@ -343,6 +347,7 @@ public interface LEDPattern { default LEDPattern breathe(Time period) { final long periodMicros = (long) period.in(Microseconds); + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { applyTo( reader, @@ -374,6 +379,7 @@ public interface LEDPattern { * @return the combined overlay pattern */ default LEDPattern overlayOn(LEDPattern base) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { // write the base pattern down first... base.applyTo(reader, writer); @@ -401,6 +407,7 @@ public interface LEDPattern { * @return the blended pattern */ default LEDPattern blend(LEDPattern other) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { applyTo(reader, writer); @@ -432,6 +439,7 @@ public interface LEDPattern { * @return the masked pattern */ default LEDPattern mask(LEDPattern mask) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { // Apply the current pattern down as normal... applyTo(reader, writer); @@ -471,6 +479,7 @@ public interface LEDPattern { default LEDPattern atBrightness(Dimensionless relativeBrightness) { double multiplier = relativeBrightness.in(Value); + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { applyTo( reader, @@ -497,6 +506,7 @@ public interface LEDPattern { * @return the pattern */ static LEDPattern solid(Color color) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { int bufLen = reader.getLength(); for (int led = 0; led < bufLen; led++) { @@ -526,6 +536,7 @@ public interface LEDPattern { * @return the mask pattern */ static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { double progress = Math.clamp(progressSupplier.getAsDouble(), 0, 1); @@ -562,6 +573,7 @@ public interface LEDPattern { * @return a motionless step pattern */ static LEDPattern steps(Map steps) { + HAL.reportUsage("LEDPattern", ""); if (steps.isEmpty()) { // no colors specified DriverStation.reportWarning("Creating LED steps with no colors!", false); @@ -623,6 +635,7 @@ public interface LEDPattern { * @return a motionless gradient pattern */ static LEDPattern gradient(GradientType type, Color... colors) { + HAL.reportUsage("LEDPattern", ""); if (colors.length == 0) { // Nothing to display DriverStation.reportWarning("Creating a gradient with no colors!", false); @@ -680,6 +693,7 @@ public interface LEDPattern { * @return the rainbow pattern */ static LEDPattern rainbow(int saturation, int value) { + HAL.reportUsage("LEDPattern", ""); return (reader, writer) -> { int bufLen = reader.getLength(); for (int i = 0; i < bufLen; i++) { diff --git a/wpilibj/src/main/java/org/wpilib/system/Timer.java b/wpilibj/src/main/java/org/wpilib/system/Timer.java index 790db4b94d..5620d24da7 100644 --- a/wpilibj/src/main/java/org/wpilib/system/Timer.java +++ b/wpilibj/src/main/java/org/wpilib/system/Timer.java @@ -41,8 +41,16 @@ public class Timer { * Return the approximate match time. The FMS does not send an official match time to the robots, * but does send an approximate match time. The value will count down the time remaining in the * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to - * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice - * Match function of the DS approximates the behavior seen on the field. + * dispute ref calls or guarantee that a function will trigger before the match ends). + * + *

When connected to the real field, this number only changes in full integer increments, and + * always counts down. + * + *

When the DS is in practice mode, this number is a floating point number, and counts down. + * + *

When the DS is in teleop or autonomous mode, this number returns -1.0. + * + *

Simulation matches DS behavior without an FMS connected. * * @return Time remaining in current match period (auto or teleop) in seconds */ diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json index 765abb2424..16e95134c9 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json @@ -706,6 +706,22 @@ "xrp" ] }, + { + "name": "XRP Timed", + "description": "An very basic timed robot program that can be used with the XRP reference robot design.", + "tags": [ + "XRP", + "Differential Drive", + "Joystick" + ], + "foldername": "xrptimed", + "gradlebase": "javaxrp", + "mainclass": "Main", + "commandversion": 2, + "extravendordeps": [ + "xrp" + ] + }, { "name": "Digital Communication Sample", "description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.", diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Main.java b/wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Main.java new file mode 100644 index 0000000000..718bd8b36c --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Main.java @@ -0,0 +1,25 @@ +// 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 org.wpilib.examples.xrptimed; + +import org.wpilib.framework.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Robot.java new file mode 100644 index 0000000000..1abcf0e8eb --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Robot.java @@ -0,0 +1,74 @@ +// 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 org.wpilib.examples.xrptimed; + +import org.wpilib.drive.DifferentialDrive; +import org.wpilib.driverstation.Joystick; +import org.wpilib.framework.TimedRobot; +import org.wpilib.system.Timer; +import org.wpilib.util.sendable.SendableRegistry; +import org.wpilib.xrp.XRPMotor; + +/** + * The methods in this class are called automatically corresponding to each mode, as described in + * the TimedRobot documentation. If you change the name of this class or the package after creating + * this project, you must also update the manifest file in the resource directory. + */ +public class Robot extends TimedRobot { + private final XRPMotor m_leftDrive = new XRPMotor(0); + private final XRPMotor m_rightDrive = new XRPMotor(1); + private final DifferentialDrive m_robotDrive = + new DifferentialDrive(m_leftDrive::set, m_rightDrive::set); + // Assumes a gamepad plugged into channel 0 + private final Joystick m_controller = new Joystick(0); + private final Timer m_timer = new Timer(); + + /** Called once at the beginning of the robot program. */ + public Robot() { + SendableRegistry.addChild(m_robotDrive, m_leftDrive); + SendableRegistry.addChild(m_robotDrive, m_rightDrive); + + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightDrive.setInverted(true); + } + + /** This function is run once each time the robot enters autonomous mode. */ + @Override + public void autonomousInit() { + m_timer.restart(); + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() { + // Drive for 2 seconds + if (m_timer.get() < 2.0) { + // Drive forwards half speed, make sure to turn input squaring off + m_robotDrive.arcadeDrive(0.5, 0.0, false); + } else { + m_robotDrive.stopMotor(); // stop robot + } + } + + /** This function is called once each time the robot enters teleoperated mode. */ + @Override + public void teleopInit() {} + + /** This function is called periodically during teleoperated mode. */ + @Override + public void teleopPeriodic() { + m_robotDrive.arcadeDrive(-m_controller.getRawAxis(2), -m_controller.getRawAxis(1)); + } + + /** This function is called once each time the robot enters test mode. */ + @Override + public void testInit() {} + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} +} diff --git a/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java index bc43dba74f..fbd62fecef 100644 --- a/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java @@ -11,6 +11,7 @@ import org.wpilib.math.linalg.Vector; import org.wpilib.math.numbers.N1; import org.wpilib.math.system.Discretization; import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.util.MathSharedStore; import org.wpilib.math.util.Num; import org.wpilib.math.util.StateSpaceUtil; @@ -119,6 +120,7 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumCols(), 1)); reset(); + MathSharedStore.getMathShared().reportUsage("LinearQuadraticRegulator", ""); } /** @@ -158,6 +160,7 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumCols(), 1)); reset(); + MathSharedStore.getMathShared().reportUsage("LinearQuadraticRegulator", ""); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java index 63bad3206f..670fd27b21 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java @@ -12,6 +12,7 @@ import org.wpilib.math.system.Discretization; import org.wpilib.math.system.LinearSystemUtil; import org.wpilib.math.system.NumericalIntegration; import org.wpilib.math.system.NumericalJacobian; +import org.wpilib.math.util.MathSharedStore; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Num; import org.wpilib.math.util.StateSpaceUtil; @@ -164,6 +165,8 @@ public class ExtendedKalmanFilter(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); reset(); + + MathSharedStore.getMathShared().reportUsage("KalmanFilter", ""); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java index 0e86941138..de8d941630 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java @@ -15,11 +15,9 @@ import org.wpilib.math.interpolation.TimeInterpolatableBuffer; import org.wpilib.math.kinematics.Kinematics; import org.wpilib.math.kinematics.Odometry; import org.wpilib.math.linalg.Matrix; -import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; import org.wpilib.math.util.MathSharedStore; -import org.wpilib.math.util.Nat; /** * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder @@ -38,8 +36,12 @@ import org.wpilib.math.util.Nat; */ public class PoseEstimator { private final Odometry m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); - private final Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); + + // Diagonal of process noise covariance matrix Q + private final double[] m_q = new double[] {0.0, 0.0, 0.0}; + + // Diagonal of Kalman gain matrix K + private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0}; private static final double kBufferDuration = 1.5; // Maps timestamps to odometry-only pose estimates @@ -47,7 +49,9 @@ public class PoseEstimator { TimeInterpolatableBuffer.createBuffer(kBufferDuration); // Maps timestamps to vision updates // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have - // been no vision measurements after the last reset + // been no vision measurements after the last reset. May contain one entry while + // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to + // ResetRotation/ResetTranslation. private final NavigableMap m_visionUpdates = new TreeMap<>(); private Pose2d m_poseEstimate; @@ -75,9 +79,10 @@ public class PoseEstimator { m_poseEstimate = m_odometry.getPose(); for (int i = 0; i < 3; ++i) { - m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); + m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0); } setVisionMeasurementStdDevs(visionMeasurementStdDevs); + MathSharedStore.getMathShared().reportUsage("PoseEstimator", ""); } /** @@ -90,6 +95,7 @@ public class PoseEstimator { * theta]ᵀ, with units in meters and radians. */ public final void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { + // Diagonal of measurement noise covariance matrix R var r = new double[3]; for (int i = 0; i < 3; ++i) { r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0); @@ -98,11 +104,10 @@ public class PoseEstimator { // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 // and C = I. See wpimath/algorithms.md. for (int row = 0; row < 3; ++row) { - if (m_q.get(row, 0) == 0.0) { - m_visionK.set(row, row, 0.0); + if (m_q[row] == 0.0) { + m_vision_k[row] = 0.0; } else { - m_visionK.set( - row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row]))); + m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row])); } } } @@ -144,9 +149,22 @@ public class PoseEstimator { */ public void resetTranslation(Translation2d translation) { m_odometry.resetTranslation(translation); + + final var latestVisionUpdate = m_visionUpdates.lastEntry(); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose(); + + if (latestVisionUpdate != null) { + // apply vision compensation to the pose rotation + final var visionUpdate = + new VisionUpdate( + new Pose2d(translation, latestVisionUpdate.getValue().visionPose.getRotation()), + new Pose2d(translation, latestVisionUpdate.getValue().odometryPose.getRotation())); + m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); + } else { + m_poseEstimate = m_odometry.getPose(); + } } /** @@ -156,9 +174,22 @@ public class PoseEstimator { */ public void resetRotation(Rotation2d rotation) { m_odometry.resetRotation(rotation); + + final var latestVisionUpdate = m_visionUpdates.lastEntry(); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose(); + + if (latestVisionUpdate != null) { + // apply vision compensation to the pose translation + final var visionUpdate = + new VisionUpdate( + new Pose2d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation), + new Pose2d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation)); + m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); + } else { + m_poseEstimate = m_odometry.getPose(); + } } /** @@ -274,29 +305,23 @@ public class PoseEstimator { var transform = visionRobotPose.minus(visionSample.get()); // Step 5: We should not trust the transform entirely, so instead we scale this transform by a - // Kalman - // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_transform = - m_visionK.times( - VecBuilder.fill( - transform.getX(), transform.getY(), transform.getRotation().getRadians())); - - // Step 6: Convert back to Transform2d. + // Kalman gain matrix representing how much we trust vision measurements compared to our current + // pose. Then, we convert the result back to a Transform2d. var scaledTransform = new Transform2d( - k_times_transform.get(0, 0), - k_times_transform.get(1, 0), - Rotation2d.fromRadians(k_times_transform.get(2, 0))); + m_vision_k[0] * transform.getX(), + m_vision_k[1] * transform.getY(), + Rotation2d.fromRadians(m_vision_k[2] * transform.getRotation().getRadians())); - // Step 7: Calculate and record the vision update. + // Step 6: Calculate and record the vision update. var visionUpdate = new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestamp, visionUpdate); - // Step 8: Remove later vision measurements. (Matches previous behavior) + // Step 7: Remove later vision measurements. (Matches previous behavior) m_visionUpdates.tailMap(timestamp, false).entrySet().clear(); - // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update, + // Step 8: Update latest pose estimate. Since we cleared all updates after this vision update, // it's guaranteed to be the latest vision update. m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); } diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java index d9ab292d2a..4442102f19 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java @@ -18,12 +18,9 @@ import org.wpilib.math.interpolation.TimeInterpolatableBuffer; import org.wpilib.math.kinematics.Kinematics; import org.wpilib.math.kinematics.Odometry3d; import org.wpilib.math.linalg.Matrix; -import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N4; -import org.wpilib.math.numbers.N6; import org.wpilib.math.util.MathSharedStore; -import org.wpilib.math.util.Nat; /** * This class wraps {@link Odometry3d} to fuse latency-compensated vision measurements with encoder @@ -46,8 +43,12 @@ import org.wpilib.math.util.Nat; */ public class PoseEstimator3d { private final Odometry3d m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N4(), Nat.N1()); - private final Matrix m_visionK = new Matrix<>(Nat.N6(), Nat.N6()); + + // Diagonal of process noise covariance matrix Q + private final double[] m_q = new double[] {0.0, 0.0, 0.0, 0.0}; + + // Diagonal of Kalman gain matrix K + private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; private static final double kBufferDuration = 1.5; // Maps timestamps to odometry-only pose estimates @@ -55,7 +56,9 @@ public class PoseEstimator3d { TimeInterpolatableBuffer.createBuffer(kBufferDuration); // Maps timestamps to vision updates // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have - // been no vision measurements after the last reset + // been no vision measurements after the last reset. May contain one entry while + // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to + // ResetRotation/ResetTranslation. private final NavigableMap m_visionUpdates = new TreeMap<>(); private Pose3d m_poseEstimate; @@ -83,9 +86,10 @@ public class PoseEstimator3d { m_poseEstimate = m_odometry.getPose(); for (int i = 0; i < 4; ++i) { - m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); + m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0); } setVisionMeasurementStdDevs(visionMeasurementStdDevs); + MathSharedStore.getMathShared().reportUsage("PoseEstimator3d", ""); } /** @@ -98,6 +102,7 @@ public class PoseEstimator3d { * theta]ᵀ, with units in meters and radians. */ public final void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { + // Diagonal of measurement covariance matrix R var r = new double[4]; for (int i = 0; i < 4; ++i) { r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0); @@ -106,17 +111,16 @@ public class PoseEstimator3d { // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 // and C = I. See wpimath/algorithms.md. for (int row = 0; row < 4; ++row) { - if (m_q.get(row, 0) == 0.0) { - m_visionK.set(row, row, 0.0); + if (m_q[row] == 0.0) { + m_vision_k[row] = 0.0; } else { - m_visionK.set( - row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row]))); + m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row])); } } // Fill in the gains for the other components of the rotation vector - double angle_gain = m_visionK.get(3, 3); - m_visionK.set(4, 4, angle_gain); - m_visionK.set(5, 5, angle_gain); + double angle_gain = m_vision_k[3]; + m_vision_k[4] = angle_gain; + m_vision_k[5] = angle_gain; } /** @@ -156,9 +160,22 @@ public class PoseEstimator3d { */ public void resetTranslation(Translation3d translation) { m_odometry.resetTranslation(translation); + + final var latestVisionUpdate = m_visionUpdates.lastEntry(); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose(); + + if (latestVisionUpdate != null) { + // apply vision compensation to the pose rotation + final var visionUpdate = + new VisionUpdate( + new Pose3d(translation, latestVisionUpdate.getValue().visionPose.getRotation()), + new Pose3d(translation, latestVisionUpdate.getValue().odometryPose.getRotation())); + m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); + } else { + m_poseEstimate = m_odometry.getPose(); + } } /** @@ -168,9 +185,22 @@ public class PoseEstimator3d { */ public void resetRotation(Rotation3d rotation) { m_odometry.resetRotation(rotation); + + final var latestVisionUpdate = m_visionUpdates.lastEntry(); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose(); + + if (latestVisionUpdate != null) { + // apply vision compensation to the pose translation + final var visionUpdate = + new VisionUpdate( + new Pose3d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation), + new Pose3d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation)); + m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); + } else { + m_poseEstimate = m_odometry.getPose(); + } } /** @@ -286,38 +316,27 @@ public class PoseEstimator3d { var transform = visionRobotPose.minus(visionSample.get()); // Step 5: We should not trust the transform entirely, so instead we scale this transform by a - // Kalman - // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_transform = - m_visionK.times( - VecBuilder.fill( - transform.getX(), - transform.getY(), - transform.getZ(), - transform.getRotation().getX(), - transform.getRotation().getY(), - transform.getRotation().getZ())); - - // Step 6: Convert back to Transform3d. + // Kalman gain matrix representing how much we trust vision measurements compared to our current + // pose. Then we convert the result back to a Transform3d. var scaledTransform = new Transform3d( - k_times_transform.get(0, 0), - k_times_transform.get(1, 0), - k_times_transform.get(2, 0), + m_vision_k[0] * transform.getX(), + m_vision_k[1] * transform.getY(), + m_vision_k[2] * transform.getZ(), new Rotation3d( - k_times_transform.get(3, 0), - k_times_transform.get(4, 0), - k_times_transform.get(5, 0))); + m_vision_k[3] * transform.getRotation().getX(), + m_vision_k[4] * transform.getRotation().getY(), + m_vision_k[5] * transform.getRotation().getZ())); - // Step 7: Calculate and record the vision update. + // Step 6: Calculate and record the vision update. var visionUpdate = new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestamp, visionUpdate); - // Step 8: Remove later vision measurements. (Matches previous behavior) + // Step 7: Remove later vision measurements. (Matches previous behavior) m_visionUpdates.tailMap(timestamp, false).entrySet().clear(); - // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update, + // Step 8: Update latest pose estimate. Since we cleared all updates after this vision update, // it's guaranteed to be the latest vision update. m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); } diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java b/wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java index 7aad50ef2b..5717ea96fc 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java @@ -9,6 +9,7 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.numbers.N1; import org.wpilib.math.system.Discretization; import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.util.MathSharedStore; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Num; import org.wpilib.math.util.StateSpaceUtil; @@ -107,6 +108,7 @@ public class SteadyStateKalmanFilter(S.getStorage().solve(C.times(P).getStorage()).transpose()); reset(); + MathSharedStore.getMathShared().reportUsage("SteadyStateKalmanFilter", ""); } /** Resets the observer. */ diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java index 0391bf8ef3..a502ddf988 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java @@ -12,6 +12,7 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.system.Discretization; import org.wpilib.math.system.NumericalIntegration; import org.wpilib.math.system.NumericalJacobian; +import org.wpilib.math.util.MathSharedStore; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Num; import org.wpilib.math.util.Pair; @@ -173,6 +174,7 @@ public class UnscentedKalmanFilter Pair, Matrix> squareRootUnscentedTransform( diff --git a/wpimath/src/main/java/org/wpilib/math/filter/EdgeCounterFilter.java b/wpimath/src/main/java/org/wpilib/math/filter/EdgeCounterFilter.java new file mode 100644 index 0000000000..2b0fe0001d --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/filter/EdgeCounterFilter.java @@ -0,0 +1,120 @@ +// 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 org.wpilib.math.filter; + +import org.wpilib.math.util.MathSharedStore; + +/** + * A rising edge counter for boolean streams. Requires that the boolean change value to true for a + * specified number of times within a specified time window after the first rising edge before the + * filtered value changes. + * + *

The filter activates when the input has risen (transitioned from false to true) the required + * number of times within the time window. Once activated, the output remains true as long as the + * input is true. The edge count resets when the time window expires or when the input goes false + * after activation. + * + *

Input must be stable; consider using a Debouncer before this filter to avoid counting noise as + * multiple edges. + */ +public class EdgeCounterFilter { + private int m_requiredEdges; + private double m_windowTimeSeconds; + + private double m_firstEdgeTimeSeconds; + private int m_currentCount; + + private boolean m_lastInput; + + /** + * Creates a new EdgeCounterFilter. + * + * @param requiredEdges The number of rising edges required before the output goes true. + * @param windowTime The maximum number of seconds in which all required edges must occur after + * the first rising edge. + */ + public EdgeCounterFilter(int requiredEdges, double windowTime) { + m_requiredEdges = requiredEdges; + m_windowTimeSeconds = windowTime; + + resetTimer(); + } + + private void resetTimer() { + m_firstEdgeTimeSeconds = MathSharedStore.getTimestamp(); + } + + private boolean hasElapsed() { + return MathSharedStore.getTimestamp() - m_firstEdgeTimeSeconds >= m_windowTimeSeconds; + } + + /** + * Applies the edge counter filter to the input stream. + * + * @param input The current value of the input stream. + * @return True if the required number of edges have occurred within the time window and the input + * is currently true; false otherwise. + */ + public boolean calculate(boolean input) { + boolean enoughEdges = m_currentCount >= m_requiredEdges; + + boolean expired = hasElapsed() && !enoughEdges; + boolean activationEnded = !input && enoughEdges; + + if (expired || activationEnded) { + m_currentCount = 0; + } + + if (input && !m_lastInput) { + if (m_currentCount == 0) { + resetTimer(); // Start timer on first rising edge + } + + m_currentCount++; + } + + m_lastInput = input; + + return input && m_currentCount >= m_requiredEdges; + } + + /** + * Sets the time window duration. + * + * @param windowTime The maximum window of seconds in which all required edges must occur after + * the first rising edge. + */ + public void setWindowTime(double windowTime) { + m_windowTimeSeconds = windowTime; + } + + /** + * Gets the time window duration. + * + * @return The maximum window of seconds in which all required edges must occur after the first + * rising edge. + */ + public double getWindowTime() { + return m_windowTimeSeconds; + } + + /** + * Sets the required number of edges. + * + * @param requiredEdges The number of rising edges required before the output goes true. + */ + public void setRequiredEdges(int requiredEdges) { + m_requiredEdges = requiredEdges; + } + + /** + * Gets the required number of edges. + * + * @return The number of rising edges required before the output goes true. + */ + public int getRequiredEdges() { + return m_requiredEdges; + } +} diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java index f16da87aa7..af65808c5c 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java @@ -16,7 +16,7 @@ public class CoordinateSystem { private static final CoordinateSystem m_ned = new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D()); - // Rotation from this coordinate system to NWU coordinate system + // Rotation from this coordinate system to NWU coordinate system when applied extrinsically private final Rotation3d m_rotation; /** @@ -85,7 +85,8 @@ public class CoordinateSystem { */ public static Translation3d convert( Translation3d translation, CoordinateSystem from, CoordinateSystem to) { - return translation.rotateBy(from.m_rotation.minus(to.m_rotation)); + // Convert to NWU, then convert to the new coordinate system + return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus()); } /** @@ -98,7 +99,8 @@ public class CoordinateSystem { */ public static Rotation3d convert( Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) { - return rotation.rotateBy(from.m_rotation.minus(to.m_rotation)); + // Convert to NWU, then convert to the new coordinate system + return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus()); } /** @@ -124,9 +126,23 @@ public class CoordinateSystem { */ public static Transform3d convert( Transform3d transform, CoordinateSystem from, CoordinateSystem to) { - var coordRot = from.m_rotation.minus(to.m_rotation); + // coordRot is the rotation that converts between the coordinate systems when applied + // extrinsically. It first converts to NWU, then converts to the new coordinate system. + var coordRot = from.m_rotation.rotateBy(to.m_rotation.unaryMinus()); + // The new rotation is the extrinsic rotation from convert(zero) to + // convert(transformRot). That is, applying convertedRot extrinsically to + // convert(zero) produces convert(transformRot). In the below snippet, we + // use matrix notation, so rotA rotB applies rotA extrinsically to rotB. + // + // convertedRot convert(zero) = convert(transformRot) + // convertedRot = convert(transformRot) convert(zero)⁻¹ + // = (coordRot transformRot) (coordRot zero)⁻¹ + // = (coordRot transformRot) coordRot⁻¹ + // + // In code, the equivalent for rotA rotB is rotB.rotateBy(rotA) (note the + // change in order), and the equivalent for rot⁻¹ is rot.unaryMinus(). return new Transform3d( convert(transform.getTranslation(), from, to), - coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot))); + coordRot.unaryMinus().rotateBy(transform.getRotation().rotateBy(coordRot))); } } diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Pose2d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Pose2d.java index 4bfecd57c6..460cd85261 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Pose2d.java @@ -220,7 +220,7 @@ public class Pose2d implements Interpolatable, ProtobufSerializable, Str public Pose2d transformBy(Transform2d other) { return new Pose2d( m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - other.getRotation().plus(m_rotation)); + other.getRotation().rotateBy(m_rotation)); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java index 759de1afba..1db6cdd75a 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java @@ -253,9 +253,12 @@ public class Pose3d implements Interpolatable, ProtobufSerializable, Str * @return The transformed pose. */ public Pose3d transformBy(Transform3d other) { + // Rotating the transform's rotation by the pose's rotation extrinsically is equivalent to + // rotating the pose's rotation by the transform's rotation intrinsically. (We define transforms + // as being applied intrinsically.) return new Pose3d( m_translation.plus(other.getTranslation().rotateBy(m_rotation)), - other.getRotation().plus(m_rotation)); + other.getRotation().rotateBy(m_rotation)); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java index 805ddf8eed..0525bb8a96 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java @@ -264,6 +264,17 @@ public class Rotation2d m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); } + /** + * Returns the current rotation relative to the given rotation. + * + * @param other The rotation describing the orientation of the new coordinate frame that the + * current rotation will be converted into. + * @return The current rotation relative to the new orientation of the coordinate frame. + */ + public Rotation2d relativeTo(Rotation2d other) { + return rotateBy(other.unaryMinus()); + } + /** * Returns matrix representation of this rotation. * diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java index c6980f670f..ce24c36efa 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java @@ -25,7 +25,49 @@ import org.wpilib.units.measure.Angle; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; -/** A rotation in a 3D coordinate frame represented by a quaternion. */ +/** + * A rotation in a 3D coordinate frame, represented by a quaternion. Note that unlike 2D rotations, + * 3D rotations are not always commutative, so changing the order of rotations changes the result. + * + *

As an example of the order of rotations mattering, suppose we have a card that looks like + * this: + * + * 

+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ *
+ * + *

If we rotate 90º clockwise around the axis into the page, then flip around the left/right + * axis, we get one result: + * + * 

+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ *
+ * + *

If we flip around the left/right axis, then rotate 90º clockwise around the axis into the + * page, we get a different result: + * + * 

+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ *
+ * + *

Because order matters for 3D rotations, we need to distinguish between extrinsic + * rotations and intrinsic rotations. Rotating extrinsically means rotating around the + * global axes, while rotating intrinsically means rotating from the perspective of the other + * rotation. A neat property is that applying a series of rotations extrinsically is the same as + * applying the same series in the opposite order intrinsically. + */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation3d @@ -285,9 +327,17 @@ public class Rotation3d } /** - * Adds two rotations together. + * Adds two rotations together. The other rotation is applied extrinsically to this rotation, + * which is equivalent to this rotation being applied intrinsically to the other rotation. See the + * class comment for definitions of extrinsic and intrinsic rotations. * - * @param other The rotation to add. + *

Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))} + * sometimes doesn't. To apply a rotation offset, use either {@code offset = + * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset = + * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected + * angle needs to change as the input angle changes. + * + * @param other The rotation to add (applied extrinsically). * @return The sum of the two rotations. */ public Rotation3d plus(Rotation3d other) { @@ -295,10 +345,19 @@ public class Rotation3d } /** - * Subtracts the new rotation from the current rotation and returns the new rotation. + * Subtracts the other rotation from the current rotation and returns the new rotation. The new + * rotation is from the perspective of the other rotation (like {@link Pose3d#minus}), so it needs + * to be applied intrinsically. See the class comment for definitions of extrinsic and intrinsic + * rotations. + * + *

Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))} + * sometimes doesn't. To apply a rotation offset, use either {@code offset = + * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset = + * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected + * angle needs to change as the input angle changes. * * @param other The rotation to subtract. - * @return The difference between the two rotations. + * @return The difference between the two rotations, from the perspective of the other rotation. */ public Rotation3d minus(Rotation3d other) { return rotateBy(other.unaryMinus()); @@ -357,6 +416,24 @@ public class Rotation3d return new Rotation3d(other.m_q.times(m_q)); } + /** + * Returns the current rotation relative to the given rotation. Because the result is in the + * perspective of the given rotation, it must be applied intrinsically. See the class comment for + * definitions of extrinsic and intrinsic rotations. + * + * @param other The rotation describing the orientation of the new coordinate frame that the + * current rotation will be converted into. + * @return The current rotation relative to the new orientation of the coordinate frame. + */ + public Rotation3d relativeTo(Rotation3d other) { + // To apply a quaternion intrinsically, we must right-multiply by that quaternion. + // Therefore, "this_q relative to other_q" is the q such that other_q q = this_q: + // + // other_q q = this_q + // q = other_q⁻¹ this_q + return new Rotation3d(other.m_q.inverse().times(m_q)); + } + /** * Returns the quaternion representation of the Rotation3d. * @@ -561,7 +638,7 @@ public class Rotation3d @Override public Rotation3d interpolate(Rotation3d endValue, double t) { - return plus(endValue.minus(this).times(Math.clamp(t, 0, 1))); + return endValue.minus(this).times(Math.clamp(t, 0, 1)).plus(this); } /** Rotation3d protobuf for serialization. */ diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Transform2d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Transform2d.java index 48cf673471..3eb461e419 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Transform2d.java @@ -36,15 +36,14 @@ public class Transform2d implements ProtobufSerializable, StructSerializable { * @param last The final pose for the transformation. */ public Transform2d(Pose2d initial, Pose2d last) { - // We are rotating the difference between the translations - // using a clockwise rotation matrix. This transforms the global - // delta into a local delta (relative to the initial pose). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = last.getTranslation() .minus(initial.getTranslation()) .rotateBy(initial.getRotation().unaryMinus()); - m_rotation = last.getRotation().minus(initial.getRotation()); + m_rotation = last.getRotation().relativeTo(initial.getRotation()); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java index 5da26ed3bb..e65e2dd2f2 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java @@ -18,7 +18,10 @@ import org.wpilib.units.measure.Distance; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; -/** Represents a transformation for a Pose3d in the pose's frame. */ +/** + * Represents a transformation for a Pose3d in the pose's frame. Translation is applied before + * rotation. (The translation is applied in the pose's original frame, not the transformed frame.) + */ public class Transform3d implements ProtobufSerializable, StructSerializable { /** * A preallocated Transform3d representing no transformation. @@ -37,15 +40,14 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { * @param last The final pose for the transformation. */ public Transform3d(Pose3d initial, Pose3d last) { - // We are rotating the difference between the translations - // using a clockwise rotation matrix. This transforms the global - // delta into a local delta (relative to the initial pose). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = last.getTranslation() .minus(initial.getTranslation()) .rotateBy(initial.getRotation().unaryMinus()); - m_rotation = last.getRotation().minus(initial.getRotation()); + m_rotation = last.getRotation().relativeTo(initial.getRotation()); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java index 79dcd243cf..1e6c0ad407 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java @@ -24,7 +24,10 @@ public class Odometry { private Pose2d m_pose; private Rotation2d m_gyroOffset; + + // Always equal to m_poseMeters.getRotation() private Rotation2d m_previousAngle; + private final T m_previousWheelPositions; /** @@ -39,7 +42,7 @@ public class Odometry { Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) { m_kinematics = kinematics; m_pose = initialPose; - m_gyroOffset = m_pose.getRotation().minus(gyroAngle); + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation()); m_previousAngle = m_pose.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -56,8 +59,8 @@ public class Odometry { */ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) { m_pose = pose; + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation()); m_previousAngle = m_pose.getRotation(); - m_gyroOffset = m_pose.getRotation().minus(gyroAngle); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } @@ -67,7 +70,8 @@ public class Odometry { * @param pose The pose to reset to. */ public void resetPose(Pose2d pose) { - m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation())); + m_gyroOffset = + m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation()); m_pose = pose; m_previousAngle = m_pose.getRotation(); } @@ -87,7 +91,7 @@ public class Odometry { * @param rotation The rotation to reset to. */ public void resetRotation(Rotation2d rotation) { - m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation())); + m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation); m_pose = new Pose2d(m_pose.getTranslation(), rotation); m_previousAngle = m_pose.getRotation(); } @@ -112,7 +116,7 @@ public class Odometry { * @return The new pose of the robot. */ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { - var angle = gyroAngle.plus(m_gyroOffset); + var angle = gyroAngle.rotateBy(m_gyroOffset); var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); twist.dtheta = angle.minus(m_previousAngle).getRadians(); diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java index dd68b4d7d3..c2554dc9cc 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java @@ -32,8 +32,15 @@ public class Odometry3d { private final Kinematics m_kinematics; private Pose3d m_pose; + // Applying a rotation intrinsically to the measured gyro angle should cause the corrected angle + // to be rotated intrinsically in the same way, so the measured gyro angle must be applied + // intrinsically. This is equivalent to applying the offset extrinsically to the measured gyro + // angle. private Rotation3d m_gyroOffset; + + // Always equal to m_poseMeters.getRotation() private Rotation3d m_previousAngle; + private final T m_previousWheelPositions; /** @@ -48,7 +55,9 @@ public class Odometry3d { Kinematics kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { m_kinematics = kinematics; m_pose = initialPose; - m_gyroOffset = m_pose.getRotation().minus(gyroAngle); + // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and + // then rotates to m_poseMeters.getRotation() + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation()); m_previousAngle = m_pose.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -65,8 +74,10 @@ public class Odometry3d { */ public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) { m_pose = pose; + // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and + // then rotates to m_poseMeters.getRotation() + m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation()); m_previousAngle = m_pose.getRotation(); - m_gyroOffset = m_pose.getRotation().minus(gyroAngle); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } @@ -76,7 +87,9 @@ public class Odometry3d { * @param pose The pose to reset to. */ public void resetPose(Pose3d pose) { - m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation())); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = + m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation()); m_pose = pose; m_previousAngle = m_pose.getRotation(); } @@ -96,7 +109,8 @@ public class Odometry3d { * @param rotation The rotation to reset to. */ public void resetRotation(Rotation3d rotation) { - m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation())); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation); m_pose = new Pose3d(m_pose.getTranslation(), rotation); m_previousAngle = m_pose.getRotation(); } @@ -121,7 +135,7 @@ public class Odometry3d { * @return The new pose of the robot. */ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { - var angle = gyroAngle.plus(m_gyroOffset); + var angle = gyroAngle.rotateBy(m_gyroOffset); var angle_difference = angle.minus(m_previousAngle).toVector(); var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); diff --git a/wpimath/src/main/java/org/wpilib/math/system/LinearSystemLoop.java b/wpimath/src/main/java/org/wpilib/math/system/LinearSystemLoop.java index dccc4089f5..17f424c9ad 100644 --- a/wpimath/src/main/java/org/wpilib/math/system/LinearSystemLoop.java +++ b/wpimath/src/main/java/org/wpilib/math/system/LinearSystemLoop.java @@ -12,6 +12,7 @@ import org.wpilib.math.controller.LinearQuadraticRegulator; import org.wpilib.math.estimator.KalmanFilter; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.numbers.N1; +import org.wpilib.math.util.MathSharedStore; import org.wpilib.math.util.Num; import org.wpilib.math.util.StateSpaceUtil; @@ -128,6 +129,7 @@ public class LinearSystemLoop(new SimpleMatrix(controller.getK().getNumCols(), 1)); reset(m_nextR); + MathSharedStore.getMathShared().reportUsage("LinearSystemLoop", ""); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java index ee265b78e9..bb35f72480 100644 --- a/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java @@ -7,6 +7,7 @@ package org.wpilib.math.trajectory; import java.util.Objects; import org.wpilib.math.trajectory.struct.TrapezoidProfileStateStruct; import org.wpilib.math.util.MathSharedStore; +import org.wpilib.util.struct.StructSerializable; /** * A trapezoid-shaped velocity profile. @@ -75,7 +76,7 @@ public class TrapezoidProfile { } /** Profile state. */ - public static class State { + public static class State implements StructSerializable { /** The struct used to serialize this class. */ public static final TrapezoidProfileStateStruct struct = new TrapezoidProfileStateStruct(); diff --git a/wpimath/src/main/native/cpp/filter/EdgeCounterFilter.cpp b/wpimath/src/main/native/cpp/filter/EdgeCounterFilter.cpp new file mode 100644 index 0000000000..6996602cef --- /dev/null +++ b/wpimath/src/main/native/cpp/filter/EdgeCounterFilter.cpp @@ -0,0 +1,46 @@ +// 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 "wpi/math/filter/EdgeCounterFilter.hpp" + +#include "wpi/math/util/MathShared.hpp" + +using namespace wpi::math; + +EdgeCounterFilter::EdgeCounterFilter(int requiredEdges, units::second_t window) + : m_requiredEdges(requiredEdges), m_windowTime(window) { + ResetTimer(); +} + +void EdgeCounterFilter::ResetTimer() { + m_firstEdgeTime = wpi::math::MathSharedStore::GetTimestamp(); +} + +bool EdgeCounterFilter::HasElapsed() const { + return wpi::math::MathSharedStore::GetTimestamp() - m_firstEdgeTime >= + m_windowTime; +} + +bool EdgeCounterFilter::Calculate(bool input) { + bool enoughEdges = m_currentCount >= m_requiredEdges; + + bool expired = HasElapsed() && !enoughEdges; + bool activationEnded = !input && enoughEdges; + + if (expired || activationEnded) { + m_currentCount = 0; + } + + if (input && !m_lastInput) { + if (m_currentCount == 0) { + ResetTimer(); // Start timer on first rising edge + } + + m_currentCount++; + } + + m_lastInput = input; + + return input && m_currentCount >= m_requiredEdges; +} diff --git a/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp b/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp index 5a849cfd17..65bfe20476 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp @@ -135,6 +135,7 @@ class LinearQuadraticRegulator { } Reset(); + wpi::math::MathSharedStore::ReportUsage("LinearQuadraticRegulator", ""); } /** @@ -194,6 +195,7 @@ class LinearQuadraticRegulator { } Reset(); + wpi::math::MathSharedStore::ReportUsage("LinearQuadraticRegulator", ""); } LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; diff --git a/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp index 3748cdad00..12d29fd7c3 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp @@ -138,6 +138,7 @@ class ExtendedKalmanFilter { m_initP = StateMatrix::Zero(); } m_P = m_initP; + wpi::math::MathSharedStore::ReportUsage("ExtendedKalmanFilter", ""); } /** @@ -222,6 +223,7 @@ class ExtendedKalmanFilter { m_initP = StateMatrix::Zero(); } m_P = m_initP; + wpi::math::MathSharedStore::ReportUsage("ExtendedKalmanFilter", ""); } /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp index d11a230aea..3a6445c02d 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp @@ -117,6 +117,7 @@ class KalmanFilter { } Reset(); + wpi::math::MathSharedStore::ReportUsage("KalmanFilter", ""); } /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp index d093efe096..d73d706be8 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp @@ -74,6 +74,7 @@ class WPILIB_DLLEXPORT PoseEstimator { } SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + wpi::math::MathSharedStore::ReportUsage("PoseEstimator", ""); } /** @@ -88,6 +89,7 @@ class WPILIB_DLLEXPORT PoseEstimator { */ void SetVisionMeasurementStdDevs( const wpi::util::array& visionMeasurementStdDevs) { + // Diagonal of measurement covariance matrix R wpi::util::array r{wpi::util::empty_array}; for (size_t i = 0; i < 3; ++i) { r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i]; @@ -97,9 +99,9 @@ class WPILIB_DLLEXPORT PoseEstimator { // and C = I. See wpimath/algorithms.md. for (size_t row = 0; row < 3; ++row) { if (m_q[row] == 0.0) { - m_visionK(row, row) = 0.0; + m_vision_K.diagonal()[row] = 0.0; } else { - m_visionK(row, row) = + m_vision_K.diagonal()[row] = m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row])); } } @@ -141,24 +143,70 @@ class WPILIB_DLLEXPORT PoseEstimator { * * @param translation The pose to translation to. */ +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif // defined(__GNUC__) && !defined(__clang__) void ResetTranslation(const Translation2d& translation) { m_odometry.ResetTranslation(translation); + + const std::optional> + latestVisionUpdate = + m_visionUpdates.empty() ? std::nullopt + : std::optional{*m_visionUpdates.crbegin()}; m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose(); + + if (latestVisionUpdate) { + // apply vision compensation to the pose rotation + const VisionUpdate visionUpdate{ + Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()}, + Pose2d{translation, + latestVisionUpdate->second.odometryPose.Rotation()}}; + m_visionUpdates[latestVisionUpdate->first] = visionUpdate; + m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose()); + } else { + m_poseEstimate = m_odometry.GetPose(); + } } +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic pop +#endif // defined(__GNUC__) && !defined(__clang__) /** * Resets the robot's rotation. * * @param rotation The rotation to reset to. */ +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif // defined(__GNUC__) && !defined(__clang__) void ResetRotation(const Rotation2d& rotation) { m_odometry.ResetRotation(rotation); + + const std::optional> + latestVisionUpdate = + m_visionUpdates.empty() ? std::nullopt + : std::optional{*m_visionUpdates.crbegin()}; m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose(); + + if (latestVisionUpdate) { + // apply vision compensation to the pose translation + const VisionUpdate visionUpdate{ + Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation}, + Pose2d{latestVisionUpdate->second.odometryPose.Translation(), + rotation}}; + m_visionUpdates[latestVisionUpdate->first] = visionUpdate; + m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose()); + } else { + m_poseEstimate = m_odometry.GetPose(); + } } +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic pop +#endif // defined(__GNUC__) && !defined(__clang__) /** * Gets the estimated robot pose. @@ -274,9 +322,9 @@ class WPILIB_DLLEXPORT PoseEstimator { // this transform by a Kalman gain matrix representing how much we trust // vision measurements compared to our current pose. Eigen::Vector3d k_times_transform = - m_visionK * Eigen::Vector3d{transform.X().value(), - transform.Y().value(), - transform.Rotation().Radians().value()}; + m_vision_K * Eigen::Vector3d{transform.X().value(), + transform.Y().value(), + transform.Rotation().Radians().value()}; // Step 6: Convert back to Transform2d. Transform2d scaledTransform{ @@ -431,14 +479,21 @@ class WPILIB_DLLEXPORT PoseEstimator { static constexpr wpi::units::second_t kBufferDuration = 1.5_s; Odometry& m_odometry; + + // Diagonal of process noise covariance matrix Q wpi::util::array m_q{wpi::util::empty_array}; - Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); + + // Kalman gain matrix K + Eigen::DiagonalMatrix m_vision_K = + Eigen::DiagonalMatrix::Zero(); // Maps timestamps to odometry-only pose estimates TimeInterpolatableBuffer m_odometryPoseBuffer{kBufferDuration}; // Maps timestamps to vision updates // Always contains one entry before the oldest entry in m_odometryPoseBuffer, - // unless there have been no vision measurements after the last reset + // unless there have been no vision measurements after the last reset. May + // contain one entry while m_odometryPoseBuffer is empty to correct for + // translation/rotation after a call to ResetRotation/ResetTranslation. std::map m_visionUpdates; Pose2d m_poseEstimate; diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp index 1cd77c0d9d..c531630383 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp @@ -79,6 +79,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { } SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + wpi::math::MathSharedStore::ReportUsage("PoseEstimator3d", ""); } /** @@ -93,6 +94,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { */ void SetVisionMeasurementStdDevs( const wpi::util::array& visionMeasurementStdDevs) { + // Diagonal of measurement noise covariance matrix R wpi::util::array r{wpi::util::empty_array}; for (size_t i = 0; i < 4; ++i) { r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i]; @@ -102,15 +104,15 @@ class WPILIB_DLLEXPORT PoseEstimator3d { // and C = I. See wpimath/algorithms.md. for (size_t row = 0; row < 4; ++row) { if (m_q[row] == 0.0) { - m_visionK(row, row) = 0.0; + m_vision_K.diagonal()[row] = 0.0; } else { - m_visionK(row, row) = + m_vision_K.diagonal()[row] = m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row])); } } - double angle_gain = m_visionK(3, 3); - m_visionK(4, 4) = angle_gain; - m_visionK(5, 5) = angle_gain; + double angle_gain = m_vision_K.diagonal()[3]; + m_vision_K.diagonal()[4] = angle_gain; + m_vision_K.diagonal()[5] = angle_gain; } /** @@ -149,24 +151,70 @@ class WPILIB_DLLEXPORT PoseEstimator3d { * * @param translation The pose to translation to. */ +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif // defined(__GNUC__) && !defined(__clang__) void ResetTranslation(const Translation3d& translation) { m_odometry.ResetTranslation(translation); + + const std::optional> + latestVisionUpdate = + m_visionUpdates.empty() ? std::nullopt + : std::optional{*m_visionUpdates.crbegin()}; m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose(); + + if (latestVisionUpdate) { + // apply vision compensation to the pose rotation + const VisionUpdate visionUpdate{ + Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()}, + Pose3d{translation, + latestVisionUpdate->second.odometryPose.Rotation()}}; + m_visionUpdates[latestVisionUpdate->first] = visionUpdate; + m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose()); + } else { + m_poseEstimate = m_odometry.GetPose(); + } } +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic pop +#endif // defined(__GNUC__) && !defined(__clang__) /** * Resets the robot's rotation. * * @param rotation The rotation to reset to. */ +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif // defined(__GNUC__) && !defined(__clang__) void ResetRotation(const Rotation3d& rotation) { m_odometry.ResetRotation(rotation); + + const std::optional> + latestVisionUpdate = + m_visionUpdates.empty() ? std::nullopt + : std::optional{*m_visionUpdates.crbegin()}; m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose(); + + if (latestVisionUpdate) { + // apply vision compensation to the pose translation + const VisionUpdate visionUpdate{ + Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation}, + Pose3d{latestVisionUpdate->second.odometryPose.Translation(), + rotation}}; + m_visionUpdates[latestVisionUpdate->first] = visionUpdate; + m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose()); + } else { + m_poseEstimate = m_odometry.GetPose(); + } } +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic pop +#endif // defined(__GNUC__) && !defined(__clang__) /** * Gets the estimated robot pose. @@ -282,12 +330,12 @@ class WPILIB_DLLEXPORT PoseEstimator3d { // this transform by a Kalman gain matrix representing how much we trust // vision measurements compared to our current pose. wpi::math::Vectord<6> k_times_transform = - m_visionK * wpi::math::Vectord<6>{transform.X().value(), - transform.Y().value(), - transform.Z().value(), - transform.Rotation().X().value(), - transform.Rotation().Y().value(), - transform.Rotation().Z().value()}; + m_vision_K * wpi::math::Vectord<6>{transform.X().value(), + transform.Y().value(), + transform.Z().value(), + transform.Rotation().X().value(), + transform.Rotation().Y().value(), + transform.Rotation().Z().value()}; // Step 6: Convert back to Transform3d. Transform3d scaledTransform{ @@ -445,14 +493,21 @@ class WPILIB_DLLEXPORT PoseEstimator3d { static constexpr wpi::units::second_t kBufferDuration = 1.5_s; Odometry3d& m_odometry; + + // Diagonal of process noise covariance matrix Q wpi::util::array m_q{wpi::util::empty_array}; - wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero(); + + // Kalman gain matrix K + Eigen::DiagonalMatrix m_vision_K = + Eigen::DiagonalMatrix::Zero(); // Maps timestamps to odometry-only pose estimates TimeInterpolatableBuffer m_odometryPoseBuffer{kBufferDuration}; // Maps timestamps to vision updates // Always contains one entry before the oldest entry in m_odometryPoseBuffer, - // unless there have been no vision measurements after the last reset + // unless there have been no vision measurements after the last reset. May + // contain one entry while m_odometryPoseBuffer is empty to correct for + // translation/rotation after a call to ResetRotation/ResetTranslation. std::map m_visionUpdates; Pose3d m_poseEstimate; diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp index a748f5bc22..613c0a3eb3 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp @@ -148,6 +148,7 @@ class SteadyStateKalmanFilter { } Reset(); + wpi::math::MathSharedStore::ReportUsage("KalmanFilter", ""); } SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default; diff --git a/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp index 3628cb3c67..b8afbe7433 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp @@ -15,6 +15,7 @@ #include "wpi/math/system/Discretization.hpp" #include "wpi/math/system/NumericalIntegration.hpp" #include "wpi/math/system/NumericalJacobian.hpp" +#include "wpi/math/util/MathShared.hpp" #include "wpi/math/util/StateSpaceUtil.hpp" #include "wpi/units/time.hpp" #include "wpi/util/array.hpp" @@ -114,6 +115,7 @@ class UnscentedKalmanFilter { m_dt = dt; Reset(); + wpi::math::MathSharedStore::ReportUsage("UnscentedKalmanFilter", ""); } /** @@ -173,6 +175,7 @@ class UnscentedKalmanFilter { m_dt = dt; Reset(); + wpi::math::MathSharedStore::ReportUsage("UnscentedKalmanFilter", ""); } /** diff --git a/wpimath/src/main/native/include/wpi/math/filter/EdgeCounterFilter.hpp b/wpimath/src/main/native/include/wpi/math/filter/EdgeCounterFilter.hpp new file mode 100644 index 0000000000..ce4dbed6b3 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/filter/EdgeCounterFilter.hpp @@ -0,0 +1,94 @@ +// 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. + +#pragma once + +#include "wpi/units/time.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::math { +/** + * A rising edge counter for boolean streams. Requires that the boolean change + * value to true for a specified number of times within a specified time window + * after the first rising edge before the filtered value changes. + * + * The filter activates when the input has risen (transitioned from false to + * true) the required number of times within the time window. Once activated, + * the output remains true as long as the input is true. The edge count resets + * when the time window expires or when the input goes false after activation. + * + * Input must be stable; consider using a Debouncer before this filter to avoid + * counting noise as multiple edges. + */ +class WPILIB_DLLEXPORT EdgeCounterFilter { + public: + /** + * Creates a new EdgeCounterFilter. + * + * @param requiredEdges The number of rising edges required before the output + * goes true. + * @param windowTime The maximum time window in which all required edges must + * occur after the first rising edge. + */ + explicit EdgeCounterFilter(int requiredEdges, + wpi::units::second_t windowTime); + + /** + * Applies the edge counter filter to the input stream. + * + * @param input The current value of the input stream. + * @return True if the required number of edges have occurred within the time + * window and the input is currently true; false otherwise. + */ + bool Calculate(bool input); + + /** + * Sets the time window duration. + * + * @param windowTime The maximum time window in which all required edges must + * occur after the first rising edge. + */ + constexpr void SetWindowTime(wpi::units::second_t windowTime) { + m_windowTime = windowTime; + } + + /** + * Gets the time window duration. + * + * @return The maximum time window in which all required edges must occur + * after the first rising edge. + */ + constexpr wpi::units::second_t GetWindowTime() const { return m_windowTime; } + + /** + * Sets the required number of edges. + * + * @param requiredEdges The number of rising edges required before the output + * goes true. + */ + constexpr void SetRequiredEdges(int requiredEdges) { + m_requiredEdges = requiredEdges; + } + + /** + * Gets the required number of edges. + * + * @return The number of rising edges required before the output goes true. + */ + constexpr int GetRequiredEdges() const { return m_requiredEdges; } + + private: + int m_requiredEdges; + wpi::units::second_t m_windowTime; + + wpi::units::second_t m_firstEdgeTime; + int m_currentCount = 0; + + bool m_lastInput = false; + + void ResetTimer(); + + bool HasElapsed() const; +}; +} // namespace wpi::math diff --git a/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp b/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp index ff3fd7ebbd..353a76faa3 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp @@ -85,7 +85,8 @@ class WPILIB_DLLEXPORT CoordinateSystem { constexpr static Translation3d Convert(const Translation3d& translation, const CoordinateSystem& from, const CoordinateSystem& to) { - return translation.RotateBy(from.m_rotation - to.m_rotation); + // Convert to NWU, then convert to the new coordinate system + return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation); } /** @@ -99,7 +100,8 @@ class WPILIB_DLLEXPORT CoordinateSystem { constexpr static Rotation3d Convert(const Rotation3d& rotation, const CoordinateSystem& from, const CoordinateSystem& to) { - return rotation.RotateBy(from.m_rotation - to.m_rotation); + // Convert to NWU, then convert to the new coordinate system + return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation); } /** @@ -128,14 +130,30 @@ class WPILIB_DLLEXPORT CoordinateSystem { constexpr static Transform3d Convert(const Transform3d& transform, const CoordinateSystem& from, const CoordinateSystem& to) { - const auto coordRot = from.m_rotation - to.m_rotation; + // coordRot is the rotation that converts between the coordinate systems + // when applied extrinsically. It first converts to NWU, then converts to + // the new coordinate system. + const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation); + // The new rotation is the extrinsic rotation from convert(zero) to + // convert(transformRot). That is, applying convertedRot extrinsically to + // convert(zero) produces convert(transformRot). In the below snippet, we + // use matrix notation, so rotA rotB applies rotA extrinsically to rotB. + // + // convertedRot convert(zero) = convert(transformRot) + // convertedRot = convert(transformRot) convert(zero)⁻¹ + // = (coordRot transformRot) (coordRot zero)⁻¹ + // = (coordRot transformRot) coordRot⁻¹ + // + // In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the + // change in order), and the equivalent for rot⁻¹ is -rot. return Transform3d{ Convert(transform.Translation(), from, to), (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))}; } private: - // Rotation from this coordinate system to NWU coordinate system + // Rotation from this coordinate system to NWU coordinate system when applied + // extrinsically Rotation3d m_rotation; }; diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp index 3e2c3b8661..d716def254 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp @@ -286,8 +286,8 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const { constexpr Pose2d Pose2d::TransformBy( const wpi::math::Transform2d& other) const { - return {m_translation + other.Translation().RotateBy(m_rotation), - other.Rotation() + m_rotation}; + return {m_translation + (other.Translation().RotateBy(m_rotation)), + other.Rotation().RotateBy(m_rotation)}; } constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const { diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp index c19d786e11..9963ef4150 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp @@ -312,8 +312,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const { } constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const { - return {m_translation + other.Translation().RotateBy(m_rotation), - other.Rotation() + m_rotation}; + // Rotating the transform's rotation by the pose's rotation extrinsically is + // equivalent to rotating the pose's rotation by the transform's rotation + // intrinsically. (We define transforms as being applied intrinsically.) + return {m_translation + (other.Translation().RotateBy(m_rotation)), + other.Rotation().RotateBy(m_rotation)}; } constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const { diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp index d4897bc93d..8f77a2e0e3 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp @@ -190,6 +190,19 @@ class WPILIB_DLLEXPORT Rotation2d { Cos() * other.Sin() + Sin() * other.Cos()}; } + /** + * Returns the current rotation relative to the given rotation. + * + * @param other The rotation describing the orientation of the new coordinate + * frame that the current rotation will be converted into. + * + * @return The current rotation relative to the new orientation of the + * coordinate frame. + */ + constexpr Rotation2d RelativeTo(const Rotation2d& other) const { + return RotateBy(-other); + } + /** * Returns matrix representation of this rotation. */ diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp index 5a1d9368b8..f1430ad744 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp @@ -15,13 +15,56 @@ #include "wpi/math/linalg/ct_matrix.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/math.hpp" +#include "wpi/util/MathExtras.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/json_fwd.hpp" namespace wpi::math { /** - * A rotation in a 3D coordinate frame represented by a quaternion. + * A rotation in a 3D coordinate frame, represented by a quaternion. Note that + * unlike 2D rotations, 3D rotations are not always commutative, so changing the + * order of rotations changes the result. + * + * As an example of the order of rotations mattering, suppose we have a card + * that looks like this: + * + * 

+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ *
+ * + * If we rotate 90º clockwise around the axis into the page, then flip around + * the left/right axis, we get one result: + * + * 
+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ *
+ * + * If we flip around the left/right axis, then rotate 90º clockwise around the + * axis into the page, we get a different result: + * + * 
+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ *
+ * + * Because order matters for 3D rotations, we need to distinguish between + * extrinsic rotations and intrinsic rotations. Rotating + * extrinsically means rotating around the global axes, while rotating + * intrinsically means rotating from the perspective of the other rotation. A + * neat property is that applying a series of rotations extrinsically is the + * same as applying the same series in the opposite order intrinsically. */ class WPILIB_DLLEXPORT Rotation3d { public: @@ -240,9 +283,18 @@ class WPILIB_DLLEXPORT Rotation3d { : Rotation3d{0_rad, 0_rad, rotation.Radians()} {} /** - * Adds two rotations together. + * Adds two rotations together. The other rotation is applied extrinsically to + * this rotation, which is equivalent to this rotation being applied + * intrinsically to the other rotation. See the class comment for definitions + * of extrinsic and intrinsic rotations. * - * @param other The rotation to add. + * Note that `a - b + b` always equals `a`, but `b + (a - b)` + * sometimes doesn't. To apply a rotation offset, use either `offset = + * -measurement + actual; newAngle = angle + offset;` or `offset = actual - + * measurement; newAngle = offset + angle;`, depending on how the corrected + * angle needs to change as the input angle changes. + * + * @param other The rotation to add (applied extrinsically). * * @return The sum of the two rotations. */ @@ -252,11 +304,20 @@ class WPILIB_DLLEXPORT Rotation3d { /** * Subtracts the new rotation from the current rotation and returns the new - * rotation. + * rotation. The new rotation is from the perspective of the other rotation + * (like Pose3d::operator-), so it needs to be applied intrinsically. See the + * class comment for definitions of extrinsic and intrinsic rotations. + * + * Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes + * doesn't. To apply a rotation offset, use either `offset = -measurement + + * actual; newAngle = angle + offset;` or `offset = actual - measurement; + * newAngle = offset + angle;`, depending on how the corrected angle needs to + * change as the input angle changes. * * @param other The rotation to subtract. * - * @return The difference between the two rotations. + * @return The difference between the two rotations, from the perspective of + * the other rotation. */ constexpr Rotation3d operator-(const Rotation3d& other) const { return *this + -other; @@ -323,6 +384,28 @@ class WPILIB_DLLEXPORT Rotation3d { return Rotation3d{other.m_q * m_q}; } + /** + * Returns the current rotation relative to the given rotation. Because the + * result is in the perspective of the given rotation, it must be applied + * intrinsically. See the class comment for definitions of extrinsic and + * intrinsic rotations. + * + * @param other The rotation describing the orientation of the new coordinate + * frame that the current rotation will be converted into. + * + * @return The current rotation relative to the new orientation of the + * coordinate frame. + */ + constexpr Rotation3d RelativeTo(const Rotation3d& other) const { + // To apply a quaternion intrinsically, we must right-multiply by that + // quaternion. Therefore, "this_q relative to other_q" is the q such that + // other_q q = this_q: + // + // other_q q = this_q + // q = other_q⁻¹ this_q + return Rotation3d{other.m_q.Inverse() * m_q}; + } + /** * Returns the quaternion representation of the Rotation3d. */ @@ -450,5 +533,12 @@ void from_json(const wpi::util::json& json, Rotation3d& rotation); } // namespace wpi::math +template <> +constexpr wpi::math::Rotation3d wpi::util::Lerp( + const wpi::math::Rotation3d& startValue, + const wpi::math::Rotation3d& endValue, double t) { + return (endValue - startValue) * t + startValue; +} + #include "wpi/math/geometry/proto/Rotation3dProto.hpp" #include "wpi/math/geometry/struct/Rotation3dStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp index 5c6be18b03..e828b31108 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp @@ -177,13 +177,12 @@ class WPILIB_DLLEXPORT Transform2d { namespace wpi::math { constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) { - // We are rotating the difference between the translations - // using a clockwise rotation matrix. This transforms the global - // delta into a local delta (relative to the initial pose). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = (final.Translation() - initial.Translation()) .RotateBy(-initial.Rotation()); - m_rotation = final.Rotation() - initial.Rotation(); + m_rotation = final.Rotation().RelativeTo(initial.Rotation()); } constexpr Transform2d Transform2d::operator+(const Transform2d& other) const { diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp index f88a032d1e..a353c64e5e 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp @@ -16,7 +16,9 @@ class Pose3d; struct Twist3d; /** - * Represents a transformation for a Pose3d in the pose's frame. + * Represents a transformation for a Pose3d in the pose's frame. Translation is + * applied before rotation. (The translation is applied in the pose's original + * frame, not the transformed frame.) */ class WPILIB_DLLEXPORT Transform3d { public: @@ -202,13 +204,12 @@ class WPILIB_DLLEXPORT Transform3d { namespace wpi::math { constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) { - // We are rotating the difference between the translations - // using a clockwise rotation matrix. This transforms the global - // delta into a local delta (relative to the initial pose). + // To transform the global translation delta to be relative to the initial + // pose, rotate by the inverse of the initial pose's orientation. m_translation = (final.Translation() - initial.Translation()) .RotateBy(-initial.Rotation()); - m_rotation = final.Rotation() - initial.Rotation(); + m_rotation = final.Rotation().RelativeTo(initial.Rotation()); } constexpr Transform3d Transform3d::operator+(const Transform3d& other) const { diff --git a/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp b/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp index 794706ab04..deb6043570 100644 --- a/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp +++ b/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp @@ -11,6 +11,8 @@ #include #include "wpi/math/geometry/Pose2d.hpp" +#include "wpi/math/geometry/Pose3d.hpp" +#include "wpi/math/geometry/Rotation3d.hpp" #include "wpi/units/time.hpp" #include "wpi/util/MathExtras.hpp" @@ -155,7 +157,8 @@ class TimeInterpolatableBuffer { std::function m_interpolatingFunc; }; -// Template specialization to ensure that Pose2d uses pose exponential +// Template specializations to ensure that Pose2d and Pose3d use pose +// exponential template <> inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( wpi::units::second_t historySize) @@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( } }) {} +template <> +inline TimeInterpolatableBuffer::TimeInterpolatableBuffer( + wpi::units::second_t historySize) + : m_historySize(historySize), + m_interpolatingFunc([](const Pose3d& start, const Pose3d& end, double t) { + if (t < 0) { + return start; + } else if (t >= 1) { + return end; + } else { + Twist3d twist = (end - start).Log(); + Twist3d scaledTwist = twist * t; + return start + scaledTwist.Exp(); + } + }) {} + } // namespace wpi::math diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp index 478bed1d90..4fda914b0d 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp @@ -47,7 +47,7 @@ class WPILIB_DLLEXPORT Odometry { m_pose(initialPose), m_previousWheelPositions(wheelPositions) { m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); } /** @@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT Odometry { const WheelPositions& wheelPositions, const Pose2d& pose) { m_pose = pose; m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); m_previousWheelPositions = wheelPositions; } @@ -74,7 +74,8 @@ class WPILIB_DLLEXPORT Odometry { * @param pose The pose to reset to. */ void ResetPose(const Pose2d& pose) { - m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + m_gyroOffset = + m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation()); m_pose = pose; m_previousAngle = pose.Rotation(); } @@ -94,7 +95,7 @@ class WPILIB_DLLEXPORT Odometry { * @param rotation The rotation to reset to. */ void ResetRotation(const Rotation2d& rotation) { - m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation); m_pose = Pose2d{m_pose.Translation(), rotation}; m_previousAngle = rotation; } @@ -118,7 +119,7 @@ class WPILIB_DLLEXPORT Odometry { */ const Pose2d& Update(const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { - auto angle = gyroAngle + m_gyroOffset; + auto angle = gyroAngle.RotateBy(m_gyroOffset); auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions); @@ -139,7 +140,10 @@ class WPILIB_DLLEXPORT Odometry { Pose2d m_pose; WheelPositions m_previousWheelPositions; + + // Always equal to m_pose.Rotation() Rotation2d m_previousAngle; + Rotation2d m_gyroOffset; }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp index f059633422..fca50ffeb2 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp @@ -47,7 +47,9 @@ class WPILIB_DLLEXPORT Odometry3d { m_pose(initialPose), m_previousWheelPositions(wheelPositions) { m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + // When applied extrinsically, m_gyroOffset cancels the + // current gyroAngle and then rotates to m_pose.Rotation() + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); } /** @@ -64,7 +66,9 @@ class WPILIB_DLLEXPORT Odometry3d { const WheelPositions& wheelPositions, const Pose3d& pose) { m_pose = pose; m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + // When applied extrinsically, m_gyroOffset cancels the + // current gyroAngle and then rotates to m_pose.Rotation() + m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); m_previousWheelPositions = wheelPositions; } @@ -74,7 +78,9 @@ class WPILIB_DLLEXPORT Odometry3d { * @param pose The pose to reset to. */ void ResetPose(const Pose3d& pose) { - m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = + m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation()); m_pose = pose; m_previousAngle = pose.Rotation(); } @@ -94,7 +100,8 @@ class WPILIB_DLLEXPORT Odometry3d { * @param rotation The rotation to reset to. */ void ResetRotation(const Rotation3d& rotation) { - m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + // Cancel the previous m_pose.Rotation() and then rotate to the new angle + m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation); m_pose = Pose3d{m_pose.Translation(), rotation}; m_previousAngle = rotation; } @@ -118,7 +125,7 @@ class WPILIB_DLLEXPORT Odometry3d { */ const Pose3d& Update(const Rotation3d& gyroAngle, const WheelPositions& wheelPositions) { - auto angle = gyroAngle + m_gyroOffset; + auto angle = gyroAngle.RotateBy(m_gyroOffset); auto angle_difference = (angle - m_previousAngle).ToVector(); auto twist2d = @@ -145,7 +152,14 @@ class WPILIB_DLLEXPORT Odometry3d { Pose3d m_pose; WheelPositions m_previousWheelPositions; + + // Always equal to m_pose.Rotation() Rotation3d m_previousAngle; + + // Applying a rotation intrinsically to the measured gyro angle should cause + // the corrected angle to be rotated intrinsically in the same way, so the + // measured gyro angle must be applied intrinsically. This is equivalent to + // applying the offset extrinsically to the measured gyro angle. Rotation3d m_gyroOffset; }; diff --git a/wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp b/wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp index fc99061e5a..a492d9f54b 100644 --- a/wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp +++ b/wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp @@ -131,6 +131,7 @@ class LinearSystemLoop { m_clampFunc(clampFunction) { m_nextR.setZero(); Reset(m_nextR); + wpi::math::MathSharedStore::ReportUsage("LinearSystemLoop", ""); } LinearSystemLoop(LinearSystemLoop&&) = default; diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java index f77e77612e..270d95c94c 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java @@ -28,6 +28,7 @@ import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.trajectory.Trajectory; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; +import org.wpilib.math.util.MathSharedStore; class SwerveDrivePoseEstimator3dTest { private static final double kEpsilon = 1e-9; @@ -499,11 +500,23 @@ class SwerveDrivePoseEstimator3dTest { () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + // Add a vision measurement with a different translation + estimator.addVisionMeasurement( + new Pose3d(3, 0, 0, Rotation3d.kZero), MathSharedStore.getTimestamp()); + + assertAll( + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + // Test reset rotation estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2)); assertAll( - () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), @@ -523,7 +536,7 @@ class SwerveDrivePoseEstimator3dTest { } assertAll( - () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), @@ -532,6 +545,22 @@ class SwerveDrivePoseEstimator3dTest { assertEquals( Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + // Add a vision measurement with a different rotation + estimator.addVisionMeasurement( + new Pose3d(2.5, 1, 0, new Rotation3d(Rotation2d.kPi)), MathSharedStore.getTimestamp()); + + assertAll( + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> + assertEquals( + Math.PI * 3.0 / 4, + estimator.getEstimatedPosition().getRotation().getZ(), + kEpsilon)); + // Test reset translation estimator.resetTranslation(new Translation3d(-1, -1, -1)); @@ -543,7 +572,9 @@ class SwerveDrivePoseEstimator3dTest { () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + Math.PI * 3.0 / 4, + estimator.getEstimatedPosition().getRotation().getZ(), + kEpsilon)); // Test reset pose estimator.resetPose(Pose3d.kZero); diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java index 39275c5c11..0c36e72f00 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -25,6 +25,7 @@ import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.trajectory.Trajectory; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; +import org.wpilib.math.util.MathSharedStore; class SwerveDrivePoseEstimatorTest { private static final double kEpsilon = 1e-9; @@ -470,11 +471,21 @@ class SwerveDrivePoseEstimatorTest { () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + // Add a vision measurement with a different translation + estimator.addVisionMeasurement( + new Pose2d(3, 0, Rotation2d.kZero), MathSharedStore.getTimestamp()); + + assertAll( + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + // Test reset rotation estimator.resetRotation(Rotation2d.kCCW_Pi_2); assertAll( - () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), () -> assertEquals( @@ -493,7 +504,7 @@ class SwerveDrivePoseEstimatorTest { } assertAll( - () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), () -> assertEquals( @@ -501,6 +512,19 @@ class SwerveDrivePoseEstimatorTest { estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + // Add a vision measurement with a different rotation + estimator.addVisionMeasurement( + new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp()); + + assertAll( + () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI * 3.0 / 4, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + // Test reset translation estimator.resetTranslation(new Translation2d(-1, -1)); @@ -509,7 +533,7 @@ class SwerveDrivePoseEstimatorTest { () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, + Math.PI * 3.0 / 4, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); diff --git a/wpimath/src/test/java/org/wpilib/math/filter/EdgeCounterFilterTest.java b/wpimath/src/test/java/org/wpilib/math/filter/EdgeCounterFilterTest.java new file mode 100644 index 0000000000..c6d49a3ef6 --- /dev/null +++ b/wpimath/src/test/java/org/wpilib/math/filter/EdgeCounterFilterTest.java @@ -0,0 +1,82 @@ +// 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 org.wpilib.math.filter; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.wpilib.util.WPIUtilJNI; + +class EdgeCounterFilterTest { + @BeforeEach + void setUp() { + WPIUtilJNI.enableMockTime(); + WPIUtilJNI.setMockTime(0L); + } + + @AfterEach + void tearDown() { + WPIUtilJNI.setMockTime(0L); + WPIUtilJNI.disableMockTime(); + } + + @Test + void edgeCounterFilterActivatedTest() { + var filter = new EdgeCounterFilter(2, 0.2); + + assertFalse(filter.calculate(true)); // First edge + + WPIUtilJNI.setMockTime(50_000L); + assertFalse(filter.calculate(false)); // First edge ended + + WPIUtilJNI.setMockTime(100_000L); + assertTrue(filter.calculate(true)); // Second edge + + WPIUtilJNI.setMockTime(150_000L); + assertTrue(filter.calculate(true)); // Still true + + WPIUtilJNI.setMockTime(250_000L); + assertTrue(filter.calculate(true)); // Still true + + WPIUtilJNI.setMockTime(300_000L); + assertFalse(filter.calculate(false)); // Input false, should reset + } + + @Test + void edgeCounterFilterExpiredTest() { + var filter = new EdgeCounterFilter(2, 0.2); + + assertFalse(filter.calculate(true)); // First edge + + WPIUtilJNI.setMockTime(50_000L); + filter.calculate(false); // First edge ended + + WPIUtilJNI.setMockTime(250_000L); + assertFalse(filter.calculate(true)); // Second edge after window expired + + WPIUtilJNI.setMockTime(300_000L); + assertFalse(filter.calculate(true)); // Still false + } + + @Test + void edgeCounterFilterParamsTest() { + var filter = new EdgeCounterFilter(2, 0.2); + + assertEquals(filter.getRequiredEdges(), 2); + assertEquals(filter.getWindowTime(), 0.2); + + filter.setRequiredEdges(3); + + assertEquals(filter.getRequiredEdges(), 3); + + filter.setWindowTime(0.5); + + assertEquals(filter.getWindowTime(), 0.5); + } +} diff --git a/wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.java index 3214300e8c..e3eaa5bdfe 100644 --- a/wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.java @@ -59,6 +59,16 @@ class Rotation2dTest { assertEquals(120.0, rot.getDegrees(), kEpsilon); } + @Test + void testRelativeTo() { + var start = Rotation2d.fromDegrees(30.0); + var end = Rotation2d.kCCW_Pi_2; + + var result = end.relativeTo(start); + + assertEquals(60.0, result.getDegrees(), kEpsilon); + } + @Test void testMinus() { var rot1 = Rotation2d.fromDegrees(70.0); diff --git a/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java index 54385ecce5..6ef74d535a 100644 --- a/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java @@ -281,6 +281,22 @@ class Rotation3dTest { assertEquals(expected, rot); } + @Test + void testRelativeTo() { + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var start = new Rotation3d(yAxis, Units.degreesToRadians(-90.0)); + var end = new Rotation3d(zAxis, Units.degreesToRadians(90.0)); + + final var intrinsicAxis = VecBuilder.fill(1.0, 1.0, 1.0); + var expected = new Rotation3d(intrinsicAxis, Units.degreesToRadians(120.0)); + + var result = end.relativeTo(start); + + assertEquals(expected, result); + } + @Test void testMinus() { final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); @@ -409,5 +425,32 @@ class Rotation3dTest { assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon); assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon); assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon); + + // t value of 0 should always produce the start + rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90)); + interpolated = rot1.interpolate(rot2, 0.0); + assertEquals(rot1.getX(), interpolated.getX(), kEpsilon); + assertEquals(rot1.getY(), interpolated.getY(), kEpsilon); + assertEquals(rot1.getZ(), interpolated.getZ(), kEpsilon); + + // The full rotation from rot1 to rot2 to 120 degrees around extrinsic <-1.0, 1.0, 1.0> + var extrinsicAxis = VecBuilder.fill(-1.0, 1.0, 1.0); + rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90)); + assertEquals(rot2, rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(120)))); + interpolated = rot1.interpolate(rot2, 0.5); + var expected = rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(60))); + assertEquals(expected.getX(), interpolated.getX(), kEpsilon); + assertEquals(expected.getY(), interpolated.getY(), kEpsilon); + assertEquals(expected.getZ(), interpolated.getZ(), kEpsilon); + + // t value of 1 should always produce the end + rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90)); + rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90)); + interpolated = rot1.interpolate(rot2, 1.0); + assertEquals(rot2.getX(), interpolated.getX(), kEpsilon); + assertEquals(rot2.getY(), interpolated.getY(), kEpsilon); + assertEquals(rot2.getZ(), interpolated.getZ(), kEpsilon); } } diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveOdometry3dTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveOdometry3dTest.java index 7c319eb7ac..5b830df0df 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveOdometry3dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveOdometry3dTest.java @@ -10,6 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; import org.wpilib.math.util.Units; class DifferentialDriveOdometry3dTest { @@ -45,4 +46,22 @@ class DifferentialDriveOdometry3dTest { () -> assertEquals(pose.getZ(), 0.0, kEpsilon), () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 90.0, kEpsilon)); } + + @Test + void testGyroOffset() { + m_odometry.resetPosition( + new Rotation3d(0, Units.degreesToRadians(5), 0), + 0, + 0, + new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90)))); + var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), 0, 0); + + assertAll( + () -> assertEquals(pose.getX(), 0.0, kEpsilon), + () -> assertEquals(pose.getY(), 0.0, kEpsilon), + () -> assertEquals(pose.getZ(), 0.0, kEpsilon), + () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), kEpsilon), + () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), kEpsilon), + () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), kEpsilon)); + } } diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java index 34b5438c72..da2f34f6cc 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java @@ -288,4 +288,22 @@ class MecanumDriveOdometry3dTest { 0.05, "Incorrect distance travelled"); } + + @Test + void testGyroOffset() { + var wheelPositions = new MecanumDriveWheelPositions(); + m_odometry.resetPosition( + new Rotation3d(0, Units.degreesToRadians(5), 0), + wheelPositions, + new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90)))); + var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), wheelPositions); + + assertAll( + () -> assertEquals(pose.getX(), 0.0, 1e-9), + () -> assertEquals(pose.getY(), 0.0, 1e-9), + () -> assertEquals(pose.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9), + () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9), + () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9)); + } } diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java index b4f30704d9..0c238e49e9 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java @@ -294,4 +294,22 @@ class SwerveDriveOdometry3dTest { assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error"); assertEquals(0.0, maxError, 0.125, "Incorrect max error"); } + + @Test + void testGyroOffset() { + SwerveModulePosition[] modulePositions = {zero, zero, zero, zero}; + m_odometry.resetPosition( + new Rotation3d(0, Units.degreesToRadians(5), 0), + modulePositions, + new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90)))); + var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), modulePositions); + + assertAll( + () -> assertEquals(pose.getX(), 0.0, 1e-9), + () -> assertEquals(pose.getY(), 0.0, 1e-9), + () -> assertEquals(pose.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9), + () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9), + () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9)); + } } diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp index e16f62d4d8..3344c45cf5 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp @@ -498,10 +498,22 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) { EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); + // Add a vision measurement with a different translation + estimator.AddVisionMeasurement( + wpi::math::Pose3d(3_m, 0_m, 0_m, wpi::math::Rotation3d{}), + wpi::math::MathSharedStore::GetTimestamp()); + + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); + // Test reset rotation estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}); - EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); @@ -517,7 +529,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) { modulePosition, modulePosition}); } - EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); @@ -525,6 +537,20 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) { EXPECT_DOUBLE_EQ(std::numbers::pi / 2, estimator.GetEstimatedPosition().Rotation().Z().value()); + // Add a vision measurement with a different rotation + estimator.AddVisionMeasurement( + wpi::math::Pose3d(2.5_m, 1_m, 0_m, + wpi::math::Rotation3d{wpi::math::Rotation2d{180_deg}}), + wpi::math::MathSharedStore::GetTimestamp()); + + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4, + estimator.GetEstimatedPosition().Rotation().Z().value()); + // Test reset translation estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m}); @@ -533,7 +559,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) { EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); - EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset pose diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 527b97ed10..a154cc03ff 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -464,10 +464,20 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { EXPECT_DOUBLE_EQ( 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + // Add a vision measurement with a different translation + estimator.AddVisionMeasurement( + wpi::math::Pose2d{3_m, 0_m, wpi::math::Rotation2d{}}, + wpi::math::MathSharedStore::GetTimestamp()); + + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); + EXPECT_DOUBLE_EQ( + 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + // Test reset rotation estimator.ResetRotation(wpi::math::Rotation2d{90_deg}); - EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); EXPECT_DOUBLE_EQ( std::numbers::pi / 2, @@ -481,19 +491,30 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { modulePosition, modulePosition}); } - EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); EXPECT_DOUBLE_EQ( std::numbers::pi / 2, estimator.GetEstimatedPosition().Rotation().Radians().value()); + // Add a vision measurement with a different rotation + estimator.AddVisionMeasurement( + wpi::math::Pose2d{2.5_m, 1_m, wpi::math::Rotation2d{180_deg}}, + wpi::math::MathSharedStore::GetTimestamp()); + + EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); + EXPECT_DOUBLE_EQ( + std::numbers::pi * 3.0 / 4, + estimator.GetEstimatedPosition().Rotation().Radians().value()); + // Test reset translation estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m}); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, + std::numbers::pi * 3.0 / 4, estimator.GetEstimatedPosition().Rotation().Radians().value()); // Test reset pose diff --git a/wpimath/src/test/native/cpp/filter/EdgeCounterFilterTest.cpp b/wpimath/src/test/native/cpp/filter/EdgeCounterFilterTest.cpp new file mode 100644 index 0000000000..5aabe6a67c --- /dev/null +++ b/wpimath/src/test/native/cpp/filter/EdgeCounterFilterTest.cpp @@ -0,0 +1,77 @@ +// 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 "wpi/math/filter/EdgeCounterFilter.hpp" + +#include + +#include "wpi/units/time.hpp" +#include "wpi/util/timestamp.h" + +static wpi::units::second_t now = 0_s; + +class EdgeCounterFilterTest : public ::testing::Test { + protected: + void SetUp() override { + WPI_SetNowImpl( + [] { return wpi::units::microsecond_t{now}.to(); }); + now = 0_ms; + } + + void TearDown() override { + now = 0_ms; + WPI_SetNowImpl(nullptr); + } +}; + +TEST_F(EdgeCounterFilterTest, EdgeCounterFilterActivated) { + wpi::math::EdgeCounterFilter filter{2, 0.2_s}; + + EXPECT_FALSE(filter.Calculate(true)); // First edge + + now = 50_ms; + EXPECT_FALSE(filter.Calculate(false)); // First edge ended + + now = 100_ms; + EXPECT_TRUE(filter.Calculate(true)); // Second edge + + now = 150_ms; + EXPECT_TRUE(filter.Calculate(true)); // Still true + + now = 250_ms; + EXPECT_TRUE(filter.Calculate(true)); // Still true + + now = 300_ms; + EXPECT_FALSE(filter.Calculate(false)); // Input false, should reset +} + +TEST_F(EdgeCounterFilterTest, EdgeCounterFilterExpired) { + wpi::math::EdgeCounterFilter filter{2, 0.2_s}; + + EXPECT_FALSE(filter.Calculate(true)); // First edge + + now = 50_ms; + filter.Calculate(false); // First edge ended + + now = 250_ms; + EXPECT_FALSE(filter.Calculate(true)); // Second edge after window expired + + now = 300_ms; + EXPECT_FALSE(filter.Calculate(true)); // Still false +} + +TEST_F(EdgeCounterFilterTest, EdgeCounterFilterParams) { + wpi::math::EdgeCounterFilter filter{2, 0.2_s}; + + EXPECT_EQ(filter.GetRequiredEdges(), 2); + EXPECT_EQ(filter.GetWindowTime(), 0.2_s); + + filter.SetRequiredEdges(3); + + EXPECT_EQ(filter.GetRequiredEdges(), 3); + + filter.SetWindowTime(0.5_s); + + EXPECT_EQ(filter.GetWindowTime(), 0.5_s); +} diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 1cedf1ad50..0792c7e1e0 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -41,6 +41,15 @@ TEST(Rotation2dTest, RotateByNonZero) { EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value()); } +TEST(Rotation2dTest, RelativeTo) { + auto start = Rotation2d{30_deg}; + auto end = Rotation2d{90_deg}; + + auto result = end.RelativeTo(start); + + EXPECT_DOUBLE_EQ(60.0, result.Degrees().value()); +} + TEST(Rotation2dTest, Minus) { const auto rot1 = Rotation2d{70_deg}; const auto rot2 = Rotation2d{30_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index f8bc7f4c4d..0b067dd33c 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -261,6 +261,21 @@ TEST(Rotation3dTest, RotateByNonZeroZ) { EXPECT_EQ(expected, rot); } +TEST(Rotation3dTest, RelativeTo) { + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + auto start = Rotation3d{yAxis, -90_deg}; + auto end = Rotation3d{zAxis, 90_deg}; + + const Eigen::Vector3d intrinsicAxis{1.0, 1.0, 1.0}; + auto expected = Rotation3d{intrinsicAxis, 120_deg}; + + auto result = end.RelativeTo(start); + + EXPECT_EQ(expected, result); +} + TEST(Rotation3dTest, Minus) { const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; @@ -379,4 +394,32 @@ TEST(Rotation3dTest, Interpolate) { EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.X()}.value()); EXPECT_DOUBLE_EQ(0.0, wpi::units::degree_t{interpolated.Y()}.value()); EXPECT_DOUBLE_EQ(-175.0, wpi::units::degree_t{interpolated.Z()}.value()); + + // t value of 0 should always produce the start + rot1 = Rotation3d{yAxis, -90_deg}; + rot2 = Rotation3d{zAxis, 90_deg}; + interpolated = wpi::util::Lerp(rot1, rot2, 0.0); + EXPECT_DOUBLE_EQ(rot1.X().value(), interpolated.X().value()); + EXPECT_DOUBLE_EQ(rot1.Y().value(), interpolated.Y().value()); + EXPECT_DOUBLE_EQ(rot1.Z().value(), interpolated.Z().value()); + + // The full rotation from rot1 to rot2 is 120 degrees around extrinsic + // <-1.0, 1.0, 1.0> + const Eigen::Vector3d extrinsicAxis{-1.0, 1.0, 1.0}; + rot1 = Rotation3d{yAxis, -90_deg}; + rot2 = Rotation3d{zAxis, 90_deg}; + EXPECT_EQ(rot2, rot1.RotateBy(Rotation3d{extrinsicAxis, 120_deg})); + interpolated = wpi::util::Lerp(rot1, rot2, 0.5); + auto expected = rot1.RotateBy(Rotation3d{extrinsicAxis, 60_deg}); + EXPECT_DOUBLE_EQ(expected.X().value(), interpolated.X().value()); + EXPECT_DOUBLE_EQ(expected.Y().value(), interpolated.Y().value()); + EXPECT_DOUBLE_EQ(expected.Z().value(), interpolated.Z().value()); + + // t value of 1 should always produce the end + rot1 = Rotation3d{yAxis, -90_deg}; + rot2 = Rotation3d{zAxis, 90_deg}; + interpolated = wpi::util::Lerp(rot1, rot2, 1.0); + EXPECT_NEAR(rot2.X().value(), interpolated.X().value(), 1e-9); + EXPECT_NEAR(rot2.Y().value(), interpolated.Y().value(), 1e-9); + EXPECT_NEAR(rot2.Z().value(), interpolated.Z().value(), 1e-9); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp index 14e3d62293..74b3310775 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp @@ -8,10 +8,10 @@ #include -static constexpr double kEpsilon = 1E-9; - using namespace wpi::math; +static constexpr double kEpsilon = 1E-9; + TEST(DifferentialDriveOdometry3dTest, Initialize) { DifferentialDriveOdometry3d odometry{ wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m, @@ -39,3 +39,20 @@ TEST(DifferentialDriveOdometry3dTest, EncoderDistances) { EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon); EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon); } + +TEST(DifferentialDriveOdometry3dTest, GyroOffset) { + DifferentialDriveOdometry3d odometry{ + wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m, + wpi::math::Pose3d{wpi::math::Translation3d{}, + wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}}; + const auto& pose = + odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m); + + EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, + kEpsilon); +} diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp index 173a6d84df..bb397baf8c 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp @@ -231,3 +231,20 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) { EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); EXPECT_LT(maxError, 0.125); } + +TEST_F(MecanumDriveOdometry3dTest, GyroOffset) { + wpi::math::MecanumDriveWheelPositions wheelPositions; + odometry.ResetPosition( + wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions, + wpi::math::Pose3d{wpi::math::Translation3d{}, + wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}); + auto pose = odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg}, + wheelPositions); + + EXPECT_NEAR(pose.X().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9); +} diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp index e9c473305e..dd721d90ca 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp @@ -225,3 +225,19 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) { EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); EXPECT_LT(maxError, 0.125); } + +TEST_F(SwerveDriveOdometry3dTest, GyroOffset) { + m_odometry.ResetPosition( + wpi::math::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero}, + wpi::math::Pose3d{wpi::math::Translation3d{}, + wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}); + auto pose = m_odometry.Update(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg}, + {zero, zero, zero, zero}); + + EXPECT_NEAR(pose.X().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9); + EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9); + EXPECT_NEAR(wpi::units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9); +} diff --git a/wpiunits/src/main/java/org/wpilib/units/Units.java b/wpiunits/src/main/java/org/wpilib/units/Units.java index 0acf2b3e1c..9b1e41cbea 100644 --- a/wpiunits/src/main/java/org/wpilib/units/Units.java +++ b/wpiunits/src/main/java/org/wpilib/units/Units.java @@ -166,7 +166,13 @@ public final class Units { * A unit of angular velocity equivalent to spinning at a rate of one {@link #Rotations Rotation} * per {@link #Minute}. Motor spec sheets often list maximum speeds in terms of RPM. */ - public static final AngularVelocityUnit RPM = Rotations.per(Minute); + public static final AngularVelocityUnit RotationsPerMinute = Rotations.per(Minute); + + /** + * A unit of angular velocity equivalent to spinning at a rate of one {@link #Rotations Rotation} + * per {@link #Minute}. Motor spec sheets often list maximum speeds in terms of RPM. + */ + public static final AngularVelocityUnit RPM = RotationsPerMinute; // alias /** * The standard SI unit of angular velocity, equivalent to spinning at a rate of one {@link diff --git a/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp b/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp index 58e6a1e4a7..4c97723d08 100644 --- a/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp +++ b/wpiutil/src/main/native/thirdparty/llvm/cpp/llvm/MemoryBuffer.cpp @@ -189,7 +189,7 @@ class MemoryBufferMMapFile : public MB { MappedFileRegion m_mfr; static uint64_t getLegalMapOffset(uint64_t offset) { - return offset & ~(MappedFileRegion::GetAlignment() - 1); + return offset & ~(static_cast(MappedFileRegion::GetAlignment()) - 1); } static uint64_t getLegalMapSize(uint64_t len, uint64_t offset) {