Duplicated check from mjansen4857#927

PickleFace5 · Dec 19, 2024 · 904ce29 · 904ce29
1 parent 143f84b
commit 904ce29
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/util/swerve.py b/pathplannerlib-python/pathplannerlib/util/swerve.py
@@ -242,9 +242,10 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re
             chassis_force_vec = chassis_force_vec + module_force_vec
 
             # Calculate the torque this module will apply to the chassis
-            angle_to_module = self._config.moduleLocations[m].angle()
-            theta = module_force_vec.angle() - angle_to_module
-            chassis_torque += force_at_carpet * self._config.modulePivotDistance[m] * theta.sin()
+            if not self._epsilonEquals(0.0, module_force_vec.norm()):
+                angle_to_module = self._config.moduleLocations[m].angle()
+                theta = module_force_vec.angle() - angle_to_module
+                chassis_torque += force_at_carpet * self._config.modulePivotDistance[m] * theta.sin()
 
         chassis_accel_vec = chassis_force_vec / self._config.massKG
         chassis_angular_accel = chassis_torque / self._config.MOI