[wpimath] Add basic wpiunits support (#5821)

To reduce the need for users to manually perform unit conversions, this allows Measure objects from wpiunits to be passed into most places in wpimath that currently expect doubles in terms of SI units like meters.

For example, users would need to know that unit conversion is required - and what the correct units are. Using units would be more difficult to write code for than just hardcoding a value or using Units.inchesToMeters.

Now, using units has no more developer overhead than using raw numbers.

wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java

@@ -8,12 +8,22 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.units.Units;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Pose2dTest {
   private static final double kEpsilon = 1E-9;
 
+  @Test
+  void testNewWithMeasures() {
+    var pose = new Pose2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45));
+
+    assertEquals(0.1524, pose.getX(), kEpsilon);
+    assertEquals(0.2032, pose.getY(), kEpsilon);
+    assertEquals(Math.PI / 4, pose.getRotation().getRadians(), kEpsilon);
+  }
+
   @Test
   void testRotateBy() {
     final double x = 1.0;

wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java

@@ -8,11 +8,19 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.units.Units;
 import org.junit.jupiter.api.Test;
 
 class Rotation2dTest {
   private static final double kEpsilon = 1E-9;
 
+  @Test
+  void testNewWithMeasures() {
+    var rot = new Rotation2d(Units.Degrees.of(45));
+
+    assertEquals(Math.PI / 4, rot.getRadians(), kEpsilon);
+  }
+
   @Test
   void testRadiansToDegrees() {
     var rot1 = Rotation2d.fromRadians(Math.PI / 3);

wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java

@@ -6,9 +6,22 @@ package edu.wpi.first.math.geometry;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.units.Units;
 import org.junit.jupiter.api.Test;
 
 class Transform2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testNewWithMeasures() {
+    var transform =
+        new Transform2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45));
+
+    assertEquals(0.1524, transform.getX(), kEpsilon);
+    assertEquals(0.2032, transform.getY(), kEpsilon);
+    assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon);
+  }
+
   @Test
   void testInverse() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.geometry;
 
+import static edu.wpi.first.units.Units.Inches;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
@@ -15,6 +16,15 @@ import org.junit.jupiter.api.Test;
 class Translation3dTest {
   private static final double kEpsilon = 1E-9;
 
+  @Test
+  void testNewWithMeasures() {
+    var translation = new Translation3d(Inches.of(6), Inches.of(8), Inches.of(16));
+
+    assertEquals(0.1524, translation.getX(), kEpsilon);
+    assertEquals(0.2032, translation.getY(), kEpsilon);
+    assertEquals(0.4064, translation.getZ(), kEpsilon);
+  }
+
   @Test
   void testSum() {
     var one = new Translation3d(1.0, 3.0, 5.0);

wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.RPM;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
@@ -44,6 +46,19 @@ class ChassisSpeedsTest {
                 kEpsilon));
   }
 
+  @Test
+  void testMeasureConstructor() {
+    var vx = InchesPerSecond.of(14.52);
+    var vy = InchesPerSecond.zero();
+    var omega = RPM.of(0.02);
+    var speeds = new ChassisSpeeds(vx, vy, omega);
+
+    assertAll(
+        () -> assertEquals(0.368808, speeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0, speeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.002094395102, speeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
   @Test
   void testFromFieldRelativeSpeeds() {
     final var chassisSpeeds =