From edfdfd00ca3962959a0f7105a5c614dc1c0d0455 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 13 Dec 2024 21:55:41 -0600 Subject: [PATCH] Prevent force from being applied if the magnitude of the force is 0 This prevents Rotation2d from screaming in the console from both x and y being 0. --- .../lib/util/swerve/SwerveSetpointGenerator.java | 8 +++++--- 1 file changed, 5 insertions(+), 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 index 60a3378d..172dc998 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 @@ -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);