Replace usage of deprecated ChassisSpeeds methods (#916)

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java

@@ -5,7 +5,6 @@
 import edu.wpi.first.math.controller.PIDController;
 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.kinematics.ChassisSpeeds;
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
@@ -17,7 +16,6 @@ public class PPHolonomicDriveController implements PathFollowingController {
   private final PIDController yController;
   private final PIDController rotationController;
 
-  private Translation2d translationError = new Translation2d();
   private boolean isEnabled = true;
 
   private static Supplier<Optional<Rotation2d>> rotationTargetOverride = null;
@@ -98,10 +96,10 @@ public ChassisSpeeds calculateRobotRelativeSpeeds(
     double xFF = targetState.fieldSpeeds.vxMetersPerSecond;
     double yFF = targetState.fieldSpeeds.vyMetersPerSecond;
 
-    this.translationError = currentPose.getTranslation().minus(targetState.pose.getTranslation());
-
     if (!this.isEnabled) {
-      return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, 0, currentPose.getRotation());
+      ChassisSpeeds ret = new ChassisSpeeds(xFF, yFF, 0);
+      ret.toRobotRelativeSpeeds(currentPose.getRotation());
+      return ret;
     }
 
     double xFeedback = this.xController.calculate(currentPose.getX(), targetState.pose.getX());
@@ -127,8 +125,10 @@ public ChassisSpeeds calculateRobotRelativeSpeeds(
       rotationFeedback = rotFeedbackOverride.getAsDouble();
     }
 
-    return ChassisSpeeds.fromFieldRelativeSpeeds(
-        xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, currentPose.getRotation());
+    ChassisSpeeds ret =
+        new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback);
+    ret.toRobotRelativeSpeeds(currentPose.getRotation());
+    return ret;
   }
 
   /**

pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java

@@ -650,9 +650,8 @@ public Optional<PathPlannerTrajectory> getIdealTrajectory(RobotConfig robotConfi
       // The ideal starting state is known, generate the ideal trajectory
       Rotation2d heading = getInitialHeading();
       Translation2d fieldSpeeds = new Translation2d(idealStartingState.velocityMPS(), heading);
-      ChassisSpeeds startingSpeeds =
-          ChassisSpeeds.fromFieldRelativeSpeeds(
-              fieldSpeeds.getX(), fieldSpeeds.getY(), 0.0, idealStartingState.rotation());
+      ChassisSpeeds startingSpeeds = new ChassisSpeeds(fieldSpeeds.getX(), fieldSpeeds.getY(), 0.0);
+      startingSpeeds.toRobotRelativeSpeeds(idealStartingState.rotation());
       idealTrajectory =
           Optional.of(
               generateTrajectory(startingSpeeds, idealStartingState.rotation(), robotConfig));

pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java

@@ -70,7 +70,7 @@ public PathPlannerTrajectory(
 
       // Set the initial module velocities
       ChassisSpeeds fieldStartingSpeeds =
-          ChassisSpeeds.fromRobotRelativeSpeeds(startingSpeeds, states.get(0).pose.getRotation());
+          fromRobotRelativeSpeeds(startingSpeeds, states.get(0).pose.getRotation());
       var initialStates = config.toSwerveModuleStates(fieldStartingSpeeds);
       for (int m = 0; m < config.numModules; m++) {
         states.get(0).moduleStates[m].speedMetersPerSecond = initialStates[m].speedMetersPerSecond;
@@ -91,7 +91,7 @@ public PathPlannerTrajectory(
           new ChassisSpeeds(endSpeedTrans.getX(), endSpeedTrans.getY(), 0.0);
       var endStates =
           config.toSwerveModuleStates(
-              ChassisSpeeds.fromFieldRelativeSpeeds(
+              fromFieldRelativeSpeeds(
                   endFieldSpeeds, states.get(states.size() - 1).pose.getRotation()));
       for (int m = 0; m < config.numModules; m++) {
         states.get(states.size() - 1).moduleStates[m].speedMetersPerSecond =
@@ -136,10 +136,9 @@ public PathPlannerTrajectory(
         state.timeSeconds = prevState.timeSeconds + dt;
 
         ChassisSpeeds prevRobotSpeeds =
-            ChassisSpeeds.fromFieldRelativeSpeeds(
-                prevState.fieldSpeeds, prevState.pose.getRotation());
+            fromFieldRelativeSpeeds(prevState.fieldSpeeds, prevState.pose.getRotation());
         ChassisSpeeds robotSpeeds =
-            ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation());
+            fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation());
         double chassisAccelX =
             (robotSpeeds.vxMetersPerSecond - prevRobotSpeeds.vxMetersPerSecond) / dt;
         double chassisAccelY =
@@ -368,8 +367,8 @@ private static void forwardAccelPass(
       }
 
       ChassisSpeeds chassisAccel =
-          ChassisSpeeds.fromFieldRelativeSpeeds(
-              accelVec.getX(), accelVec.getY(), angularAccel, state.pose.getRotation());
+          new ChassisSpeeds(accelVec.getX(), accelVec.getY(), angularAccel);
+      chassisAccel.toRobotRelativeSpeeds(state.pose.getRotation());
       var accelStates = config.toSwerveModuleStates(chassisAccel);
       for (int m = 0; m < config.numModules; m++) {
         double moduleAcceleration = accelStates[m].speedMetersPerSecond;
@@ -446,9 +445,8 @@ private static void forwardAccelPass(
           maxChassisVel,
           maxChassisAngVel);
 
-      state.fieldSpeeds =
-          ChassisSpeeds.fromRobotRelativeSpeeds(
-              config.toChassisSpeeds(state.moduleStates), state.pose.getRotation());
+      state.fieldSpeeds = config.toChassisSpeeds(state.moduleStates);
+      state.fieldSpeeds.toFieldRelativeSpeeds(state.pose.getRotation());
       state.linearVelocity =
           Math.hypot(state.fieldSpeeds.vxMetersPerSecond, state.fieldSpeeds.vyMetersPerSecond);
     }
@@ -510,9 +508,8 @@ private static void reverseAccelPass(
       }
 
       ChassisSpeeds chassisAccel =
-          ChassisSpeeds.fromFieldRelativeSpeeds(
-              new ChassisSpeeds(accelVec.getX(), accelVec.getY(), angularAccel),
-              state.pose.getRotation());
+          new ChassisSpeeds(accelVec.getX(), accelVec.getY(), angularAccel);
+      chassisAccel.toRobotRelativeSpeeds(state.pose.getRotation());
       var accelStates = config.toSwerveModuleStates(chassisAccel);
       for (int m = 0; m < config.numModules; m++) {
         double moduleAcceleration = accelStates[m].speedMetersPerSecond;
@@ -579,9 +576,8 @@ private static void reverseAccelPass(
           maxChassisVel,
           maxChassisAngVel);
 
-      state.fieldSpeeds =
-          ChassisSpeeds.fromRobotRelativeSpeeds(
-              config.toChassisSpeeds(state.moduleStates), state.pose.getRotation());
+      state.fieldSpeeds = config.toChassisSpeeds(state.moduleStates);
+      state.fieldSpeeds.toFieldRelativeSpeeds(state.pose.getRotation());
       state.linearVelocity =
           Math.hypot(state.fieldSpeeds.vxMetersPerSecond, state.fieldSpeeds.vyMetersPerSecond);
     }
@@ -771,4 +767,24 @@ private static Rotation2d cosineInterpolate(Rotation2d start, Rotation2d end, do
     double t2 = (1.0 - Math.cos(t * Math.PI)) / 2.0;
     return start.interpolate(end, t2);
   }
+
+  // TODO: Remove if ChassisSpeeds factory methods get un-deprecated
+  private static ChassisSpeeds fromRobotRelativeSpeeds(
+      ChassisSpeeds speeds, Rotation2d robotRotation) {
+    ChassisSpeeds ret =
+        new ChassisSpeeds(
+            speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+    ret.toFieldRelativeSpeeds(robotRotation);
+    return ret;
+  }
+
+  // TODO: Remove if ChassisSpeeds factory methods get un-deprecated
+  private static ChassisSpeeds fromFieldRelativeSpeeds(
+      ChassisSpeeds speeds, Rotation2d robotRotation) {
+    ChassisSpeeds ret =
+        new ChassisSpeeds(
+            speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+    ret.toRobotRelativeSpeeds(robotRotation);
+    return ret;
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java

@@ -317,7 +317,7 @@ public SwerveSetpoint generateSetpoint(
             prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond + min_s * dx,
             prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond + min_s * dy,
             prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta);
-    retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt);
+    retSpeeds.discretize(dt);
 
     double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond;
     double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond;