Support java units library (#867)

pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java

@@ -1,5 +1,7 @@
 package com.pathplanner.lib.auto;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
 import com.pathplanner.lib.commands.*;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PathFollowingController;
@@ -9,6 +11,7 @@
 import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -273,6 +276,20 @@ public static Command pathfindToPose(
     return pathfindToPoseCommandBuilder.apply(pose, constraints, goalEndVelocity);
   }
 
+  /**
+   * Build a command to pathfind to a given pose. If not using a holonomic drivetrain, the pose
+   * rotation and rotation delay distance will have no effect.
+   *
+   * @param pose The pose to pathfind to
+   * @param constraints The constraints to use while pathfinding
+   * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose
+   * @return A command to pathfind to a given pose
+   */
+  public static Command pathfindToPose(
+      Pose2d pose, PathConstraints constraints, LinearVelocity goalEndVelocity) {
+    return pathfindToPose(pose, constraints, goalEndVelocity.in(MetersPerSecond));
+  }
+
   /**
    * Build a command to pathfind to a given pose. If not using a holonomic drivetrain, the pose
    * rotation will have no effect.
@@ -304,6 +321,22 @@ public static Command pathfindToPoseFlipped(
         shouldFlipPath);
   }
 
+  /**
+   * Build a command to pathfind to a given pose that will be flipped based on the value of the path
+   * flipping supplier when this command is run. If not using a holonomic drivetrain, the pose
+   * rotation and rotation delay distance will have no effect.
+   *
+   * @param pose The pose to pathfind to. This will be flipped if the path flipping supplier returns
+   *     true
+   * @param constraints The constraints to use while pathfinding
+   * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose
+   * @return A command to pathfind to a given pose
+   */
+  public static Command pathfindToPoseFlipped(
+      Pose2d pose, PathConstraints constraints, LinearVelocity goalEndVelocity) {
+    return pathfindToPoseFlipped(pose, constraints, goalEndVelocity.in(MetersPerSecond));
+  }
+
   /**
    * Build a command to pathfind to a given pose that will be flipped based on the value of the path
    * flipping supplier when this command is run. If not using a holonomic drivetrain, the pose

pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java

@@ -111,7 +111,8 @@ public void initialize() {
 
     if (path.getIdealStartingState() != null) {
       // Check if we match the ideal starting state
-      boolean idealVelocity = Math.abs(linearVel - path.getIdealStartingState().velocity()) <= 0.25;
+      boolean idealVelocity =
+          Math.abs(linearVel - path.getIdealStartingState().velocityMPS()) <= 0.25;
       boolean idealRotation =
           !robotConfig.isHolonomic
               || Math.abs(
@@ -186,7 +187,7 @@ public void end(boolean interrupted) {
 
     // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting
     // the command to smoothly transition into some auto-alignment routine
-    if (!interrupted && path.getGoalEndState().velocity() < 0.1) {
+    if (!interrupted && path.getGoalEndState().velocityMPS() < 0.1) {
       var ff = new DriveFeedforward[robotConfig.numModules];
       for (int m = 0; m < robotConfig.numModules; m++) {
         ff[m] = new DriveFeedforward(0.0, 0.0, 0.0);

pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathPlannerAuto.java

@@ -1,5 +1,8 @@
 package com.pathplanner.lib.commands;
 
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Seconds;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.AutoBuilderException;
 import com.pathplanner.lib.auto.CommandUtil;
@@ -13,6 +16,8 @@
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.Timer;
@@ -163,6 +168,16 @@ public Trigger timeElapsed(double time) {
     return condition(() -> timer.hasElapsed(time));
   }
 
+  /**
+   * Trigger that is high when the given time has elapsed
+   *
+   * @param time The amount of time this auto should run before the trigger is activated
+   * @return timeElapsed trigger
+   */
+  public Trigger timeElapsed(Time time) {
+    return timeElapsed(time.in(Seconds));
+  }
+
   /**
    * Trigger that is high when within a range of time since the start of this auto
    *
@@ -174,6 +189,17 @@ public Trigger timeRange(double startTime, double endTime) {
     return condition(() -> timer.get() >= startTime && timer.get() <= endTime);
   }
 
+  /**
+   * Trigger that is high when within a range of time since the start of this auto
+   *
+   * @param startTime The starting time of the range
+   * @param endTime The ending time of the range
+   * @return timeRange trigger
+   */
+  public Trigger timeRange(Time startTime, Time endTime) {
+    return timeRange(startTime.in(Seconds), endTime.in(Seconds));
+  }
+
   /**
    * Create an EventTrigger that will be polled by this auto instead of globally across all path
    * following commands
@@ -222,6 +248,19 @@ public Trigger nearFieldPosition(Translation2d fieldPosition, double toleranceMe
                 <= toleranceMeters);
   }
 
+  /**
+   * Create a trigger that is high when near a given field position. This field position is not
+   * automatically flipped
+   *
+   * @param fieldPosition The target field position
+   * @param tolerance The position tolerance. The trigger will be high when within this distance
+   *     from the target position
+   * @return nearFieldPosition trigger
+   */
+  public Trigger nearFieldPosition(Translation2d fieldPosition, Distance tolerance) {
+    return nearFieldPosition(fieldPosition, tolerance.in(Meters));
+  }
+
   /**
    * Create a trigger that is high when near a given field position. This field position will be
    * automatically flipped
@@ -246,6 +285,19 @@ public Trigger nearFieldPositionAutoFlipped(
         });
   }
 
+  /**
+   * Create a trigger that is high when near a given field position. This field position will be
+   * automatically flipped
+   *
+   * @param blueFieldPosition The target field position if on the blue alliance
+   * @param tolerance The position tolerance. The trigger will be high when within this distance
+   *     from the target position
+   * @return nearFieldPositionAutoFlipped trigger
+   */
+  public Trigger nearFieldPositionAutoFlipped(Translation2d blueFieldPosition, Distance tolerance) {
+    return nearFieldPositionAutoFlipped(blueFieldPosition, tolerance.in(Meters));
+  }
+
   /**
    * Create a trigger that will be high when the robot is within a given area on the field. These
    * positions will not be automatically flipped

pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java

@@ -1,5 +1,7 @@
 package com.pathplanner.lib.commands;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -16,6 +18,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -81,7 +84,7 @@ public PathfindingCommand(
     Pathfinding.ensureInitialized();
 
     Rotation2d targetRotation = Rotation2d.kZero;
-    double goalEndVel = targetPath.getGlobalConstraints().maxVelocityMps();
+    double goalEndVel = targetPath.getGlobalConstraints().maxVelocityMPS();
     if (targetPath.isChoreoPath()) {
       // Can get() here without issue since all choreo trajectories have ideal trajectories
       PathPlannerTrajectory choreoTraj = targetPath.getIdealTrajectory(robotConfig).orElseThrow();
@@ -162,6 +165,46 @@ public PathfindingCommand(
     HAL.report(tResourceType.kResourceType_PathFindingCommand, instances);
   }
 
+  /**
+   * Constructs a new base pathfinding command that will generate a path towards the given pose.
+   *
+   * @param targetPose the pose to pathfind to, the rotation component is only relevant for
+   *     holonomic drive trains
+   * @param constraints the path constraints to use while pathfinding
+   * @param goalEndVel The goal end velocity when reaching the target pose
+   * @param poseSupplier a supplier for the robot's current pose
+   * @param speedsSupplier a supplier for the robot's current robot relative speeds
+   * @param output Output function that accepts robot-relative ChassisSpeeds and feedforwards for
+   *     each drive motor. If using swerve, these feedforwards will be in FL, FR, BL, BR order. If
+   *     using a differential drive, they will be in L, R order.
+   *     <p>NOTE: These feedforwards are assuming unoptimized module states. When you optimize your
+   *     module states, you will need to reverse the feedforwards for modules that have been flipped
+   * @param controller Path following controller that will be used to follow the path
+   * @param robotConfig The robot configuration
+   * @param requirements the subsystems required by this command
+   */
+  public PathfindingCommand(
+      Pose2d targetPose,
+      PathConstraints constraints,
+      LinearVelocity goalEndVel,
+      Supplier<Pose2d> poseSupplier,
+      Supplier<ChassisSpeeds> speedsSupplier,
+      BiConsumer<ChassisSpeeds, DriveFeedforward[]> output,
+      PathFollowingController controller,
+      RobotConfig robotConfig,
+      Subsystem... requirements) {
+    this(
+        targetPose,
+        constraints,
+        goalEndVel.in(MetersPerSecond),
+        poseSupplier,
+        speedsSupplier,
+        output,
+        controller,
+        robotConfig,
+        requirements);
+  }
+
   /**
    * Constructs a new base pathfinding command that will generate a path towards the given pose.
    *
@@ -215,7 +258,7 @@ public void initialize() {
           new Pose2d(this.targetPath.getPoint(0).position, originalTargetPose.getRotation());
       if (shouldFlipPath.getAsBoolean()) {
         targetPose = FlippingUtil.flipFieldPose(this.originalTargetPose);
-        goalEndState = new GoalEndState(goalEndState.velocity(), targetPose.getRotation());
+        goalEndState = new GoalEndState(goalEndState.velocityMPS(), targetPose.getRotation());
       }
     }
 
@@ -351,7 +394,7 @@ public boolean isFinished() {
 
       double currentVel =
           Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
-      double stoppingDistance = Math.pow(currentVel, 2) / (2 * constraints.maxAccelerationMps());
+      double stoppingDistance = Math.pow(currentVel, 2) / (2 * constraints.maxAccelerationMPSSq());
 
       return currentPose.getTranslation().getDistance(targetPose.getTranslation())
           <= stoppingDistance;
@@ -370,7 +413,7 @@ public void end(boolean interrupted) {
 
     // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting
     // the command to smoothly transition into some auto-alignment routine
-    if (!interrupted && goalEndState.velocity() < 0.1) {
+    if (!interrupted && goalEndState.velocityMPS() < 0.1) {
       var ff = new DriveFeedforward[robotConfig.numModules];
       for (int m = 0; m < robotConfig.numModules; m++) {
         ff[m] = new DriveFeedforward(0.0, 0.0, 0.0);

pathplannerlib/src/main/java/com/pathplanner/lib/config/ModuleConfig.java

@@ -1,6 +1,11 @@
 package com.pathplanner.lib.config;
 
+import static edu.wpi.first.units.Units.*;
+
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
 
 /** Configuration class describing a robot's drive module */
 public class ModuleConfig {
@@ -56,4 +61,34 @@ public ModuleConfig(
         Math.max(
             this.driveMotor.getTorque(Math.min(maxSpeedCurrentDraw, this.driveCurrentLimit)), 0.0);
   }
+
+  /**
+   * Configuration of a robot drive module. This can either be a swerve module, or one side of a
+   * differential drive train.
+   *
+   * @param wheelRadius Radius of the drive wheels.
+   * @param maxDriveVelocity The max speed that the drive motor can reach while actually driving the
+   *     robot at full output.
+   * @param wheelCOF The coefficient of friction between the drive wheel and the carpet. If you are
+   *     unsure, just use a placeholder value of 1.0.
+   * @param driveMotor The DCMotor representing the drive motor gearbox, including gear reduction
+   * @param driveCurrentLimit The current limit of the drive motor
+   * @param numMotors The number of motors per module. For swerve, this is 1. For differential, this
+   *     is usually 2.
+   */
+  public ModuleConfig(
+      Distance wheelRadius,
+      LinearVelocity maxDriveVelocity,
+      double wheelCOF,
+      DCMotor driveMotor,
+      Current driveCurrentLimit,
+      int numMotors) {
+    this(
+        wheelRadius.in(Meters),
+        maxDriveVelocity.in(MetersPerSecond),
+        wheelCOF,
+        driveMotor,
+        driveCurrentLimit.in(Amps),
+        numMotors);
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/config/RobotConfig.java

@@ -1,9 +1,14 @@
 package com.pathplanner.lib.config;
 
+import static edu.wpi.first.units.Units.*;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.*;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Mass;
+import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.wpilibj.Filesystem;
 import java.io.*;
 import org.ejml.simple.SimpleMatrix;
@@ -92,6 +97,29 @@ public RobotConfig(
     }
   }
 
+  /**
+   * Create a robot config object for a HOLONOMIC DRIVE robot
+   *
+   * @param mass The mass of the robot, including bumpers and battery
+   * @param MOI The moment of inertia of the robot
+   * @param moduleConfig The drive module config
+   * @param trackwidthMeters The distance between the left and right side of the drivetrain
+   * @param wheelbaseMeters The distance between the front and back side of the drivetrain
+   */
+  public RobotConfig(
+      Mass mass,
+      MomentOfInertia MOI,
+      ModuleConfig moduleConfig,
+      Distance trackwidthMeters,
+      Distance wheelbaseMeters) {
+    this(
+        mass.in(Kilograms),
+        MOI.in(KilogramSquareMeters),
+        moduleConfig,
+        trackwidthMeters.in(Meters),
+        wheelbaseMeters.in(Meters));
+  }
+
   /**
    * Create a robot config object for a DIFFERENTIAL DRIVE robot
    *
@@ -134,6 +162,23 @@ public RobotConfig(
     }
   }
 
+  /**
+   * Create a robot config object for a DIFFERENTIAL DRIVE robot
+   *
+   * @param mass The mass of the robot, including bumpers and battery
+   * @param MOI The moment of inertia of the robot
+   * @param moduleConfig The drive module config
+   * @param trackwidthMeters The distance between the left and right side of the drivetrain
+   */
+  public RobotConfig(
+      Mass mass, MomentOfInertia MOI, ModuleConfig moduleConfig, Distance trackwidthMeters) {
+    this(
+        mass.in(Kilograms),
+        MOI.in(KilogramSquareMeters),
+        moduleConfig,
+        trackwidthMeters.in(Meters));
+  }
+
   /**
    * Convert robot-relative chassis speeds to an array of swerve module states. This will use
    * differential kinematics for diff drive robots, then convert the wheel speeds to module states.