refactor python feedforwards

pathplannerlib-python/pathplannerlib/auto.py

@@ -6,7 +6,7 @@
 from wpimath.geometry import Pose2d, Translation2d
 from wpimath.kinematics import ChassisSpeeds
 from .commands import FollowPathCommand, PathfindingCommand, PathfindThenFollowPath
-from .util import FlippingUtil, DriveFeedforward
+from .util import FlippingUtil, DriveFeedforwards
 from .controller import PathFollowingController
 from .events import EventTrigger, PointTowardsZoneTrigger
 import os
@@ -438,7 +438,7 @@ class AutoBuilder:
     @staticmethod
     def configure(pose_supplier: Callable[[], Pose2d], reset_pose: Callable[[Pose2d], None],
                   robot_relative_speeds_supplier: Callable[[], ChassisSpeeds],
-                  output: Callable[[ChassisSpeeds, List[DriveFeedforward]], None],
+                  output: Callable[[ChassisSpeeds, DriveFeedforwards], None],
                   controller: PathFollowingController,
                   robot_config: RobotConfig,
                   should_flip_path: Callable[[], bool],

pathplannerlib-python/pathplannerlib/commands.py

@@ -3,7 +3,7 @@
 from .trajectory import PathPlannerTrajectory
 from .telemetry import PPLibTelemetry
 from .logging import PathPlannerLogging
-from .util import floatLerp, FlippingUtil, DriveFeedforward
+from .util import floatLerp, FlippingUtil, DriveFeedforwards
 from wpimath.geometry import Pose2d
 from wpimath.kinematics import ChassisSpeeds
 from wpilib import Timer
@@ -19,7 +19,7 @@ class FollowPathCommand(Command):
     _originalPath: PathPlannerPath
     _poseSupplier: Callable[[], Pose2d]
     _speedsSupplier: Callable[[], ChassisSpeeds]
-    _output: Callable[[ChassisSpeeds, List[DriveFeedforward]], None]
+    _output: Callable[[ChassisSpeeds, DriveFeedforwards], None]
     _controller: PathFollowingController
     _robotConfig: RobotConfig
     _shouldFlipPath: Callable[[], bool]
@@ -35,7 +35,7 @@ class FollowPathCommand(Command):
 
     def __init__(self, path: PathPlannerPath, pose_supplier: Callable[[], Pose2d],
                  speeds_supplier: Callable[[], ChassisSpeeds],
-                 output: Callable[[ChassisSpeeds, List[DriveFeedforward]], None],
+                 output: Callable[[ChassisSpeeds, DriveFeedforwards], None],
                  controller: PathFollowingController, robot_config: RobotConfig,
                  should_flip_path: Callable[[], bool], *requirements: Subsystem):
         """
@@ -152,7 +152,7 @@ def end(self, interrupted: bool):
         # Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting
         # the command to smoothly transition into some auto-alignment routine
         if not interrupted and self._path.getGoalEndState().velocity < 0.1:
-            self._output(ChassisSpeeds(), [DriveFeedforward()] * self._robotConfig.numModules)
+            self._output(ChassisSpeeds(), DriveFeedforwards.zeros(self._robotConfig.numModules))
 
         PathPlannerLogging.logActivePath(None)
 
@@ -168,7 +168,7 @@ class PathfindingCommand(Command):
     _constraints: PathConstraints
     _poseSupplier: Callable[[], Pose2d]
     _speedsSupplier: Callable[[], ChassisSpeeds]
-    _output: Callable[[ChassisSpeeds, List[DriveFeedforward]], None]
+    _output: Callable[[ChassisSpeeds, DriveFeedforwards], None]
     _controller: PathFollowingController
     _robotConfig: RobotConfig
     _shouldFlipPath: Callable[[], bool]
@@ -184,7 +184,7 @@ class PathfindingCommand(Command):
 
     def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pose2d],
                  speeds_supplier: Callable[[], ChassisSpeeds],
-                 output: Callable[[ChassisSpeeds, List[DriveFeedforward]], None],
+                 output: Callable[[ChassisSpeeds, DriveFeedforwards], None],
                  controller: PathFollowingController, robot_config: RobotConfig,
                  should_flip_path: Callable[[], bool], *requirements: Subsystem,
                  target_path: PathPlannerPath = None, target_pose: Pose2d = None,
@@ -267,7 +267,7 @@ def initialize(self):
                 self._goalEndState = GoalEndState(self._goalEndState.velocity, self._targetPose.rotation())
 
         if currentPose.translation().distance(self._targetPose.translation()) < 0.5:
-            self._output(ChassisSpeeds(), [DriveFeedforward()] * self._robotConfig.numModules)
+            self._output(ChassisSpeeds(), DriveFeedforwards.zeros(self._robotConfig.numModules))
             self._finish = True
         else:
             Pathfinding.setStartPosition(currentPose.translation())
@@ -376,7 +376,7 @@ def end(self, interrupted: bool):
         # Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting
         # the command to smoothly transition into some auto-alignment routine
         if not interrupted and self._goalEndState.velocity < 0.1:
-            self._output(ChassisSpeeds(), [DriveFeedforward()] * self._robotConfig.numModules)
+            self._output(ChassisSpeeds(), DriveFeedforwards.zeros(self._robotConfig.numModules))
 
         PathPlannerLogging.logActivePath(None)
 
@@ -385,7 +385,7 @@ class PathfindThenFollowPath(SequentialCommandGroup):
     def __init__(self, goal_path: PathPlannerPath, pathfinding_constraints: PathConstraints,
                  pose_supplier: Callable[[], Pose2d],
                  speeds_supplier: Callable[[], ChassisSpeeds],
-                 output: Callable[[ChassisSpeeds, List[DriveFeedforward]], None],
+                 output: Callable[[ChassisSpeeds, DriveFeedforwards], None],
                  controller: PathFollowingController, robot_config: RobotConfig,
                  should_flip_path: Callable[[], bool], *requirements: Subsystem):
         """

pathplannerlib-python/pathplannerlib/path.py

@@ -10,7 +10,7 @@
 from commands2 import Command
 
 from .events import OneShotTriggerEvent, ScheduleCommandEvent, Event
-from .util import cubicLerp, calculateRadius, floatLerp, FlippingUtil, translation2dFromJson
+from .util import cubicLerp, calculateRadius, floatLerp, FlippingUtil, translation2dFromJson, DriveFeedforwards
 from .trajectory import PathPlannerTrajectory, PathPlannerTrajectoryState
 from .config import RobotConfig
 from wpilib import getDeployDirectory
@@ -546,6 +546,7 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None:
                 state.linearVelocity = math.hypot(xVel, yVel)
                 state.pose = Pose2d(xPos, yPos, rotationRad)
                 state.fieldSpeeds = ChassisSpeeds(xVel, yVel, angularVelRps)
+                state.feedforwards = DriveFeedforwards.zeros(4)
 
                 fullTrajStates.append(state)
 

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, DriveFeedforward
+from .util import floatLerp, rotationLerp, poseLerp, calculateRadius, FlippingUtil, DriveFeedforwards
 from .config import RobotConfig
 from .events import *
 from typing import List, Union, TYPE_CHECKING
@@ -30,7 +30,7 @@ class PathPlannerTrajectoryState:
     fieldSpeeds: ChassisSpeeds = ChassisSpeeds()
     pose: Pose2d = field(default_factory=Pose2d)
     linearVelocity: float = 0.0
-    feedforwards: List[DriveFeedforward] = field(default_factory=list)
+    feedforwards: DriveFeedforwards = None
 
     heading: Rotation2d = field(default_factory=Rotation2d)
     deltaPos: float = 0.0
@@ -62,10 +62,7 @@ def interpolate(self, end_val: PathPlannerTrajectoryState, t: float) -> PathPlan
         )
         lerpedState.pose = poseLerp(self.pose, end_val.pose, t)
         lerpedState.linearVelocity = floatLerp(self.linearVelocity, end_val.linearVelocity, t)
