[wpiunits] Java units API rewrite (#6958)

Java generics are too limited to do what we need. This refactors generic code previously in Unit and Measure into unit-specific classes that can have unit-safe math operations (notably, times and divide) that can return values in known units instead of a wildcarded Measure<?>.

Unit-specific measure implementations are automatically generated by ./wpiunits/generate_units.py, which generates generic interfaces and mutable and immutable implementations of those interfaces. These make up the bulk of the diff of this PR (approximately 9300 LOC).

This also adds units for angular and linear velocities, accelerations, and momenta; moment of inertia; and torque.

wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java

@@ -14,11 +14,9 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
 import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
-import edu.wpi.first.units.Angle;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.Time;
-import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -72,10 +70,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
    * @param vy Sideways velocity.
    * @param omega Angular velocity.
    */
-  public ChassisSpeeds(
-      Measure<Velocity<Distance>> vx,
-      Measure<Velocity<Distance>> vy,
-      Measure<Velocity<Angle>> omega) {
+  public ChassisSpeeds(LinearVelocity vx, LinearVelocity vy, AngularVelocity omega) {
     this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
   }
 
@@ -148,10 +143,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
    * @return Discretized ChassisSpeeds.
    */
   public static ChassisSpeeds discretize(
-      Measure<Velocity<Distance>> vx,
-      Measure<Velocity<Distance>> vy,
-      Measure<Velocity<Angle>> omega,
-      Measure<Time> dt) {
+      LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
     return discretize(
         vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), dt.in(Seconds));
   }
@@ -219,10 +211,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
    * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
    */
   public static ChassisSpeeds fromFieldRelativeSpeeds(
-      Measure<Velocity<Distance>> vx,
-      Measure<Velocity<Distance>> vy,
-      Measure<Velocity<Angle>> omega,
-      Rotation2d robotAngle) {
+      LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
     return fromFieldRelativeSpeeds(
         vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
   }
@@ -287,10 +276,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
    * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
    */
   public static ChassisSpeeds fromRobotRelativeSpeeds(
-      Measure<Velocity<Distance>> vx,
-      Measure<Velocity<Distance>> vy,
-      Measure<Velocity<Angle>> omega,
-      Rotation2d robotAngle) {
+      LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
     return fromRobotRelativeSpeeds(
         vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
   }

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java

@@ -11,8 +11,7 @@ import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -58,7 +57,7 @@ public class DifferentialDriveKinematics
    *     between the left wheels and right wheels. However, the empirical value may be larger than
    *     the physical measured value due to scrubbing effects.
    */
-  public DifferentialDriveKinematics(Measure<Distance> trackWidth) {
+  public DifferentialDriveKinematics(Distance trackWidth) {
     this(trackWidth.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java

@@ -10,8 +10,7 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 
 /**
  * Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -55,8 +54,8 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
    */
   public DifferentialDriveOdometry(
       Rotation2d gyroAngle,
-      Measure<Distance> leftDistance,
-      Measure<Distance> rightDistance,
+      Distance leftDistance,
+      Distance rightDistance,
       Pose2d initialPoseMeters) {
     this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
   }
@@ -81,7 +80,7 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
    * @param rightDistance The distance traveled by the right encoder.
    */
   public DifferentialDriveOdometry(
-      Rotation2d gyroAngle, Measure<Distance> leftDistance, Measure<Distance> rightDistance) {
+      Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance) {
     this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero);
   }
 
@@ -119,10 +118,7 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
    * @param poseMeters The position on the field that your robot is at.
    */
   public void resetPosition(
-      Rotation2d gyroAngle,
-      Measure<Distance> leftDistance,
-      Measure<Distance> rightDistance,
-      Pose2d poseMeters) {
+      Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d poseMeters) {
     resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters);
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java

@@ -10,8 +10,7 @@ import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import java.util.Objects;
 
 /** Represents the wheel positions for a differential drive drivetrain. */
@@ -48,7 +47,7 @@ public class DifferentialDriveWheelPositions
    * @param left Distance measured by the left side.
    * @param right Distance measured by the right side.
    */
-  public DifferentialDriveWheelPositions(Measure<Distance> left, Measure<Distance> right) {
+  public DifferentialDriveWheelPositions(Distance left, Distance right) {
     this(left.in(Meters), right.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java

@@ -8,9 +8,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -50,8 +48,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
    * @param left The left speed.
    * @param right The right speed.
    */
-  public DifferentialDriveWheelSpeeds(
-      Measure<Velocity<Distance>> left, Measure<Velocity<Distance>> right) {
+  public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) {
     this(left.in(MetersPerSecond), right.in(MetersPerSecond));
   }
 
@@ -85,7 +82,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
    *
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
-  public void desaturate(Measure<Velocity<Distance>> attainableMaxSpeed) {
+  public void desaturate(LinearVelocity attainableMaxSpeed) {
     desaturate(attainableMaxSpeed.in(MetersPerSecond));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java

@@ -10,8 +10,7 @@ import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelPositionsProto;
 import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelPositionsStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -71,10 +70,7 @@ public class MecanumDriveWheelPositions
    * @param rearRight Distance measured by the rear right wheel.
    */
   public MecanumDriveWheelPositions(
-      Measure<Distance> frontLeft,
-      Measure<Distance> frontRight,
-      Measure<Distance> rearLeft,
-      Measure<Distance> rearRight) {
+      Distance frontLeft, Distance frontRight, Distance rearLeft, Distance rearRight) {
     this(frontLeft.in(Meters), frontRight.in(Meters), rearLeft.in(Meters), rearRight.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java

@@ -8,9 +8,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 
 import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelSpeedsProto;
 import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelSpeedsStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -65,10 +63,10 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
    * @param rearRight Speed of the rear right wheel.
    */
   public MecanumDriveWheelSpeeds(
-      Measure<Velocity<Distance>> frontLeft,
-      Measure<Velocity<Distance>> frontRight,
-      Measure<Velocity<Distance>> rearLeft,
-      Measure<Velocity<Distance>> rearRight) {
+      LinearVelocity frontLeft,
+      LinearVelocity frontRight,
+      LinearVelocity rearLeft,
+      LinearVelocity rearRight) {
     this(
         frontLeft.in(MetersPerSecond),
         frontRight.in(MetersPerSecond),
@@ -114,7 +112,7 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
    *
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
-  public void desaturate(Measure<Velocity<Distance>> attainableMaxSpeed) {
+  public void desaturate(LinearVelocity attainableMaxSpeed) {
     desaturate(attainableMaxSpeed.in(MetersPerSecond));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -14,10 +14,8 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.SwerveDriveKinematicsProto;
 import edu.wpi.first.math.kinematics.struct.SwerveDriveKinematicsStruct;
-import edu.wpi.first.units.Angle;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.Struct;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -307,7 +305,7 @@ public class SwerveDriveKinematics
    * @param attainableMaxSpeed The absolute max speed that a module can reach.
    */
   public static void desaturateWheelSpeeds(
-      SwerveModuleState[] moduleStates, Measure<Velocity<Distance>> attainableMaxSpeed) {
+      SwerveModuleState[] moduleStates, LinearVelocity attainableMaxSpeed) {
     desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
   }
 
@@ -379,9 +377,9 @@ public class SwerveDriveKinematics
   public static void desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
       ChassisSpeeds desiredChassisSpeed,
-      Measure<Velocity<Distance>> attainableMaxModuleSpeed,
-      Measure<Velocity<Distance>> attainableMaxTranslationalSpeed,
-      Measure<Velocity<Angle>> attainableMaxRotationalVelocity) {
+      LinearVelocity attainableMaxModuleSpeed,
+      LinearVelocity attainableMaxTranslationalSpeed,
+      AngularVelocity attainableMaxRotationalVelocity) {
     desaturateWheelSpeeds(
         moduleStates,
         desiredChassisSpeed,

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java

@@ -11,8 +11,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.proto.SwerveModulePositionProto;
 import edu.wpi.first.math.kinematics.struct.SwerveModulePositionStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -55,7 +54,7 @@ public class SwerveModulePosition
    * @param distance The distance measured by the wheel of the module.
    * @param angle The angle of the module.
    */
-  public SwerveModulePosition(Measure<Distance> distance, Rotation2d angle) {
+  public SwerveModulePosition(Distance distance, Rotation2d angle) {
     this(distance.in(Meters), angle);
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -9,9 +9,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto;
 import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -51,7 +49,7 @@ public class SwerveModuleState
    * @param speed The speed of the wheel of the module.
    * @param angle The angle of the module.
    */
-  public SwerveModuleState(Measure<Velocity<Distance>> speed, Rotation2d angle) {
+  public SwerveModuleState(LinearVelocity speed, Rotation2d angle) {
     this(speed.in(MetersPerSecond), angle);
   }