Fix wheel force/torque feedforward calculations (#801)

pathplannerlib-python/pathplannerlib/config.py

@@ -66,15 +66,16 @@ class RobotConfig:
     moduleConfig: ModuleConfig
 
     moduleLocations: List[Translation2d]
-    diffKinematics: Union[DifferentialDriveKinematics, None]
-    swerveKinematics: Union[SwerveDrive4Kinematics, None]
     isHolonomic: bool
 
     numModules: int
     modulePivotDistance: List[float]
     wheelFrictionForce: float
     maxTorqueFriction: float
 
+    _swerveKinematics: Union[SwerveDrive4Kinematics, None]
+    _diffKinematics: Union[DifferentialDriveKinematics, None]
+
     def __init__(self, massKG: float, MOI: float, moduleConfig: ModuleConfig, trackwidthMeters: float,
                  wheelbaseMeters: float = None):
         """
@@ -95,8 +96,8 @@ def __init__(self, massKG: float, MOI: float, moduleConfig: ModuleConfig, trackw
                 Translation2d(0.0, trackwidthMeters / 2.0),
                 Translation2d(0.0, -trackwidthMeters / 2.0),
             ]
-            self.swerveKinematics = None
-            self.diffKinematics = DifferentialDriveKinematics(trackwidthMeters)
+            self._swerveKinematics = None
+            self._diffKinematics = DifferentialDriveKinematics(trackwidthMeters)
             self.isHolonomic = False
         else:
             self.moduleLocations = [
@@ -105,13 +106,13 @@ def __init__(self, massKG: float, MOI: float, moduleConfig: ModuleConfig, trackw
                 Translation2d(-wheelbaseMeters / 2.0, trackwidthMeters / 2.0),
                 Translation2d(-wheelbaseMeters / 2.0, -trackwidthMeters / 2.0),
             ]
-            self.swerveKinematics = SwerveDrive4Kinematics(
+            self._swerveKinematics = SwerveDrive4Kinematics(
                 Translation2d(wheelbaseMeters / 2.0, trackwidthMeters / 2.0),
                 Translation2d(wheelbaseMeters / 2.0, -trackwidthMeters / 2.0),
                 Translation2d(-wheelbaseMeters / 2.0, trackwidthMeters / 2.0),
                 Translation2d(-wheelbaseMeters / 2.0, -trackwidthMeters / 2.0),
             )
-            self.diffKinematics = None
+            self._diffKinematics = None
             self.isHolonomic = True
 
         self.numModules = len(self.moduleLocations)
@@ -128,9 +129,9 @@ def toSwerveModuleStates(self, speeds: ChassisSpeeds) -> List[SwerveModuleState]
         :return: List of swerve module states
         """
         if self.isHolonomic:
-            return self.swerveKinematics.toSwerveModuleStates(speeds)
+            return self._swerveKinematics.toSwerveModuleStates(speeds)
         else:
-            wheelSpeeds = self.diffKinematics.toWheelSpeeds(speeds)
+            wheelSpeeds = self._diffKinematics.toWheelSpeeds(speeds)
             return [
                 SwerveModuleState(wheelSpeeds.left, Rotation2d()),
                 SwerveModuleState(wheelSpeeds.right, Rotation2d())
@@ -145,10 +146,10 @@ def toChassisSpeeds(self, states: List[SwerveModuleState]) -> ChassisSpeeds:
         :return: Robot-relative chassis speeds
         """
         if self.isHolonomic:
-            return self.swerveKinematics.toChassisSpeeds(states)
+            return self._swerveKinematics.toChassisSpeeds(states)
         else:
             wheelSpeeds = DifferentialDriveWheelSpeeds(states[0].speed, states[1].speed)
-            return self.diffKinematics.toChassisSpeeds(wheelSpeeds)
+            return self._diffKinematics.toChassisSpeeds(wheelSpeeds)
 
     @staticmethod
     def fromGUISettings() -> 'RobotConfig':

pathplannerlib-python/pathplannerlib/trajectory.py

@@ -238,26 +238,10 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     dt = (2 * state.deltaPos) / (v + v0)
                     state.timeSeconds = prevState.timeSeconds + dt
 
-                    linearAccel = (state.linearVelocity - prevState.linearVelocity) / dt
-                    forceVec = Translation2d(linearAccel, prevState.heading) * config.massKG
-
-                    angularAccel = (state.fieldSpeeds.omega - prevState.fieldSpeeds.omega) / dt
-                    angTorque = angularAccel * config.MOI
-
-                    # Use kinematics to convert chassis forces to wheel forces
-                    wheelForces = config.toSwerveModuleStates(ChassisSpeeds(forceVec.x, forceVec.y, angTorque))
-
                     for m in range(config.numModules):
                         accel = (state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt
-                        forceAtCarpet = wheelForces[m].speed
-                        wheelTorque = forceAtCarpet * config.moduleConfig.wheelRadiusMeters
-                        torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.Kt
-
-                        # Negate the torque/force if the motor is slowing down
-                        if accel < 0:
-                            prevState.feedforwards.append(DriveFeedforward(accel, -forceAtCarpet, -torqueCurrent))
-                        else:
-                            prevState.feedforwards.append(DriveFeedforward(accel, forceAtCarpet, torqueCurrent))
+                        # Does not currently support force calculations
+                        prevState.feedforwards.append(DriveFeedforward(accel, 0.0, 0.0))
 
                     # Un-added events have their timestamp set to a waypoint relative position
                     # When adding the event to this trajectory, set its timestamp properly

pathplannerlib/src/main/java/com/pathplanner/lib/config/RobotConfig.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Filesystem;
 import java.io.*;
+import org.ejml.simple.SimpleMatrix;
 import org.json.simple.JSONObject;
 import org.json.simple.parser.JSONParser;
 import org.json.simple.parser.ParseException;
@@ -24,13 +25,13 @@ public class RobotConfig {
 
   /** Robot-relative locations of each drive module in meters */
   public final Translation2d[] moduleLocations;
-  /** Swerve kinematics used to convert ChassisSpeeds to/from module states. */
-  public final SwerveDriveKinematics swerveKinematics;
-  /** Differential kinematics used to convert ChassisSpeeds to/from module states. */
-  public final DifferentialDriveKinematics diffKinematics;
   /** Is the robot holonomic? */
   public final boolean isHolonomic;
 
+  private final SwerveDriveKinematics swerveKinematics;
+  private final DifferentialDriveKinematics diffKinematics;
+  private final SimpleMatrix forceKinematics;
+
   // Pre-calculated values that can be reused for every trajectory generation
   /** Number of drive modules */
   public final int numModules;
@@ -80,6 +81,15 @@ public RobotConfig(
     }
     this.wheelFrictionForce = this.moduleConfig.wheelCOF * ((this.massKG / numModules) * 9.8);
     this.maxTorqueFriction = this.wheelFrictionForce * this.moduleConfig.wheelRadiusMeters;
+
+    this.forceKinematics = new SimpleMatrix(this.numModules * 2, 3);
+    for (int i = 0; i < this.numModules; i++) {
+      Translation2d modPosReciprocal =
+          new Translation2d(
+              1.0 / this.moduleLocations[i].getNorm(), this.moduleLocations[i].getAngle());
+      this.forceKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -modPosReciprocal.getY());
+      this.forceKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, modPosReciprocal.getX());
+    }
   }
 
   /**
@@ -113,6 +123,15 @@ public RobotConfig(
     }
     this.wheelFrictionForce = this.moduleConfig.wheelCOF * ((this.massKG / numModules) * 9.8);
     this.maxTorqueFriction = this.wheelFrictionForce * this.moduleConfig.wheelRadiusMeters;
+
+    this.forceKinematics = new SimpleMatrix(this.numModules * 2, 3);
+    for (int i = 0; i < this.numModules; i++) {
+      Translation2d modPosReciprocal =
+          new Translation2d(
+              1.0 / this.moduleLocations[i].getNorm(), this.moduleLocations[i].getAngle());
+      this.forceKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -modPosReciprocal.getY());
+      this.forceKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, modPosReciprocal.getX());
+    }
   }
 
   /**
@@ -152,6 +171,36 @@ public ChassisSpeeds toChassisSpeeds(SwerveModuleState[] states) {
     }
   }
 
+  /**
+   * Convert chassis forces (passed as ChassisSpeeds) to individual wheel force vectors
+   *
+   * @param chassisForces The linear X/Y force and torque acting on the whole robot
+   * @return Array of individual wheel force vectors
+   */
+  public Translation2d[] chassisForcesToWheelForceVectors(ChassisSpeeds chassisForces) {
+    var chassisForceVector = new SimpleMatrix(3, 1);
+    chassisForceVector.setColumn(
+        0,
+        0,
+        chassisForces.vxMetersPerSecond,
+        chassisForces.vyMetersPerSecond,
+        chassisForces.omegaRadiansPerSecond);
+
+    // Divide the chassis force vector by numModules since force is additive. All module forces will
+    // add up to the chassis force
+    var moduleForceMatrix = forceKinematics.mult(chassisForceVector.divide(numModules));
+
+    Translation2d[] forceVectors = new Translation2d[numModules];
+    for (int m = 0; m < numModules; m++) {
+      double x = moduleForceMatrix.get(m * 2, 0);
+      double y = moduleForceMatrix.get(m * 2 + 1, 0);
+
+      forceVectors[m] = new Translation2d(x, y);
+    }
+
+    return forceVectors;
+  }
+
   /**
    * Load the robot config from the shared settings file created by the GUI
    *

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

@@ -129,35 +129,37 @@ public PathPlannerTrajectory(
         double dt = (2 * state.deltaPos) / (v + v0);
         state.timeSeconds = prevState.timeSeconds + dt;
 
-        double linearAccel = (state.linearVelocity - prevState.linearVelocity) / dt;
-        Translation2d forceVec =
-            new Translation2d(linearAccel, prevState.heading).times(config.massKG);
+        ChassisSpeeds prevRobotSpeeds =
+            ChassisSpeeds.fromFieldRelativeSpeeds(
+                prevState.fieldSpeeds, prevState.pose.getRotation());
+        ChassisSpeeds robotSpeeds =
+            ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation());
+        double chassisAccelX =
+            (robotSpeeds.vxMetersPerSecond - prevRobotSpeeds.vxMetersPerSecond) / dt;
+        double chassisAccelY =
+            (robotSpeeds.vyMetersPerSecond - prevRobotSpeeds.vyMetersPerSecond) / dt;
+        double chassisForceX = chassisAccelX * config.massKG;
+        double chassisForceY = chassisAccelY * config.massKG;
 
         double angularAccel =
-            (state.fieldSpeeds.omegaRadiansPerSecond - prevState.fieldSpeeds.omegaRadiansPerSecond)
-                / dt;
+            (robotSpeeds.omegaRadiansPerSecond - prevRobotSpeeds.omegaRadiansPerSecond) / dt;
         double angTorque = angularAccel * config.MOI;
+        ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque);
 
-        // Use kinematics to convert chassis forces to wheel forces
-        var wheelForces =
-            config.toSwerveModuleStates(
-                new ChassisSpeeds(forceVec.getX(), forceVec.getY(), angTorque));
+        Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces);
 
         for (int m = 0; m < config.numModules; m++) {
           double accel =
               (state.moduleStates[m].speedMetersPerSecond
                       - prevState.moduleStates[m].speedMetersPerSecond)
                   / dt;
-          double forceAtCarpet = wheelForces[m].speedMetersPerSecond;
-          double wheelTorque = forceAtCarpet * config.moduleConfig.wheelRadiusMeters;
+          double appliedForce =
+              wheelForces[m].getNorm()
+                  * wheelForces[m].getAngle().minus(state.moduleStates[m].angle).getCos();
+          double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters;
           double torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.KtNMPerAmp;
 
-          // Negate the torque/force if the motor is slowing down
-          if (accel < 0) {
-            prevState.feedforwards[m] = new DriveFeedforward(accel, -forceAtCarpet, -torqueCurrent);
-          } else {
-            prevState.feedforwards[m] = new DriveFeedforward(accel, forceAtCarpet, torqueCurrent);
-          }
+          prevState.feedforwards[m] = new DriveFeedforward(accel, appliedForce, torqueCurrent);
         }
 
         // Un-added events have their timestamp set to a waypoint relative position

pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforward.java

@@ -7,7 +7,7 @@
  * Collection of different feedforward values for a drive motor/module
  *
  * @param accelerationMPS Linear acceleration at the wheel in meters per second
- * @param forceNewtons Linear force at the wheel in newtons
+ * @param forceNewtons Linear force applied by the motor at the wheel in newtons
  * @param torqueCurrentAmps Torque-current of the motor in amps
  */
 public record DriveFeedforward(

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

@@ -309,34 +309,35 @@ public SwerveSetpoint generateSetpoint(
 
     DriveFeedforward[] retFF = new DriveFeedforward[config.numModules];
 
-    double prevLinearVel =
-        Math.hypot(
-            prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond,
-            prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond);
-    double linearVel = Math.hypot(retSpeeds.vxMetersPerSecond, retSpeeds.vyMetersPerSecond);
-    double linearAccel = (linearVel - prevLinearVel) / dt;
-    Rotation2d heading =
-        new Rotation2d(
-            prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond,
-            prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond);
-    Translation2d forceVec = new Translation2d(linearAccel, heading).times(config.massKG);
+    double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond;
+    double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond;
+    double chassisAccelX = (retSpeeds.vxMetersPerSecond - prevVelX) / dt;
+    double chassisAccelY = (retSpeeds.vyMetersPerSecond - prevVelY) / dt;
+    double chassisForceX = chassisAccelX * config.massKG;
+    double chassisForceY = chassisAccelY * config.massKG;
 
     double angularAccel =
         (retSpeeds.omegaRadiansPerSecond - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond)
             / dt;
     double angTorque = angularAccel * config.MOI;
+    ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque);
 
-    // Use kinematics to convert chassis forces to wheel forces
-    var wheelForces =
-        config.toSwerveModuleStates(new ChassisSpeeds(forceVec.getX(), forceVec.getY(), angTorque));
+    Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces);
 
     var retStates = config.toSwerveModuleStates(retSpeeds);
     for (int m = 0; m < config.numModules; m++) {
+      double appliedForce =
+          wheelForces[m].getNorm() * wheelForces[m].getAngle().minus(retStates[m].angle).getCos();
+      double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters;
+      double torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.KtNMPerAmp;
+
       final var maybeOverride = overrideSteering.get(m);
       if (maybeOverride.isPresent()) {
         var override = maybeOverride.get();
         if (flipHeading(retStates[m].angle.unaryMinus().rotateBy(override))) {
           retStates[m].speedMetersPerSecond *= -1.0;
+          appliedForce *= -1.0;
+          torqueCurrent *= -1.0;
         }
         retStates[m].angle = override;
       }
@@ -345,21 +346,15 @@ public SwerveSetpoint generateSetpoint(
       if (flipHeading(deltaRotation)) {
         retStates[m].angle = retStates[m].angle.rotateBy(Rotation2d.fromDegrees(180));
         retStates[m].speedMetersPerSecond *= -1.0;
+        appliedForce *= -1.0;
+        torqueCurrent *= -1.0;
       }
 
       double accel =
           (retStates[m].speedMetersPerSecond - prevSetpoint.moduleStates()[m].speedMetersPerSecond)
               / dt;
-      double forceAtCarpet = wheelForces[m].speedMetersPerSecond;
-      double wheelTorque = forceAtCarpet * config.moduleConfig.wheelRadiusMeters;
-      double torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.KtNMPerAmp;
 
-      // Negate the torque/force if the motor should apply it in the negative direction
-      if (accel < 0) {
-        retFF[m] = new DriveFeedforward(accel, -forceAtCarpet, -torqueCurrent);
-      } else {
-        retFF[m] = new DriveFeedforward(accel, forceAtCarpet, torqueCurrent);
-      }
+      retFF[m] = new DriveFeedforward(accel, appliedForce, torqueCurrent);
     }
 
     return new SwerveSetpoint(retSpeeds, retStates, retFF);