-        lerpedState.feedforwards = [
-            self.feedforwards[m].interpolate(end_val.feedforwards[m], t) for m in
-            range(len(self.feedforwards))
-        ]
+        lerpedState.feedforwards = self.feedforwards.interpolate(end_val.feedforwards, t)
 
         return lerpedState
 
@@ -82,7 +79,7 @@ def reverse(self) -> PathPlannerTrajectoryState:
         reversedState.fieldSpeeds = ChassisSpeeds(reversedSpeeds.x, reversedSpeeds.y, self.fieldSpeeds.omega)
         reversedState.pose = Pose2d(self.pose.translation(), self.pose.rotation() + Rotation2d.fromDegrees(180))
         reversedState.linearVelocity = -self.linearVelocity
-        reversedState.driveMotorTorqueCurrent = [ff.reverse() for ff in self.feedforwards]
+        reversedState.feedforwards = self.feedforwards.reverse()
 
         return reversedState
 
@@ -98,7 +95,7 @@ def flip(self) -> PathPlannerTrajectoryState:
         flipped.linearVelocity = self.linearVelocity
         flipped.pose = FlippingUtil.flipFieldPose(self.pose)
         flipped.fieldSpeeds = FlippingUtil.flipFieldSpeeds(self.fieldSpeeds)
-        flipped.feedforwards = FlippingUtil.flipFeedforwards(self.feedforwards)
+        flipped.feedforwards = self.feedforwards.flip()
 
         return flipped
 
@@ -230,15 +227,23 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     chassisForces = ChassisSpeeds(chassisForceX, chassisForceY, angTorque)
 
                     wheelForces = config.chassisForcesToWheelForceVectors(chassisForces)
-
+                    accelFF = []
+                    linearForceFF = []
+                    torqueCurrentFF = []
+                    forceXFF = []
+                    forceYFF = []
                     for m in range(config.numModules):
-                        accel = (state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt
                         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))
