[wpimath] Move Java classes to edu.wpi.first.math (#3316)

wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java

@@ -4,13 +4,13 @@
 
 package edu.wpi.first.wpilibj;
 
-import edu.wpi.first.wpiutil.math.MathUtil;
+import edu.wpi.first.math.MathUtil;
 
 /**
  * A class that limits the rate of change of an input value. Useful for implementing voltage,
  * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
  * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
- * edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
+ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
  */
 public class SlewRateLimiter {
   private final double m_rateLimit;

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java

@@ -4,10 +4,10 @@
 
 package edu.wpi.first.wpilibj.controller;
 
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
 
 /**
  * This holonomic drive controller can be used to follow trajectories using a holonomic drive train

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java

@@ -6,10 +6,10 @@ package edu.wpi.first.wpilibj.controller;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 
 /** Implements a PID control loop. */
 @SuppressWarnings("PMD.TooManyFields")

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java

@@ -6,10 +6,10 @@ package edu.wpi.first.wpilibj.controller;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpiutil.math.MathUtil;
 
 /**
  * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java

@@ -1,166 +0,0 @@
-// 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.wpilibj.controller;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-
-/**
- * Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
- * to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law
- * in addition to the linear ones we have used so far like PID? If we use the original approach with
- * PID controllers for left and right position and velocity states, the controllers only deal with
- * the local pose. If the robot deviates from the path, there is no way for the controllers to
- * correct and the robot may not reach the desired global pose. This is due to multiple endpoints
- * existing for the robot which have the same encoder path arc lengths.
- *
- * <p>Instead of using wheel path arc lengths (which are in the robot's local coordinate frame),
- * nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this
- * extra information to guide a linear reference tracker like the PID controllers back in by
- * adjusting the references of the PID controllers.
- *
- * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear
- * controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y,
- * and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because
- * that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e
- * Mobile per i SErvizi e le TEcnologie").
- *
- * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
- * Engineering in the FIRST Robotics Competition</a> section on Ramsete unicycle controller for a
- * derivation and analysis.
- */
-public class RamseteController {
-  @SuppressWarnings("MemberName")
-  private final double m_b;
-
-  @SuppressWarnings("MemberName")
-  private final double m_zeta;
-
-  private Pose2d m_poseError = new Pose2d();
-  private Pose2d m_poseTolerance = new Pose2d();
-  private boolean m_enabled = true;
-
-  /**
-   * Construct a Ramsete unicycle controller.
-   *
-   * @param b Tuning parameter (b &gt; 0) for which larger values make convergence more aggressive
-   *     like a proportional term.
-   * @param zeta Tuning parameter (0 &lt; zeta &lt; 1) for which larger values provide more damping
-   *     in response.
-   */
-  @SuppressWarnings("ParameterName")
-  public RamseteController(double b, double zeta) {
-    m_b = b;
-    m_zeta = zeta;
-  }
-
-  /**
-   * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7
-   * have been well-tested to produce desirable results.
-   */
-  public RamseteController() {
-    this(2.0, 0.7);
-  }
-
-  /** Returns true if the pose error is within tolerance of the reference. */
-  public boolean atReference() {
-    final var eTranslate = m_poseError.getTranslation();
-    final var eRotate = m_poseError.getRotation();
-    final var tolTranslate = m_poseTolerance.getTranslation();
-    final var tolRotate = m_poseTolerance.getRotation();
-    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
-        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
-        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
-  }
-
-  /**
-   * Sets the pose error which is considered tolerable for use with atReference().
-   *
-   * @param poseTolerance Pose error which is tolerable.
-   */
-  public void setTolerance(Pose2d poseTolerance) {
-    m_poseTolerance = poseTolerance;
-  }
-
-  /**
-   * Returns the next output of the Ramsete controller.
-   *
-   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
-   * trajectory.
-   *
-   * @param currentPose The current pose.
-   * @param poseRef The desired pose.
-   * @param linearVelocityRefMeters The desired linear velocity in meters per second.
-   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public ChassisSpeeds calculate(
-      Pose2d currentPose,
-      Pose2d poseRef,
-      double linearVelocityRefMeters,
-      double angularVelocityRefRadiansPerSecond) {
-    if (!m_enabled) {
-      return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
-    }
-
-    m_poseError = poseRef.relativeTo(currentPose);
-
-    // Aliases for equation readability
-    final double eX = m_poseError.getX();
-    final double eY = m_poseError.getY();
-    final double eTheta = m_poseError.getRotation().getRadians();
-    final double vRef = linearVelocityRefMeters;
-    final double omegaRef = angularVelocityRefRadiansPerSecond;
-
-    double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
-
-    return new ChassisSpeeds(
-        vRef * m_poseError.getRotation().getCos() + k * eX,
-        0.0,
-        omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
-  }
-
-  /**
-   * Returns the next output of the Ramsete controller.
-   *
-   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
-   * trajectory.
-   *
-   * @param currentPose The current pose.
-   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
-    return calculate(
-        currentPose,
-        desiredState.poseMeters,
-        desiredState.velocityMetersPerSecond,
-        desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
-  }
-
-  /**
-   * Enables and disables the controller for troubleshooting purposes.
-   *
-   * @param enabled If the controller is enabled or not.
-   */
-  public void setEnabled(boolean enabled) {
-    m_enabled = enabled;
-  }
-
-  /**
-   * Returns sin(x) / x.
-   *
-   * @param x Value of which to take sinc(x).
-   */
-  @SuppressWarnings("ParameterName")
-  private static double sinc(double x) {
-    if (Math.abs(x) < 1e-9) {
-      return 1.0 - 1.0 / 6.0 * x * x;
-    } else {
-      return Math.sin(x) / x;
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java

@@ -7,11 +7,11 @@ package edu.wpi.first.wpilibj.drive;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 import java.util.StringJoiner;
 
 /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java

@@ -7,11 +7,11 @@ package edu.wpi.first.wpilibj.drive;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 import java.util.StringJoiner;
 
 /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

@@ -7,11 +7,11 @@ package edu.wpi.first.wpilibj.drive;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 import java.util.StringJoiner;
 
 /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.wpilibj.interfaces;
 
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 /** Interface for yaw rate gyros. */
 public interface Gyro extends AutoCloseable {
@@ -50,7 +50,7 @@ public interface Gyro extends AutoCloseable {
   double getRate();
 
   /**
-   * Return the heading of the robot as a {@link edu.wpi.first.wpilibj.geometry.Rotation2d}.
+   * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
    *
    * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
    * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
@@ -61,8 +61,7 @@ public interface Gyro extends AutoCloseable {
    *
    * <p>This heading is based on integration of the returned rate from the gyro.
    *
-   * @return the current heading of the robot as a {@link
-   *     edu.wpi.first.wpilibj.geometry.Rotation2d}.
+   * @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
    */
   default Rotation2d getRotation2d() {
     return Rotation2d.fromDegrees(-getAngle());

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java

@@ -5,8 +5,8 @@
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.AnalogEncoder;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 
 /** Class to control a simulated analog encoder. */
 public class AnalogEncoderSim {

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

@@ -4,21 +4,21 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.NumericalIntegration;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N7;
 
 /**
  * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set
@@ -101,9 +101,9 @@ public class DifferentialDrivetrainSim {
    *
    * @param drivetrainPlant The {@link LinearSystem} representing the robot's drivetrain. This
    *     system can be created with {@link
-   *     edu.wpi.first.wpilibj.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor,
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor,
    *     double, double, double, double, double)} or {@link
-   *     edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
+   *     edu.wpi.first.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
    *     double, double)}.
    * @param driveMotor A {@link DCMotor} representing the drivetrain.
    * @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

@@ -4,14 +4,14 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.NumericalIntegration;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 
 /** Represents a simulated elevator mechanism. */
 public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java

@@ -4,12 +4,12 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
 
 /** Represents a simulated flywheel mechanism. */
 public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

@@ -4,12 +4,12 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
 import org.ejml.MatrixDimensionException;
 import org.ejml.simple.SimpleMatrix;
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java

@@ -4,14 +4,14 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.NumericalIntegration;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 
 /** Represents a simulated single jointed arm mechanism. */
 public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java

@@ -4,10 +4,10 @@
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import java.util.ArrayList;
 import java.util.List;
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java

@@ -4,11 +4,11 @@
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 import java.util.ArrayList;

wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.wpilibj.util;
 
-import edu.wpi.first.wpiutil.math.MathUtil;
+import edu.wpi.first.math.MathUtil;
 import java.util.Objects;
 
 /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.wpilibj.util;
 
-import edu.wpi.first.wpiutil.math.MathUtil;
+import edu.wpi.first.math.MathUtil;
 import java.util.Objects;
 
 /** Represents colors with 8 bits of precision. */