mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
Compare commits
6 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
c89401250f | ||
|
|
8be7720a68 | ||
|
|
21b5389bbe | ||
|
|
9e1258440b | ||
|
|
812a1b8e1a | ||
|
|
18249badc0 |
@@ -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 = "/edu/wpi/first/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;
|
||||
|
||||
@@ -135,6 +135,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 frc
|
||||
|
||||
@@ -156,6 +158,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");
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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 = "/edu/wpi/first/fields/";
|
||||
|
||||
/** Alias to the current game. */
|
||||
public static final Fields kDefaultField = k2025Reefscape;
|
||||
public static final Fields kDefaultField = k2026Rebuilt;
|
||||
|
||||
public final String m_resourceFile;
|
||||
|
||||
|
||||
@@ -17,10 +17,12 @@
|
||||
#include "fields/2023-chargedup.h"
|
||||
#include "fields/2024-crescendo.h"
|
||||
#include "fields/2025-reefscape.h"
|
||||
#include "fields/2026-rebuilt.h"
|
||||
|
||||
using namespace 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,
|
||||
|
||||
12
fieldImages/src/main/native/include/fields/2026-rebuilt.h
Normal file
12
fieldImages/src/main/native/include/fields/2026-rebuilt.h
Normal file
@@ -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 <string_view>
|
||||
|
||||
namespace fields {
|
||||
std::string_view GetResource_2026_rebuilt_json();
|
||||
std::string_view GetResource_2026_field_png();
|
||||
} // namespace fields
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 742 KiB |
@@ -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"
|
||||
}
|
||||
@@ -343,7 +343,7 @@ static bool InputPose(frc::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<float>())},
|
||||
m_height{storage.GetFloat("height", kDefaultHeight.to<float>())},
|
||||
|
||||
@@ -36,6 +36,7 @@ kRobotDriveSwerve_YAGSL = 15
|
||||
kRobotDriveSwerve_CTRE = 16
|
||||
kRobotDriveSwerve_MaxSwerve = 17
|
||||
kRobotDriveSwerve_AdvantageKit = 18
|
||||
kRobotDriveSwerve_EasySwerve = 19
|
||||
kDriverStationCIO_Analog = 1
|
||||
kDriverStationCIO_DigitalIn = 2
|
||||
kDriverStationCIO_DigitalOut = 3
|
||||
@@ -69,7 +70,7 @@ kDashboard_Shuffleboard = 4
|
||||
kDashboard_Elastic = 5
|
||||
kDashboard_LabVIEW = 6
|
||||
kDashboard_AdvantageScope = 7
|
||||
kDashboard_QFRCDashboard = 8
|
||||
kDashboard_QDash = 8
|
||||
kDashboard_FRCWebComponents = 9
|
||||
kDataLogLocation_Onboard = 1
|
||||
kDataLogLocation_USB = 2
|
||||
|
||||
@@ -134,3 +134,4 @@ kResourceType_PoseEstimator3d = 132
|
||||
kResourceType_LinearSystemLoop = 133
|
||||
kResourceType_LumynLabs_ConnectorX = 134
|
||||
kResourceType_LumynLabs_ConnectorXAnimate = 135
|
||||
kResourceType_RevMAXSplineEncoder = 136
|
||||
|
||||
@@ -291,6 +291,8 @@ public final class FRCNetComm {
|
||||
public static final int kResourceType_LumynLabs_ConnectorX = 134;
|
||||
/** kResourceType_LumynLabs_ConnectorXAnimate = 135. */
|
||||
public static final int kResourceType_LumynLabs_ConnectorXAnimate = 135;
|
||||
/** kResourceType_RevMAXSplineEncoder = 136. */
|
||||
public static final int kResourceType_RevMAXSplineEncoder = 136;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -377,6 +379,8 @@ public final class FRCNetComm {
|
||||
public static final int kRobotDriveSwerve_MaxSwerve = 17;
|
||||
/** kRobotDriveSwerve_AdvantageKit = 18. */
|
||||
public static final int kRobotDriveSwerve_AdvantageKit = 18;
|
||||
/** kRobotDriveSwerve_EasySwerve = 19. */
|
||||
public static final int kRobotDriveSwerve_EasySwerve = 19;
|
||||
/** kDriverStationCIO_Analog = 1. */
|
||||
public static final int kDriverStationCIO_Analog = 1;
|
||||
/** kDriverStationCIO_DigitalIn = 2. */
|
||||
@@ -443,8 +447,8 @@ public final class FRCNetComm {
|
||||
public static final int kDashboard_LabVIEW = 6;
|
||||
/** kDashboard_AdvantageScope = 7. */
|
||||
public static final int kDashboard_AdvantageScope = 7;
|
||||
/** kDashboard_QFRCDashboard = 8. */
|
||||
public static final int kDashboard_QFRCDashboard = 8;
|
||||
/** kDashboard_QDash = 8. */
|
||||
public static final int kDashboard_QDash = 8;
|
||||
/** kDashboard_FRCWebComponents = 9. */
|
||||
public static final int kDashboard_FRCWebComponents = 9;
|
||||
/** kDataLogLocation_Onboard = 1. */
|
||||
|
||||
@@ -187,6 +187,7 @@ namespace HALUsageReporting {
|
||||
kResourceType_LinearSystemLoop = 133,
|
||||
kResourceType_LumynLabs_ConnectorX = 134,
|
||||
kResourceType_LumynLabs_ConnectorXAnimate = 135,
|
||||
kResourceType_RevMAXSplineEncoder = 136,
|
||||
};
|
||||
enum tInstances : int32_t {
|
||||
kLanguage_LabVIEW = 1,
|
||||
@@ -227,6 +228,7 @@ namespace HALUsageReporting {
|
||||
kRobotDriveSwerve_CTRE = 16,
|
||||
kRobotDriveSwerve_MaxSwerve = 17,
|
||||
kRobotDriveSwerve_AdvantageKit = 18,
|
||||
kRobotDriveSwerve_EasySwerve = 19,
|
||||
kDriverStationCIO_Analog = 1,
|
||||
kDriverStationCIO_DigitalIn = 2,
|
||||
kDriverStationCIO_DigitalOut = 3,
|
||||
@@ -260,7 +262,7 @@ namespace HALUsageReporting {
|
||||
kDashboard_Elastic = 5,
|
||||
kDashboard_LabVIEW = 6,
|
||||
kDashboard_AdvantageScope = 7,
|
||||
kDashboard_QFRCDashboard = 8,
|
||||
kDashboard_QDash = 8,
|
||||
kDashboard_FRCWebComponents = 9,
|
||||
kDataLogLocation_Onboard = 1,
|
||||
kDataLogLocation_USB = 2,
|
||||
|
||||
@@ -156,6 +156,7 @@ typedef enum
|
||||
kResourceType_LinearSystemLoop = 133,
|
||||
kResourceType_LumynLabs_ConnectorX = 134,
|
||||
kResourceType_LumynLabs_ConnectorXAnimate = 135,
|
||||
kResourceType_RevMAXSplineEncoder = 136,
|
||||
|
||||
// kResourceType_MaximumID = 255,
|
||||
} tResourceType;
|
||||
@@ -200,6 +201,7 @@ typedef enum
|
||||
kRobotDriveSwerve_CTRE = 16,
|
||||
kRobotDriveSwerve_MaxSwerve = 17,
|
||||
kRobotDriveSwerve_AdvantageKit = 18,
|
||||
kRobotDriveSwerve_EasySwerve = 19,
|
||||
kDriverStationCIO_Analog = 1,
|
||||
kDriverStationCIO_DigitalIn = 2,
|
||||
kDriverStationCIO_DigitalOut = 3,
|
||||
@@ -233,7 +235,7 @@ typedef enum
|
||||
kDashboard_Elastic = 5,
|
||||
kDashboard_LabVIEW = 6,
|
||||
kDashboard_AdvantageScope = 7,
|
||||
kDashboard_QFRCDashboard = 8,
|
||||
kDashboard_QDash = 8,
|
||||
kDashboard_FRCWebComponents = 9,
|
||||
kDataLogLocation_Onboard = 1,
|
||||
kDataLogLocation_USB = 2,
|
||||
|
||||
@@ -57,7 +57,8 @@ static double Lerp(units::second_t time,
|
||||
*/
|
||||
static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
|
||||
std::vector<PreparedData> 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<int>(run.voltage.size()) - 1; ++i) {
|
||||
@@ -101,7 +102,7 @@ static void CopyRawData(wpi::StringMap<MotorData>* 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());
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command.button;
|
||||
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.math.filter.Debouncer;
|
||||
import edu.wpi.first.math.filter.EdgeCounterFilter;
|
||||
import edu.wpi.first.wpilibj.event.EventLoop;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
@@ -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.
|
||||
*
|
||||
* <p>This is useful for implementing "double-click" style functionality.
|
||||
*
|
||||
* <p>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());
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
#include <utility>
|
||||
|
||||
#include <frc/filter/Debouncer.h>
|
||||
#include <frc/filter/EdgeCountFilter.h>
|
||||
|
||||
#include "frc2/command/CommandPtr.h"
|
||||
|
||||
@@ -184,6 +185,14 @@ Trigger Trigger::Debounce(units::second_t debounceTime,
|
||||
});
|
||||
}
|
||||
|
||||
Trigger Trigger::MultiPress(int requiredPresses, units::second_t windowTime) {
|
||||
return Trigger(m_loop,
|
||||
[filter = frc::EdgeCounterFilter(requiredPresses, windowTime),
|
||||
condition = m_condition]() mutable {
|
||||
return filter.Calculate(condition());
|
||||
});
|
||||
}
|
||||
|
||||
bool Trigger::Get() const {
|
||||
return m_condition();
|
||||
}
|
||||
|
||||
@@ -284,6 +284,22 @@ class Trigger {
|
||||
frc::Debouncer::DebounceType type =
|
||||
frc::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.
|
||||
*
|
||||
* <p>This is useful for implementing "double-click" style functionality.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
|
||||
@@ -306,10 +306,11 @@ RobotBase::RobotBase() {
|
||||
HAL_Report(HALUsageReporting::kResourceType_Dashboard,
|
||||
HALUsageReporting::kDashboard_AdvantageScope);
|
||||
m_dashboardDetected = true;
|
||||
} else if (event.GetConnectionInfo()->remote_id.starts_with(
|
||||
} else if (event.GetConnectionInfo()->remote_id.starts_with("QDash") ||
|
||||
event.GetConnectionInfo()->remote_id.starts_with(
|
||||
"QFRCDashboard")) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_Dashboard,
|
||||
HALUsageReporting::kDashboard_QFRCDashboard);
|
||||
HALUsageReporting::kDashboard_QDash);
|
||||
m_dashboardDetected = true;
|
||||
} else if (event.GetConnectionInfo()->remote_id.starts_with(
|
||||
"FRC Web Components")) {
|
||||
|
||||
@@ -211,9 +211,9 @@ public abstract class RobotBase implements AutoCloseable {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_Dashboard, tInstances.kDashboard_AdvantageScope);
|
||||
m_dashboardDetected = true;
|
||||
} else if (event.connInfo.remote_id.startsWith("QFRCDashboard")) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_Dashboard, tInstances.kDashboard_QFRCDashboard);
|
||||
} else if (event.connInfo.remote_id.startsWith("QDash")
|
||||
|| event.connInfo.remote_id.startsWith("QFRCDashboard")) {
|
||||
HAL.report(tResourceType.kResourceType_Dashboard, tInstances.kDashboard_QDash);
|
||||
m_dashboardDetected = true;
|
||||
} else if (event.connInfo.remote_id.startsWith("FRC Web Components")) {
|
||||
HAL.report(
|
||||
|
||||
@@ -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 edu.wpi.first.math.filter;
|
||||
|
||||
import edu.wpi.first.math.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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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;
|
||||
}
|
||||
}
|
||||
@@ -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)));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -220,7 +220,7 @@ public class Pose2d implements Interpolatable<Pose2d>, 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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -254,9 +254,12 @@ public class Pose3d implements Interpolatable<Pose3d>, 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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -275,6 +275,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.
|
||||
*
|
||||
|
||||
@@ -26,7 +26,49 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable;
|
||||
import edu.wpi.first.util.struct.StructSerializable;
|
||||
import java.util.Objects;
|
||||
|
||||
/** 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.
|
||||
*
|
||||
* <p>As an example of the order of rotations mattering, suppose we have a card that looks like
|
||||
* this:
|
||||
*
|
||||
* <pre>
|
||||
* ┌───┐ ┌───┐
|
||||
* │ X │ │ x │
|
||||
* Front: │ | │ Back: │ · │
|
||||
* │ | │ │ · │
|
||||
* └───┘ └───┘
|
||||
* </pre>
|
||||
*
|
||||
* <p>If we rotate 90º clockwise around the axis into the page, then flip around the left/right
|
||||
* axis, we get one result:
|
||||
*
|
||||
* <pre>
|
||||
* ┌───┐
|
||||
* │ X │ ┌───────┐ ┌───────┐
|
||||
* │ | │ → │------X│ → │······x│
|
||||
* │ | │ └───────┘ └───────┘
|
||||
* └───┘
|
||||
* </pre>
|
||||
*
|
||||
* <p>If we flip around the left/right axis, then rotate 90º clockwise around the axis into the
|
||||
* page, we get a different result:
|
||||
*
|
||||
* <pre>
|
||||
* ┌───┐ ┌───┐
|
||||
* │ X │ │ · │ ┌───────┐
|
||||
* │ | │ → │ · │ → │x······│
|
||||
* │ | │ │ x │ └───────┘
|
||||
* └───┘ └───┘
|
||||
* </pre>
|
||||
*
|
||||
* <p>Because order matters for 3D rotations, we need to distinguish between <em>extrinsic</em>
|
||||
* rotations and <em>intrinsic</em> 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
|
||||
@@ -286,9 +328,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.
|
||||
* <p>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) {
|
||||
@@ -296,10 +346,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.
|
||||
*
|
||||
* <p>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());
|
||||
@@ -358,6 +417,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.
|
||||
*
|
||||
@@ -562,7 +639,7 @@ public class Rotation3d
|
||||
|
||||
@Override
|
||||
public Rotation3d interpolate(Rotation3d endValue, double t) {
|
||||
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
|
||||
return endValue.minus(this).times(MathUtil.clamp(t, 0, 1)).plus(this);
|
||||
}
|
||||
|
||||
/** Rotation3d protobuf for serialization. */
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -17,7 +17,10 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable;
|
||||
import edu.wpi.first.util.struct.StructSerializable;
|
||||
import java.util.Objects;
|
||||
|
||||
/** 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.
|
||||
@@ -36,15 +39,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());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -24,7 +24,10 @@ public class Odometry<T> {
|
||||
private Pose2d m_poseMeters;
|
||||
|
||||
private Rotation2d m_gyroOffset;
|
||||
|
||||
// Always equal to m_poseMeters.getRotation()
|
||||
private Rotation2d m_previousAngle;
|
||||
|
||||
private final T m_previousWheelPositions;
|
||||
|
||||
/**
|
||||
@@ -42,7 +45,7 @@ public class Odometry<T> {
|
||||
Pose2d initialPoseMeters) {
|
||||
m_kinematics = kinematics;
|
||||
m_poseMeters = initialPoseMeters;
|
||||
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
|
||||
m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation());
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
|
||||
}
|
||||
@@ -60,7 +63,7 @@ public class Odometry<T> {
|
||||
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
|
||||
m_poseMeters = poseMeters;
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
|
||||
m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation());
|
||||
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
|
||||
}
|
||||
|
||||
@@ -70,7 +73,10 @@ public class Odometry<T> {
|
||||
* @param poseMeters The pose to reset to.
|
||||
*/
|
||||
public void resetPose(Pose2d poseMeters) {
|
||||
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
|
||||
m_gyroOffset =
|
||||
m_gyroOffset
|
||||
.rotateBy(m_poseMeters.getRotation().unaryMinus())
|
||||
.rotateBy(poseMeters.getRotation());
|
||||
m_poseMeters = poseMeters;
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
}
|
||||
@@ -90,7 +96,8 @@ public class Odometry<T> {
|
||||
* @param rotation The rotation to reset to.
|
||||
*/
|
||||
public void resetRotation(Rotation2d rotation) {
|
||||
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
|
||||
m_gyroOffset =
|
||||
m_gyroOffset.rotateBy(m_poseMeters.getRotation().unaryMinus()).rotateBy(rotation);
|
||||
m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation);
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
}
|
||||
@@ -115,7 +122,7 @@ public class Odometry<T> {
|
||||
* @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();
|
||||
|
||||
@@ -32,8 +32,15 @@ public class Odometry3d<T> {
|
||||
private final Kinematics<?, T> m_kinematics;
|
||||
private Pose3d m_poseMeters;
|
||||
|
||||
// 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;
|
||||
|
||||
/**
|
||||
@@ -51,7 +58,9 @@ public class Odometry3d<T> {
|
||||
Pose3d initialPoseMeters) {
|
||||
m_kinematics = kinematics;
|
||||
m_poseMeters = initialPoseMeters;
|
||||
m_gyroOffset = m_poseMeters.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_poseMeters.getRotation());
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
|
||||
}
|
||||
@@ -69,7 +78,9 @@ public class Odometry3d<T> {
|
||||
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
|
||||
m_poseMeters = poseMeters;
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
m_gyroOffset = m_poseMeters.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_poseMeters.getRotation());
|
||||
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
|
||||
}
|
||||
|
||||
@@ -79,7 +90,11 @@ public class Odometry3d<T> {
|
||||
* @param poseMeters The pose to reset to.
|
||||
*/
|
||||
public void resetPose(Pose3d poseMeters) {
|
||||
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
|
||||
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
|
||||
m_gyroOffset =
|
||||
m_gyroOffset
|
||||
.rotateBy(m_poseMeters.getRotation().unaryMinus())
|
||||
.rotateBy(poseMeters.getRotation());
|
||||
m_poseMeters = poseMeters;
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
}
|
||||
@@ -99,7 +114,9 @@ public class Odometry3d<T> {
|
||||
* @param rotation The rotation to reset to.
|
||||
*/
|
||||
public void resetRotation(Rotation3d rotation) {
|
||||
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
|
||||
// Cancel the previous m_pose.Rotation() and then rotate to the new angle
|
||||
m_gyroOffset =
|
||||
m_gyroOffset.rotateBy(m_poseMeters.getRotation().unaryMinus()).rotateBy(rotation);
|
||||
m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation);
|
||||
m_previousAngle = m_poseMeters.getRotation();
|
||||
}
|
||||
@@ -124,7 +141,7 @@ public class Odometry3d<T> {
|
||||
* @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);
|
||||
|
||||
46
wpimath/src/main/native/cpp/filter/EdgeCountFilter.cpp
Normal file
46
wpimath/src/main/native/cpp/filter/EdgeCountFilter.cpp
Normal file
@@ -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 "frc/filter/EdgeCountFilter.h"
|
||||
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
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;
|
||||
}
|
||||
94
wpimath/src/main/native/include/frc/filter/EdgeCountFilter.h
Normal file
94
wpimath/src/main/native/include/frc/filter/EdgeCountFilter.h
Normal file
@@ -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/SymbolExports.h>
|
||||
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* 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, 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(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 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;
|
||||
units::second_t m_windowTime;
|
||||
|
||||
units::second_t m_firstEdgeTime;
|
||||
int m_currentCount = 0;
|
||||
|
||||
bool m_lastInput = false;
|
||||
|
||||
void ResetTimer();
|
||||
|
||||
bool HasElapsed() const;
|
||||
};
|
||||
} // namespace frc
|
||||
@@ -86,7 +86,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);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -100,7 +101,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);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -129,14 +131,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;
|
||||
};
|
||||
|
||||
|
||||
@@ -320,7 +320,7 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
|
||||
|
||||
constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
|
||||
return {m_translation + (other.Translation().RotateBy(m_rotation)),
|
||||
other.Rotation() + m_rotation};
|
||||
other.Rotation().RotateBy(m_rotation)};
|
||||
}
|
||||
|
||||
constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
|
||||
|
||||
@@ -392,8 +392,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
|
||||
}
|
||||
|
||||
constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
|
||||
// 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() + m_rotation};
|
||||
other.Rotation().RotateBy(m_rotation)};
|
||||
}
|
||||
|
||||
constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
|
||||
|
||||
@@ -198,6 +198,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.
|
||||
*/
|
||||
|
||||
@@ -10,6 +10,7 @@
|
||||
#include <Eigen/Core>
|
||||
#include <fmt/format.h>
|
||||
#include <gcem.hpp>
|
||||
#include <wpi/MathExtras.h>
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/json_fwd.h>
|
||||
|
||||
@@ -24,7 +25,49 @@
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* 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:
|
||||
*
|
||||
* <pre>
|
||||
* ┌───┐ ┌───┐
|
||||
* │ X │ │ x │
|
||||
* Front: │ | │ Back: │ · │
|
||||
* │ | │ │ · │
|
||||
* └───┘ └───┘
|
||||
* </pre>
|
||||
*
|
||||
* If we rotate 90º clockwise around the axis into the page, then flip around
|
||||
* the left/right axis, we get one result:
|
||||
*
|
||||
* <pre>
|
||||
* ┌───┐
|
||||
* │ X │ ┌───────┐ ┌───────┐
|
||||
* │ | │ → │------X│ → │······x│
|
||||
* │ | │ └───────┘ └───────┘
|
||||
* └───┘
|
||||
* </pre>
|
||||
*
|
||||
* If we flip around the left/right axis, then rotate 90º clockwise around the
|
||||
* axis into the page, we get a different result:
|
||||
*
|
||||
* <pre>
|
||||
* ┌───┐ ┌───┐
|
||||
* │ X │ │ · │ ┌───────┐
|
||||
* │ | │ → │ · │ → │x······│
|
||||
* │ | │ │ x │ └───────┘
|
||||
* └───┘ └───┘
|
||||
* </pre>
|
||||
*
|
||||
* Because order matters for 3D rotations, we need to distinguish between
|
||||
* <em>extrinsic</em> rotations and <em>intrinsic</em> 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:
|
||||
@@ -242,9 +285,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.
|
||||
*/
|
||||
@@ -254,11 +306,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.
|
||||
*/
|
||||
@@ -449,5 +532,11 @@ void from_json(const wpi::json& json, Rotation3d& rotation);
|
||||
|
||||
} // namespace frc
|
||||
|
||||
template <>
|
||||
constexpr frc::Rotation3d wpi::Lerp(const frc::Rotation3d& startValue,
|
||||
const frc::Rotation3d& endValue, double t) {
|
||||
return (endValue - startValue) * t + startValue;
|
||||
}
|
||||
|
||||
#include "frc/geometry/proto/Rotation3dProto.h"
|
||||
#include "frc/geometry/struct/Rotation3dStruct.h"
|
||||
|
||||
@@ -167,13 +167,12 @@ class WPILIB_DLLEXPORT Transform2d {
|
||||
namespace frc {
|
||||
|
||||
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 {
|
||||
|
||||
@@ -16,7 +16,9 @@ namespace frc {
|
||||
class Pose3d;
|
||||
|
||||
/**
|
||||
* 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:
|
||||
@@ -192,13 +194,12 @@ class WPILIB_DLLEXPORT Transform3d {
|
||||
namespace frc {
|
||||
|
||||
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 {
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
#include "frc/geometry/Rotation3d.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
@@ -155,7 +157,8 @@ class TimeInterpolatableBuffer {
|
||||
std::function<T(const T&, const T&, double)> 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<Pose2d>::TimeInterpolatableBuffer(
|
||||
units::second_t historySize)
|
||||
@@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
|
||||
}
|
||||
}) {}
|
||||
|
||||
template <>
|
||||
inline TimeInterpolatableBuffer<Pose3d>::TimeInterpolatableBuffer(
|
||||
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 = start.Log(end);
|
||||
Twist3d scaledTwist = twist * t;
|
||||
return start.Exp(scaledTwist);
|
||||
}
|
||||
}) {}
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -45,7 +45,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());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -62,7 +62,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;
|
||||
}
|
||||
|
||||
@@ -72,7 +72,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();
|
||||
}
|
||||
@@ -92,7 +93,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;
|
||||
}
|
||||
@@ -116,7 +117,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);
|
||||
@@ -136,7 +137,10 @@ class WPILIB_DLLEXPORT Odometry {
|
||||
Pose2d m_pose;
|
||||
|
||||
WheelPositions m_previousWheelPositions;
|
||||
|
||||
// Always equal to m_pose.Rotation()
|
||||
Rotation2d m_previousAngle;
|
||||
|
||||
Rotation2d m_gyroOffset;
|
||||
};
|
||||
|
||||
|
||||
@@ -48,7 +48,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());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -65,7 +67,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;
|
||||
}
|
||||
|
||||
@@ -75,7 +79,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();
|
||||
}
|
||||
@@ -95,7 +101,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;
|
||||
}
|
||||
@@ -119,7 +126,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;
|
||||
};
|
||||
|
||||
|
||||
@@ -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 edu.wpi.first.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 edu.wpi.first.util.WPIUtilJNI;
|
||||
import org.junit.jupiter.api.AfterEach;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -295,4 +295,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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -305,4 +305,22 @@ class SwerveDriveOdometry3dTest {
|
||||
0.0, errorSum / (trajectory.getTotalTimeSeconds() / 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));
|
||||
}
|
||||
}
|
||||
|
||||
75
wpimath/src/test/native/cpp/filter/EdgeCountFilterTest.cpp
Normal file
75
wpimath/src/test/native/cpp/filter/EdgeCountFilterTest.cpp
Normal file
@@ -0,0 +1,75 @@
|
||||
// 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 <gtest/gtest.h>
|
||||
#include <wpi/timestamp.h>
|
||||
|
||||
#include "frc/filter/EdgeCountFilter.h"
|
||||
#include "units/time.h"
|
||||
|
||||
static units::second_t now = 0_s;
|
||||
|
||||
class EdgeCounterFilterTest : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() override {
|
||||
WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
|
||||
now = 0_ms;
|
||||
}
|
||||
|
||||
void TearDown() override {
|
||||
now = 0_ms;
|
||||
WPI_SetNowImpl(nullptr);
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(EdgeCounterFilterTest, EdgeCounterFilterActivated) {
|
||||
frc::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) {
|
||||
frc::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) {
|
||||
frc::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);
|
||||
}
|
||||
@@ -42,6 +42,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};
|
||||
|
||||
@@ -253,6 +253,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};
|
||||
|
||||
@@ -371,4 +386,32 @@ TEST(Rotation3dTest, Interpolate) {
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
|
||||
EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
|
||||
EXPECT_DOUBLE_EQ(-175.0, 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::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::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::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);
|
||||
}
|
||||
|
||||
@@ -38,3 +38,18 @@ 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{
|
||||
frc::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m,
|
||||
frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}};
|
||||
const auto& pose =
|
||||
odometry.Update(frc::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(units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, kEpsilon);
|
||||
}
|
||||
|
||||
@@ -221,3 +221,19 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
|
||||
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
|
||||
EXPECT_LT(maxError, 0.125);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometry3dTest, GyroOffset) {
|
||||
frc::MecanumDriveWheelPositions wheelPositions;
|
||||
odometry.ResetPosition(
|
||||
frc::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions,
|
||||
frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
|
||||
auto pose =
|
||||
odometry.Update(frc::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(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
|
||||
EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
|
||||
EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
|
||||
}
|
||||
|
||||
@@ -222,3 +222,18 @@ 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(
|
||||
frc::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero},
|
||||
frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
|
||||
auto pose = m_odometry.Update(frc::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(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
|
||||
EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
|
||||
EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user