+                        accelFF.append((state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt)
+                        linearForceFF.append(appliedForce)
+                        torqueCurrentFF.append(torqueCurrent)
+                        forceXFF.append(wheelForces[m].x)
+                        forceYFF.append(wheelForces[m].y)
+                    prevState.feedforwards = DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF)
 
                     # Un-added events have their timestamp set to a waypoint relative position
                     # When adding the event to this trajectory, set its timestamp properly
@@ -254,7 +259,7 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch
                     self._events[-1].setTimestamp(self._states[-1].timeSeconds)
 
                 # Create feedforwards for the end state
-                self._states[-1].feedforwards = [DriveFeedforward(0, 0, 0)] * config.numModules
+                self._states[-1].feedforwards = DriveFeedforwards.zeros(config.numModules)
 
     def getEvents(self) -> List[Event]:
         """

pathplannerlib-python/pathplannerlib/util.py

@@ -1,4 +1,4 @@
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 
 from wpimath.geometry import Translation2d, Rotation2d, Pose2d
 from wpimath.kinematics import ChassisSpeeds
@@ -19,20 +19,71 @@ class FieldSymmetry(Enum):
 
 
 @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)
+class DriveFeedforwards:
+    accelerationsMPS: List[float] = field(default_factory=list)
+    forcesNewtons: List[float] = field(default_factory=list)
+    torqueCurrentsAmps: List[float] = field(default_factory=list)
+    robotRelativeForcesXNewtons: List[float] = field(default_factory=list)
+    robotRelativeForcesYNewtons: List[float] = field(default_factory=list)
+
+    @staticmethod
+    def zeros(numModules: int) -> 'DriveFeedforwards':
+        """
+        Create drive feedforwards consisting of all zeros
+
+        :param numModules: Number of drive modules
+        :return: Zero feedforwards
+        """
+        return DriveFeedforwards(
+            [0.0] * numModules,
+            [0.0] * numModules,
+            [0.0] * numModules,
+            [0.0] * numModules,
+            [0.0] * numModules
+        )
+
+    def interpolate(self, endVal: 'DriveFeedforwards', t: float) -> 'DriveFeedforwards':
+        return DriveFeedforwards(
+            DriveFeedforwards._interpolateList(self.accelerationsMPS, endVal.accelerationsMPS, t),
+            DriveFeedforwards._interpolateList(self.forcesNewtons, endVal.forcesNewtons, t),
+            DriveFeedforwards._interpolateList(self.torqueCurrentsAmps, endVal.torqueCurrentsAmps, t),
+            DriveFeedforwards._interpolateList(self.robotRelativeForcesXNewtons, endVal.robotRelativeForcesXNewtons, t),
+            DriveFeedforwards._interpolateList(self.robotRelativeForcesYNewtons, endVal.robotRelativeForcesYNewtons, t)
+        )
+
+    def reverse(self) -> 'DriveFeedforwards':
+        """
+        Reverse the feedforwards for driving backwards. This should only be used for differential drive robots.
+
+        :return: Reversed feedforwards
+        """
+        if len(self.accelerationsMPS) != 2:
+            raise RuntimeError('Feedforwards should only be reversed for differential drive trains')
+        return DriveFeedforwards(
+            [-self.accelerationsMPS[1], -self.accelerationsMPS[0]],
+            [-self.forcesNewtons[1], -self.forcesNewtons[0]],
+            [-self.torqueCurrentsAmps[1], -self.torqueCurrentsAmps[0]],
+            [-self.robotRelativeForcesXNewtons[1], -self.robotRelativeForcesXNewtons[0]],
+            [-self.robotRelativeForcesYNewtons[1], -self.robotRelativeForcesYNewtons[0]]
         )
 
-    def reverse(self) -> 'DriveFeedforward':
-        return DriveFeedforward(-self.accelerationMPS, -self.forceNewtons, -self.torqueCurrentAmps)
+    def flip(self) -> 'DriveFeedforwards':
+        """
+        Flip the feedforwards for the other side of the field. Only does anything if mirrored symmetry is used
+
+        :return: Flipped feedforwards
+        """
+        return DriveFeedforwards(
+            FlippingUtil.flipFeedforwards(self.accelerationsMPS),
+            FlippingUtil.flipFeedforwards(self.forcesNewtons),
+            FlippingUtil.flipFeedforwards(self.torqueCurrentsAmps),
+            FlippingUtil.flipFeedforwards(self.robotRelativeForcesXNewtons),
+            FlippingUtil.flipFeedforwards(self.robotRelativeForcesYNewtons)
+        )
+
+    @staticmethod
+    def _interpolateList(a: List[float], b: List[float], t: float) -> List[float]:
+        return [floatLerp(a[i], b[i], t) for i in range(len(a))]
 
 
 class FlippingUtil:
@@ -88,7 +139,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[float]) -> List[float]:
         """
         Flip a list of drive feedforwards for the other side of the field.
         Only does anything if mirrored symmetry is used