From 2d72e3da19778301140163c7bbeff906d7be8aeb Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Mon, 25 Nov 2024 14:38:18 -0500 Subject: [PATCH] Replace usage of deprecated ChassisSpeeds methods (#916) --- .../PPHolonomicDriveController.java | 14 +++--- .../pathplanner/lib/path/PathPlannerPath.java | 5 +- .../lib/trajectory/PathPlannerTrajectory.java | 48 ++++++++++++------- .../util/swerve/SwerveSetpointGenerator.java | 2 +- 4 files changed, 42 insertions(+), 27 deletions(-) diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java index a44b42dc..f797098f 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java +++ b/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> 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; } /** diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 0ee5dd0f..4bfbb13e 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -650,9 +650,8 @@ public Optional 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)); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java index 8f8565e3..1d56a2c0 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/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; + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java index bd9e6ca8..60a3378d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java +++ b/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;