Feedforwards refactor (#870)

Writerside/topics/pplib-Follow-a-Single-Path.md

@@ -85,7 +85,7 @@ public class DriveSubsystem extends SubsystemBase {
                 path,
                 this::getPose, // Robot pose supplier
                 this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                this::drive, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND torque current feedforwards
+                this::drive, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND feedforwards
                 new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
                         new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
                         new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
@@ -129,7 +129,7 @@ frc2::CommandPtr  DriveSubsystem::followPathCommand(std::string pathName){
         path,
         [this](){ return getPose(); }, // Robot pose supplier
         [this](){ return getRobotRelativeSpeeds(); }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-        [this](frc::ChassisSpeeds speeds, std::vector<units::ampere_t> torqueCurrentFF){ driveRobotRelative(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND torque current feedforwards
+        [this](frc::ChassisSpeeds speeds, DriveFeedforwards feedforwards){ driveRobotRelative(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND feedforwards
         PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
             PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
             PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
@@ -167,7 +167,7 @@ def followPathCommand(pathName: str):
         path,
         self.getPose, # Robot pose supplier
         self.getRobotRelativeSpeeds, # ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-        self.driveRobotRelative, # Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND torque current feedforwards
+        self.driveRobotRelative, # Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND feedforwards
         PPHolonomicDriveController( # PPHolonomicController is the built in path following controller for holonomic drive trains
             PIDConstants(5.0, 0.0, 0.0), # Translation PID constants
             PIDConstants(5.0, 0.0, 0.0) # Rotation PID constants
@@ -203,7 +203,7 @@ public class DriveSubsystem extends SubsystemBase {
                 path,
                 this::getPose, // Robot pose supplier
                 this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                this::drive, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND torque current feedforwards
+                this::drive, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND feedforwards
                 new PPLTVController(0.02), // PPLTVController is the built in path following controller for differential drive trains
                 Constants.robotConfig, // The robot configuration
                 () -> {
@@ -244,7 +244,7 @@ frc2::CommandPtr  DriveSubsystem::followPathCommand(std::string pathName){
         path,
         [this](){ return getPose(); }, // Robot pose supplier
         [this](){ return getRobotRelativeSpeeds(); }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-        [this](frc::ChassisSpeeds speeds, std::vector<units::ampere_t> torqueCurrentFF){ driveRobotRelative(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND torque current feedforwards
+        [this](frc::ChassisSpeeds speeds, DriveFeedforwards feedforwards){ driveRobotRelative(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND feedforwards
         PPLTVController(0.02_s), // PPLTVController is the built in path following controller for differential drive trains
         Constants::robotConfig, // The robot configuration
         []() {
@@ -279,7 +279,7 @@ def followPathCommand(pathName: str):
         path,
         self.getPose, # Robot pose supplier
         self.getRobotRelativeSpeeds, # ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-        self.driveRobotRelative, # Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND torque current feedforwards
+        self.driveRobotRelative, # Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds, AND feedforwards
         PPLTVController(0.02), # PPLTVController is the built in path following controller for differential drive trains
         Constants.robotConfig, # The robot configuration
         self.shouldFlipPath, # Supplier to control path flipping based on alliance color

Writerside/topics/pplib-Swerve-Setpoint-Generator.md

@@ -49,7 +49,7 @@ public class SwerveSubsystem extends Subsystem {
         // Initialize the previous setpoint to the robot's current speeds & module states
         ChassisSpeeds currentSpeeds = getCurrentSpeeds(); // Method to get current robot-relative chassis speeds
         SwerveModuleState[] currentStates = getCurrentModuleStates(); // Method to get the current swerve module states
-        previousSetpoint = new SwerveSetpoint(currentSpeeds, currentStates);
+        previousSetpoint = new SwerveSetpoint(currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules));
     }
 
     /**

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]:
         """