Add python force feedforward calculations (#814)

pathplannerlib-python/pathplannerlib/auto.py

@@ -5,7 +5,7 @@
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds
 from .commands import FollowPathCommand, PathfindingCommand, PathfindThenFollowPath
-from .util import FlippingUtil
+from .util import FlippingUtil, DriveFeedforward
 from .controller import PathFollowingController
 import os
 from wpilib import getDeployDirectory, reportError, reportWarning, SendableChooser
@@ -15,8 +15,6 @@
 from .config import RobotConfig
 from hal import report, tResourceType
 
-from .trajectory import DriveFeedforward
-
 
 class NamedCommands:
     _namedCommands: dict = {}

pathplannerlib-python/pathplannerlib/commands.py

@@ -1,9 +1,9 @@
 from .controller import *
 from .path import PathPlannerPath, GoalEndState, PathConstraints
-from .trajectory import PathPlannerTrajectory, DriveFeedforward
+from .trajectory import PathPlannerTrajectory
 from .telemetry import PPLibTelemetry
 from .logging import PathPlannerLogging
-from .util import floatLerp, FlippingUtil
+from .util import floatLerp, FlippingUtil, DriveFeedforward
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds
 from wpilib import Timer

pathplannerlib-python/pathplannerlib/config.py

@@ -7,6 +7,8 @@
 import os
 import json
 from wpilib import getDeployDirectory
+import numpy as np
+from numpy.typing import NDArray
 
 
 @dataclass
@@ -75,6 +77,7 @@ class RobotConfig:
 
     _swerveKinematics: Union[SwerveDrive4Kinematics, None]
     _diffKinematics: Union[DifferentialDriveKinematics, None]
+    _forceKinematics: NDArray
 
     def __init__(self, massKG: float, MOI: float, moduleConfig: ModuleConfig, trackwidthMeters: float,
                  wheelbaseMeters: float = None):
@@ -120,6 +123,12 @@ def __init__(self, massKG: float, MOI: float, moduleConfig: ModuleConfig, trackw
         self.wheelFrictionForce = self.moduleConfig.wheelCOF * ((self.massKG / self.numModules) * 9.8)
         self.maxTorqueFriction = self.wheelFrictionForce * self.moduleConfig.wheelRadiusMeters
 
+        self._forceKinematics = np.zeros((self.numModules * 2, 3))
+        for i in range(self.numModules):
+            modPosReciprocal = Translation2d(1.0 / self.moduleLocations[i].norm(), self.moduleLocations[i].angle())
+            self._forceKinematics[i * 2] = [1.0, 0.0, -modPosReciprocal.Y()]
+            self._forceKinematics[i * 2 + 1] = [0.0, 1.0, modPosReciprocal.X()]
+
     def toSwerveModuleStates(self, speeds: ChassisSpeeds) -> List[SwerveModuleState]:
         """
         Convert robot-relative chassis speeds to a list of swerve module states. This will use
@@ -151,6 +160,28 @@ def toChassisSpeeds(self, states: List[SwerveModuleState]) -> ChassisSpeeds:
             wheelSpeeds = DifferentialDriveWheelSpeeds(states[0].speed, states[1].speed)
             return self._diffKinematics.toChassisSpeeds(wheelSpeeds)
 
+    def chassisForcesToWheelForceVectors(self, chassisForces: ChassisSpeeds) -> List[Translation2d]:
+        """
+        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: List of individual wheel force vectors
+        """
+        chassisForceVector = np.array([chassisForces.vx, chassisForces.vy, chassisForces.omega]).reshape((3, 1))
+
+        # Divide the chassis force vector by numModules since force is additive. All module forces will
+        # add up to the chassis force
+        moduleForceMatrix = np.matmul(self._forceKinematics, (chassisForceVector / self.numModules))
+
+        forceVectors = []
+        for m in range(self.numModules):
+            x = moduleForceMatrix[m * 2][0]
+            y = moduleForceMatrix[m * 2 + 1][0]
+
+            forceVectors.append(Translation2d(x, y))
+
+        return forceVectors
+
     @staticmethod
     def fromGUISettings() -> 'RobotConfig':
         """

pathplannerlib-python/pathplannerlib/trajectory.py

@@ -4,7 +4,7 @@
 from dataclasses import dataclass, field
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
 from wpimath.kinematics import ChassisSpeeds, SwerveModuleState
-from .util import floatLerp, rotationLerp, poseLerp, calculateRadius, FlippingUtil
+from .util import floatLerp, rotationLerp, poseLerp, calculateRadius, FlippingUtil, DriveFeedforward
 from .config import RobotConfig
 from .events import *
 from typing import List, Union, TYPE_CHECKING
@@ -13,23 +13,6 @@
     from .path import PathPlannerPath, PathConstraints
 
 
-@dataclass(frozen=True)
-class DriveFeedforward:
-    accelerationMPS: float = 0.0
-    forceNewtons: float = 0.0
-    torqueCurrentAmps: float = 0.0
-
-    def interpolate(self, endVal: DriveFeedforward, t: float) -> DriveFeedforward:
-        return DriveFeedforward(
-            floatLerp(self.accelerationMPS, endVal.accelerationMPS, t),
-            floatLerp(self.forceNewtons, endVal.forceNewtons, t),
-            floatLerp(self.torqueCurrentAmps, endVal.torqueCurrentAmps, t)
-        )
-
-    def reverse(self) -> DriveFeedforward:
-        return DriveFeedforward(-self.accelerationMPS, -self.forceNewtons, -self.torqueCurrentAmps)
-
-
 @dataclass
 class SwerveModuleTrajectoryState(SwerveModuleState):
     fieldAngle: Rotation2d = Rotation2d()
@@ -226,10 +209,28 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     dt = (2 * state.deltaPos) / (v + v0)
                     state.timeSeconds = prevState.timeSeconds + dt
 
+                    prevRobotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(prevState.fieldSpeeds,
+                                                                            prevState.pose.rotation())
+                    robotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.rotation())
+                    chassisAccelX = (robotSpeeds.vx - prevRobotSpeeds.vx) / dt
+                    chassisAccelY = (robotSpeeds.vy - prevRobotSpeeds.vy) / dt
+                    chassisForceX = chassisAccelX * config.massKG
+                    chassisForceY = chassisAccelY * config.massKG
+
+                    angularAccel = (robotSpeeds.omega - prevRobotSpeeds.omega) / dt
+                    angTorque = angularAccel * config.MOI
+                    chassisForces = ChassisSpeeds(chassisForceX, chassisForceY, angTorque)
+
+                    wheelForces = config.chassisForcesToWheelForceVectors(chassisForces)
+
                     for m in range(config.numModules):
                         accel = (state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt
-                        # Does not currently support force calculations
-                        prevState.feedforwards.append(DriveFeedforward(accel, 0.0, 0.0))
+                        appliedForce = wheelForces[m].norm() * (
+                                    wheelForces[m].angle() - state.moduleStates[m].angle).cos()
+                        wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters
+                        torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.Kt
+
+                        prevState.feedforwards.append(DriveFeedforward(accel, appliedForce, torqueCurrent))
 
                     # Un-added events have their timestamp set to a waypoint relative position
                     # When adding the event to this trajectory, set its timestamp properly

pathplannerlib-python/pathplannerlib/util.py

@@ -1,18 +1,34 @@
+from dataclasses import dataclass
+
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
 from wpimath.kinematics import ChassisSpeeds
 from enum import Enum
 import math
-from typing import List, TYPE_CHECKING
-
-if TYPE_CHECKING:
-    from .trajectory import DriveFeedforward
+from typing import List
 
 
 class FieldSymmetry(Enum):
     kRotational = 1
     kMirrored = 2
 
 
+@dataclass(frozen=True)
+class DriveFeedforward:
+    accelerationMPS: float = 0.0
+    forceNewtons: float = 0.0
+    torqueCurrentAmps: float = 0.0
+
+    def interpolate(self, endVal: 'DriveFeedforward', t: float) -> 'DriveFeedforward':
+        return DriveFeedforward(
+            floatLerp(self.accelerationMPS, endVal.accelerationMPS, t),
+            floatLerp(self.forceNewtons, endVal.forceNewtons, t),
+            floatLerp(self.torqueCurrentAmps, endVal.torqueCurrentAmps, t)
+        )
+
+    def reverse(self) -> 'DriveFeedforward':
+        return DriveFeedforward(-self.accelerationMPS, -self.forceNewtons, -self.torqueCurrentAmps)
+
+
 class FlippingUtil:
     symmetryType: FieldSymmetry = FieldSymmetry.kMirrored
     fieldSizeX: float = 16.54175
@@ -66,7 +82,7 @@ def flipFieldSpeeds(fieldSpeeds: ChassisSpeeds) -> ChassisSpeeds:
             return ChassisSpeeds(-fieldSpeeds.vx, -fieldSpeeds.vy, fieldSpeeds.omega)
 
     @staticmethod
-    def flipFeedforwards(feedforwards: List['DriveFeedforward']) -> List['DriveFeedforward']:
+    def flipFeedforwards(feedforwards: List[DriveFeedforward]) -> List[DriveFeedforward]:
         """
         Flip a list of drive feedforwards for the other side of the field.
         Only does anything if mirrored symmetry is used

pathplannerlib-python/pyproject.toml

@@ -21,6 +21,7 @@ dependencies = [
     "robotpy-commands-v2 >=2024.3.1",
     "robotpy-wpiutil >=2024.3.1",
     "robotpy-hal >=2024.3.1",
+    "numpy >=2.1.1",
 ]
 requires-python = ">=3.8"
 

pathplannerlib-python/requirements.txt

@@ -3,4 +3,5 @@ pyntcore>=2024.3.1
 robotpy-wpimath>=2024.3.1
 robotpy-commands-v2>=2024.3.1
 robotpy-wpiutil>=2024.3.1
-robotpy-hal>=2024.3.1
+robotpy-hal>=2024.3.1
+numpy>=2.1.1