Prevent force from being applied if the magnitude of the force is 0 (#…

…927)
mjansen4857 · Dec 18, 2024 · 3853d48 · 3853d48
1 parent b3b99fd
commit 3853d48
Showing 1 changed file with 5 additions and 3 deletions.
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
@@ -273,9 +273,11 @@ public SwerveSetpoint generateSetpoint(
       chassisForceVec = chassisForceVec.plus(moduleForceVec);
 
       // Calculate the torque this module will apply to the chassis
-      Rotation2d angleToModule = config.moduleLocations[m].getAngle();
-      Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule);
-      chassisTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin();
+      if (!epsilonEquals(0, moduleForceVec.getNorm())) {
+        Rotation2d angleToModule = config.moduleLocations[m].getAngle();
+        Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule);
+        chassisTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin();
+      }
     }
 
     Translation2d chassisAccelVec = chassisForceVec.div(config.massKG);