Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2026-02-15 00:51:21 -08:00
parent 36c489247c 77dfad97c6
commit af865f8020
98 changed files with 3031 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java
View File

@@ -28,6 +28,7 @@ import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.trajectory.Trajectory;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.MathSharedStore;
class SwerveDrivePoseEstimator3dTest {
private static final double kEpsilon = 1e-9;
@@ -499,11 +500,23 @@ class SwerveDrivePoseEstimator3dTest {
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
// Add a vision measurement with a different translation
estimator.addVisionMeasurement(
new Pose3d(3, 0, 0, Rotation3d.kZero), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
// Test reset rotation
estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -523,7 +536,7 @@ class SwerveDrivePoseEstimator3dTest {
}
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -532,6 +545,22 @@ class SwerveDrivePoseEstimator3dTest {
assertEquals(
Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
// Add a vision measurement with a different rotation
estimator.addVisionMeasurement(
new Pose3d(2.5, 1, 0, new Rotation3d(Rotation2d.kPi)), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() ->
assertEquals(
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getZ(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation3d(-1, -1, -1));
@@ -543,7 +572,9 @@ class SwerveDrivePoseEstimator3dTest {
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getZ(),
kEpsilon));
// Test reset pose
estimator.resetPose(Pose3d.kZero);

30
wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -25,6 +25,7 @@ import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.trajectory.Trajectory;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.MathSharedStore;
class SwerveDrivePoseEstimatorTest {
private static final double kEpsilon = 1e-9;
@@ -470,11 +471,21 @@ class SwerveDrivePoseEstimatorTest {
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Add a vision measurement with a different translation
estimator.addVisionMeasurement(
new Pose2d(3, 0, Rotation2d.kZero), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test reset rotation
estimator.resetRotation(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
@@ -493,7 +504,7 @@ class SwerveDrivePoseEstimatorTest {
}
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
@@ -501,6 +512,19 @@ class SwerveDrivePoseEstimatorTest {
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Add a vision measurement with a different rotation
estimator.addVisionMeasurement(
new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation2d(-1, -1));
@@ -509,7 +533,7 @@ class SwerveDrivePoseEstimatorTest {
() -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));

82
wpimath/src/test/java/org/wpilib/math/filter/EdgeCounterFilterTest.java Normal file
View File

@@ -0,0 +1,82 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.filter;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.wpilib.util.WPIUtilJNI;
class EdgeCounterFilterTest {
@BeforeEach
void setUp() {
WPIUtilJNI.enableMockTime();
WPIUtilJNI.setMockTime(0L);
}
@AfterEach
void tearDown() {
WPIUtilJNI.setMockTime(0L);
WPIUtilJNI.disableMockTime();
}
@Test
void edgeCounterFilterActivatedTest() {
var filter = new EdgeCounterFilter(2, 0.2);
assertFalse(filter.calculate(true)); // First edge
WPIUtilJNI.setMockTime(50_000L);
assertFalse(filter.calculate(false)); // First edge ended
WPIUtilJNI.setMockTime(100_000L);
assertTrue(filter.calculate(true)); // Second edge
WPIUtilJNI.setMockTime(150_000L);
assertTrue(filter.calculate(true)); // Still true
WPIUtilJNI.setMockTime(250_000L);
assertTrue(filter.calculate(true)); // Still true
WPIUtilJNI.setMockTime(300_000L);
assertFalse(filter.calculate(false)); // Input false, should reset
}
@Test
void edgeCounterFilterExpiredTest() {
var filter = new EdgeCounterFilter(2, 0.2);
assertFalse(filter.calculate(true)); // First edge
WPIUtilJNI.setMockTime(50_000L);
filter.calculate(false); // First edge ended
WPIUtilJNI.setMockTime(250_000L);
assertFalse(filter.calculate(true)); // Second edge after window expired
WPIUtilJNI.setMockTime(300_000L);
assertFalse(filter.calculate(true)); // Still false
}
@Test
void edgeCounterFilterParamsTest() {
var filter = new EdgeCounterFilter(2, 0.2);
assertEquals(filter.getRequiredEdges(), 2);
assertEquals(filter.getWindowTime(), 0.2);
filter.setRequiredEdges(3);
assertEquals(filter.getRequiredEdges(), 3);
filter.setWindowTime(0.5);
assertEquals(filter.getWindowTime(), 0.5);
}
}

10
wpimath/src/test/java/org/wpilib/math/geometry/Rotation2dTest.java
View File

@@ -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);

43
wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java
View File

@@ -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);
}
}

19
wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveOdometry3dTest.java
View File

@@ -10,6 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.util.Units;
class DifferentialDriveOdometry3dTest {
@@ -45,4 +46,22 @@ class DifferentialDriveOdometry3dTest {
() -> assertEquals(pose.getZ(), 0.0, kEpsilon),
() -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 90.0, kEpsilon));
}
@Test
void testGyroOffset() {
m_odometry.resetPosition(
new Rotation3d(0, Units.degreesToRadians(5), 0),
0,
0,
new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), 0, 0);
assertAll(
() -> assertEquals(pose.getX(), 0.0, kEpsilon),
() -> assertEquals(pose.getY(), 0.0, kEpsilon),
() -> assertEquals(pose.getZ(), 0.0, kEpsilon),
() -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), kEpsilon),
() -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), kEpsilon),
() -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), kEpsilon));
}
}

18
wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java
View File

@@ -288,4 +288,22 @@ class MecanumDriveOdometry3dTest {
0.05,
"Incorrect distance travelled");
}
@Test
void testGyroOffset() {
var wheelPositions = new MecanumDriveWheelPositions();
m_odometry.resetPosition(
new Rotation3d(0, Units.degreesToRadians(5), 0),
wheelPositions,
new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), wheelPositions);
assertAll(
() -> assertEquals(pose.getX(), 0.0, 1e-9),
() -> assertEquals(pose.getY(), 0.0, 1e-9),
() -> assertEquals(pose.getZ(), 0.0, 1e-9),
() -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9),
() -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9),
() -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9));
}
}

18
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java
View File

@@ -294,4 +294,22 @@ class SwerveDriveOdometry3dTest {
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
}
@Test
void testGyroOffset() {
SwerveModulePosition[] modulePositions = {zero, zero, zero, zero};
m_odometry.resetPosition(
new Rotation3d(0, Units.degreesToRadians(5), 0),
modulePositions,
new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), modulePositions);
assertAll(
() -> assertEquals(pose.getX(), 0.0, 1e-9),
() -> assertEquals(pose.getY(), 0.0, 1e-9),
() -> assertEquals(pose.getZ(), 0.0, 1e-9),
() -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9),
() -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9),
() -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9));
}
}