From e04e819a10d774f8c5f5b9fb48401ba1ed69a871 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Fri, 18 Oct 2024 14:28:57 -0400 Subject: [PATCH] Feedforwards refactor (#870) --- .../topics/pplib-Follow-a-Single-Path.md | 12 +- .../topics/pplib-Swerve-Setpoint-Generator.md | 2 +- pathplannerlib-python/pathplannerlib/auto.py | 4 +- .../pathplannerlib/commands.py | 18 +- pathplannerlib-python/pathplannerlib/path.py | 3 +- .../pathplannerlib/trajectory.py | 29 +-- pathplannerlib-python/pathplannerlib/util.py | 79 ++++++-- pathplannerlib/.vscode/settings.json | 4 +- .../com/pathplanner/lib/auto/AutoBuilder.java | 4 +- .../lib/commands/FollowPathCommand.java | 12 +- .../lib/commands/PathfindThenFollowPath.java | 4 +- .../lib/commands/PathfindingCommand.java | 22 +-- .../pathplanner/lib/path/PathPlannerPath.java | 4 +- .../lib/trajectory/PathPlannerTrajectory.java | 28 +-- .../PathPlannerTrajectoryState.java | 16 +- .../lib/util/DriveFeedforward.java | 75 -------- .../lib/util/DriveFeedforwards.java | 176 ++++++++++++++++++ .../pathplanner/lib/util/FlippingUtil.java | 8 +- .../lib/util/swerve/SwerveSetpoint.java | 8 +- .../util/swerve/SwerveSetpointGenerator.java | 22 ++- .../cpp/pathplanner/lib/auto/AutoBuilder.cpp | 2 +- .../lib/commands/FollowPathCommand.cpp | 9 +- .../lib/commands/PathfindingCommand.cpp | 18 +- .../pathplanner/lib/path/PathPlannerPath.cpp | 1 + .../lib/trajectory/PathPlannerTrajectory.cpp | 26 ++- .../trajectory/PathPlannerTrajectoryState.cpp | 11 +- .../pathplanner/lib/auto/AutoBuilder.h | 5 +- .../lib/commands/FollowPathCommand.h | 7 +- .../lib/commands/PathfindThenFollowPath.h | 3 +- .../lib/commands/PathfindingCommand.h | 10 +- .../trajectory/PathPlannerTrajectoryState.h | 4 +- .../pathplanner/lib/util/DriveFeedforward.h | 37 ---- .../pathplanner/lib/util/DriveFeedforwards.h | 96 ++++++++++ .../pathplanner/lib/util/FlippingUtil.h | 18 +- 34 files changed, 487 insertions(+), 290 deletions(-) delete mode 100644 pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforward.java create mode 100644 pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java delete mode 100644 pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforward.h create mode 100644 pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforwards.h diff --git a/Writerside/topics/pplib-Follow-a-Single-Path.md b/Writerside/topics/pplib-Follow-a-Single-Path.md index 77b59f82..2ad2d595 100644 --- a/Writerside/topics/pplib-Follow-a-Single-Path.md +++ b/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 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 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 diff --git a/Writerside/topics/pplib-Swerve-Setpoint-Generator.md b/Writerside/topics/pplib-Swerve-Setpoint-Generator.md index 6d58ac17..f244b0cf 100644 --- a/Writerside/topics/pplib-Swerve-Setpoint-Generator.md +++ b/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)); } /** diff --git a/pathplannerlib-python/pathplannerlib/auto.py b/pathplannerlib-python/pathplannerlib/auto.py index 8871ebc8..e556b772 100644 --- a/pathplannerlib-python/pathplannerlib/auto.py +++ b/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], diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py index 184aeb14..01bb5730 100644 --- a/pathplannerlib-python/pathplannerlib/commands.py +++ b/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): """ diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 36a43eff..f7261c87 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/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) diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py index 513816e1..215cc797 100644 --- a/pathplannerlib-python/pathplannerlib/trajectory.py +++ b/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]: """ diff --git a/pathplannerlib-python/pathplannerlib/util.py b/pathplannerlib-python/pathplannerlib/util.py index 2f457841..58a561ca 100644 --- a/pathplannerlib-python/pathplannerlib/util.py +++ b/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 diff --git a/pathplannerlib/.vscode/settings.json b/pathplannerlib/.vscode/settings.json index 57b620b4..e21957a4 100644 --- a/pathplannerlib/.vscode/settings.json +++ b/pathplannerlib/.vscode/settings.json @@ -101,6 +101,8 @@ "xlocmon": "cpp", "xlocnum": "cpp", "xloctime": "cpp", - "xtree": "cpp" + "xtree": "cpp", + "expected": "cpp", + "source_location": "cpp" } } \ No newline at end of file diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java index b7b3fe0a..85e4b648 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java @@ -7,7 +7,7 @@ import com.pathplanner.lib.controllers.PathFollowingController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.FlippingUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -63,7 +63,7 @@ public static void configure( Supplier poseSupplier, Consumer resetPose, Supplier robotRelativeSpeedsSupplier, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, BooleanSupplier shouldFlipPath, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java index 51b11e3d..cfd65ec9 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java @@ -8,7 +8,7 @@ import com.pathplanner.lib.events.EventScheduler; import com.pathplanner.lib.path.*; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.PPLibTelemetry; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.Pose2d; @@ -30,7 +30,7 @@ public class FollowPathCommand extends Command { private final PathPlannerPath originalPath; private final Supplier poseSupplier; private final Supplier speedsSupplier; - private final BiConsumer output; + private final BiConsumer output; private final PathFollowingController controller; private final RobotConfig robotConfig; private final BooleanSupplier shouldFlipPath; @@ -60,7 +60,7 @@ public FollowPathCommand( PathPlannerPath path, Supplier poseSupplier, Supplier speedsSupplier, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, BooleanSupplier shouldFlipPath, @@ -188,11 +188,7 @@ public void end(boolean interrupted) { // 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 (!interrupted && path.getGoalEndState().velocityMPS() < 0.1) { - var ff = new DriveFeedforward[robotConfig.numModules]; - for (int m = 0; m < robotConfig.numModules; m++) { - ff[m] = new DriveFeedforward(0.0, 0.0, 0.0); - } - output.accept(new ChassisSpeeds(), ff); + output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); } PathPlannerLogging.logActivePath(null); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPath.java index 2f51f9fe..33b9f880 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPath.java @@ -4,7 +4,7 @@ import com.pathplanner.lib.controllers.PathFollowingController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -38,7 +38,7 @@ public PathfindThenFollowPath( PathConstraints pathfindingConstraints, Supplier poseSupplier, Supplier currentRobotRelativeSpeeds, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, BooleanSupplier shouldFlipPath, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index 35adb074..48af9ff1 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -39,7 +39,7 @@ public class PathfindingCommand extends Command { private final PathConstraints constraints; private final Supplier poseSupplier; private final Supplier speedsSupplier; - private final BiConsumer output; + private final BiConsumer output; private final PathFollowingController controller; private final RobotConfig robotConfig; private final BooleanSupplier shouldFlipPath; @@ -74,7 +74,7 @@ public PathfindingCommand( PathConstraints constraints, Supplier poseSupplier, Supplier speedsSupplier, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, BooleanSupplier shouldFlipPath, @@ -140,7 +140,7 @@ public PathfindingCommand( double goalEndVel, Supplier poseSupplier, Supplier speedsSupplier, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, Subsystem... requirements) { @@ -189,7 +189,7 @@ public PathfindingCommand( LinearVelocity goalEndVel, Supplier poseSupplier, Supplier speedsSupplier, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, Subsystem... requirements) { @@ -227,7 +227,7 @@ public PathfindingCommand( PathConstraints constraints, Supplier poseSupplier, Supplier speedsSupplier, - BiConsumer output, + BiConsumer output, PathFollowingController controller, RobotConfig robotConfig, Subsystem... requirements) { @@ -263,11 +263,7 @@ public void initialize() { } if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) < 0.5) { - var ff = new DriveFeedforward[robotConfig.numModules]; - for (int m = 0; m < robotConfig.numModules; m++) { - ff[m] = new DriveFeedforward(0.0, 0.0, 0.0); - } - output.accept(new ChassisSpeeds(), ff); + output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); finish = true; } else { Pathfinding.setStartPosition(currentPose.getTranslation()); @@ -414,11 +410,7 @@ public void end(boolean interrupted) { // 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 (!interrupted && goalEndState.velocityMPS() < 0.1) { - var ff = new DriveFeedforward[robotConfig.numModules]; - for (int m = 0; m < robotConfig.numModules; m++) { - ff[m] = new DriveFeedforward(0.0, 0.0, 0.0); - } - output.accept(new ChassisSpeeds(), ff); + output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); } PathPlannerLogging.logActivePath(null); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index cbe6bb0c..c12ef96d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -7,7 +7,7 @@ import com.pathplanner.lib.events.ScheduleCommandEvent; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.FileVersionException; import com.pathplanner.lib.util.GeometryUtil; import com.pathplanner.lib.util.PPLibTelemetry; @@ -375,7 +375,7 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName) state.linearVelocity = Math.hypot(xVel, yVel); state.pose = new Pose2d(new Translation2d(xPos, yPos), new Rotation2d(rotationRad)); state.fieldSpeeds = new ChassisSpeeds(xVel, yVel, angularVelRps); - state.feedforwards = new DriveFeedforward[0]; + state.feedforwards = DriveFeedforwards.zeros(4); fullTrajStates.add(state); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java index 135e8f87..2bceff97 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java @@ -8,7 +8,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPoint; import com.pathplanner.lib.path.PointTowardsZone; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -153,20 +153,29 @@ public PathPlannerTrajectory( ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque); Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces); - + double[] accelFF = new double[config.numModules]; + double[] linearForceFF = new double[config.numModules]; + double[] torqueCurrentFF = new double[config.numModules]; + double[] forceXFF = new double[config.numModules]; + double[] forceYFF = new double[config.numModules]; for (int m = 0; m < config.numModules; m++) { - double accel = - (state.moduleStates[m].speedMetersPerSecond - - prevState.moduleStates[m].speedMetersPerSecond) - / dt; double appliedForce = wheelForces[m].getNorm() * wheelForces[m].getAngle().minus(state.moduleStates[m].angle).getCos(); double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters; double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque); - prevState.feedforwards[m] = new DriveFeedforward(accel, appliedForce, torqueCurrent); + accelFF[m] = + (state.moduleStates[m].speedMetersPerSecond + - prevState.moduleStates[m].speedMetersPerSecond) + / dt; + linearForceFF[m] = appliedForce; + torqueCurrentFF[m] = torqueCurrent; + forceXFF[m] = wheelForces[m].getX(); + forceYFF[m] = wheelForces[m].getY(); } + prevState.feedforwards = + new 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 @@ -188,9 +197,7 @@ public PathPlannerTrajectory( } // Create feedforwards for the end state - for (int m = 0; m < config.numModules; m++) { - states.get(states.size() - 1).feedforwards[m] = new DriveFeedforward(0, 0, 0); - } + states.get(states.size() - 1).feedforwards = DriveFeedforwards.zeros(config.numModules); } } @@ -252,7 +259,6 @@ private static void generateStates( } state.moduleStates = new SwerveModuleTrajectoryState[config.numModules]; - state.feedforwards = new DriveFeedforward[config.numModules]; for (int m = 0; m < config.numModules; m++) { state.moduleStates[m] = new SwerveModuleTrajectoryState(); state.moduleStates[m].fieldPos = diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java index e1820234..b0c14148 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java @@ -1,7 +1,7 @@ package com.pathplanner.lib.trajectory; import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.FlippingUtil; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -22,7 +22,7 @@ public class PathPlannerTrajectoryState implements Interpolatable { - /** - * Collection of different feedforward values for a drive motor/module - * - * @param acceleration Linear acceleration at the wheel - * @param force Linear force applied by the motor at the wheel - * @param torqueCurrent Torque-current of the motor - */ - public DriveFeedforward(LinearAcceleration acceleration, Force force, Current torqueCurrent) { - this(acceleration.in(MetersPerSecondPerSecond), force.in(Newtons), torqueCurrent.in(Amps)); - } - - @Override - public DriveFeedforward interpolate(DriveFeedforward endValue, double t) { - return new DriveFeedforward( - MathUtil.interpolate(accelerationMPS, endValue.accelerationMPS, t), - MathUtil.interpolate(forceNewtons, endValue.forceNewtons, t), - MathUtil.interpolate(torqueCurrentAmps, endValue.torqueCurrentAmps, t)); - } - - /** - * Reverse the feedforwards for driving backwards. - * - * @return Reversed feedforwards - */ - public DriveFeedforward reverse() { - return new DriveFeedforward(-accelerationMPS, -forceNewtons, -torqueCurrentAmps); - } - - /** - * Get the linear acceleration at the wheel - * - * @return Linear acceleration at the wheel - */ - public LinearAcceleration acceleration() { - return MetersPerSecondPerSecond.of(accelerationMPS); - } - - /** - * Get the linear force at the wheel - * - * @return Linear force at the wheel - */ - public Force force() { - return Newtons.of(forceNewtons); - } - - /** - * Get the torque-current of the motor - * - * @return Torque-current of the motor - */ - public Current torqueCurrent() { - return Amps.of(torqueCurrentAmps); - } -} diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java new file mode 100644 index 00000000..e8a61128 --- /dev/null +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java @@ -0,0 +1,176 @@ +package com.pathplanner.lib.util; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Force; +import edu.wpi.first.units.measure.LinearAcceleration; +import java.util.Arrays; + +/** + * Collection of different feedforward values for each drive module. If using swerve, these values + * will all be in FL, FR, BL, BR order. If using a differential drive, these will be in L, R order. + * + * @param accelerationsMPSSq Linear acceleration at the wheels in meters per second + * @param linearForcesNewtons Linear force applied by the motors at the wheels in newtons + * @param torqueCurrentsAmps Torque-current of the drive motors in amps + * @param robotRelativeForcesXNewtons X components of robot-relative force vectors for the wheels in + * newtons. The magnitude of these vectors will typically be greater than the linear force + * feedforwards due to friction forces. + * @param robotRelativeForcesYNewtons X components of robot-relative force vectors for the wheels in + * newtons. The magnitude of these vectors will typically be greater than the linear force + * feedforwards due to friction forces. + */ +public record DriveFeedforwards( + double[] accelerationsMPSSq, + double[] linearForcesNewtons, + double[] torqueCurrentsAmps, + double[] robotRelativeForcesXNewtons, + double[] robotRelativeForcesYNewtons) + implements Interpolatable { + /** + * Collection of different feedforward values for each drive module. If using swerve, these values + * will all be in FL, FR, BL, BR order. If using a differential drive, these will be in L, R + * order. + * + * @param accelerations Linear acceleration at the wheels + * @param linearForces Linear force applied by the motors at the wheels + * @param torqueCurrents Torque-current of the drive motors + * @param robotRelativeForcesX X components of robot-relative force vectors for the wheels. The + * magnitude of these vectors will typically be greater than the linear force feedforwards due + * to friction forces. + * @param robotRelativeForcesY X components of robot-relative force vectors for the wheels. The + * magnitude of these vectors will typically be greater than the linear force feedforwards due + * to friction forces. + */ + public DriveFeedforwards( + LinearAcceleration[] accelerations, + Force[] linearForces, + Current[] torqueCurrents, + Force[] robotRelativeForcesX, + Force[] robotRelativeForcesY) { + this( + Arrays.stream(accelerations).mapToDouble(x -> x.in(MetersPerSecondPerSecond)).toArray(), + Arrays.stream(linearForces).mapToDouble(x -> x.in(Newtons)).toArray(), + Arrays.stream(torqueCurrents).mapToDouble(x -> x.in(Amps)).toArray(), + Arrays.stream(robotRelativeForcesX).mapToDouble(x -> x.in(Newtons)).toArray(), + Arrays.stream(robotRelativeForcesY).mapToDouble(x -> x.in(Newtons)).toArray()); + } + + /** + * Create drive feedforwards consisting of all zeros + * + * @param numModules Number of drive modules + * @return Zero feedforwards + */ + public static DriveFeedforwards zeros(int numModules) { + return new DriveFeedforwards( + new double[numModules], + new double[numModules], + new double[numModules], + new double[numModules], + new double[numModules]); + } + + @Override + public DriveFeedforwards interpolate(DriveFeedforwards endValue, double t) { + return new DriveFeedforwards( + interpolateArray(accelerationsMPSSq, endValue.accelerationsMPSSq, t), + interpolateArray(linearForcesNewtons, endValue.linearForcesNewtons, t), + interpolateArray(torqueCurrentsAmps, endValue.torqueCurrentsAmps, t), + interpolateArray(robotRelativeForcesXNewtons, endValue.robotRelativeForcesXNewtons, t), + interpolateArray(robotRelativeForcesYNewtons, endValue.robotRelativeForcesYNewtons, t)); + } + + /** + * Reverse the feedforwards for driving backwards. This should only be used for differential drive + * robots. + * + * @return Reversed feedforwards + */ + public DriveFeedforwards reverse() { + if (accelerationsMPSSq.length != 2) { + throw new IllegalStateException( + "Feedforwards should only be reversed for differential drive trains"); + } + + return new DriveFeedforwards( + new double[] {-accelerationsMPSSq[1], -accelerationsMPSSq[0]}, + new double[] {-linearForcesNewtons[1], -linearForcesNewtons[0]}, + new double[] {-torqueCurrentsAmps[1], -torqueCurrentsAmps[0]}, + new double[] {-robotRelativeForcesXNewtons[1], -robotRelativeForcesXNewtons[0]}, + new double[] {-robotRelativeForcesYNewtons[1], -robotRelativeForcesYNewtons[0]}); + } + + /** + * Flip the feedforwards for the other side of the field. Only does anything if mirrored symmetry + * is used + * + * @return Flipped feedforwards + */ + public DriveFeedforwards flip() { + return new DriveFeedforwards( + FlippingUtil.flipFeedforwards(accelerationsMPSSq), + FlippingUtil.flipFeedforwards(linearForcesNewtons), + FlippingUtil.flipFeedforwards(torqueCurrentsAmps), + FlippingUtil.flipFeedforwards(robotRelativeForcesXNewtons), + FlippingUtil.flipFeedforwards(robotRelativeForcesYNewtons)); + } + + /** + * Get the linear accelerations at the wheels + * + * @return Linear accelerations at the wheels + */ + public LinearAcceleration[] accelerations() { + return Arrays.stream(accelerationsMPSSq) + .mapToObj(MetersPerSecondPerSecond::of) + .toArray(LinearAcceleration[]::new); + } + + /** + * Get the linear forces at the wheels + * + * @return Linear forces at the wheels + */ + public Force[] linearForces() { + return Arrays.stream(linearForcesNewtons).mapToObj(Newtons::of).toArray(Force[]::new); + } + + /** + * Get the torque-current of the drive motors + * + * @return Torque-current of the drive motors + */ + public Current[] torqueCurrents() { + return Arrays.stream(torqueCurrentsAmps).mapToObj(Amps::of).toArray(Current[]::new); + } + + /** + * Get the X components of the robot-relative force vectors at the wheels + * + * @return X components of the robot-relative force vectors at the wheels + */ + public Force[] robotRelativeForcesX() { + return Arrays.stream(robotRelativeForcesXNewtons).mapToObj(Newtons::of).toArray(Force[]::new); + } + + /** + * Get the Y components of the robot-relative force vectors at the wheels + * + * @return Y components of the robot-relative force vectors at the wheels + */ + public Force[] robotRelativeForcesY() { + return Arrays.stream(robotRelativeForcesYNewtons).mapToObj(Newtons::of).toArray(Force[]::new); + } + + private static double[] interpolateArray(double[] a, double[] b, double t) { + double[] ret = new double[a.length]; + for (int i = 0; i < a.length; i++) { + ret[i] = MathUtil.interpolate(a[i], b[i], t); + } + return ret; + } +} diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/FlippingUtil.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/FlippingUtil.java index 492bbbfa..b33bd8a2 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/FlippingUtil.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/FlippingUtil.java @@ -88,15 +88,13 @@ public static ChassisSpeeds flipFieldSpeeds(ChassisSpeeds fieldSpeeds) { * @param feedforwards Array of drive feedforwards * @return The flipped feedforwards */ - public static DriveFeedforward[] flipFeedforwards(DriveFeedforward[] feedforwards) { + public static double[] flipFeedforwards(double[] feedforwards) { return switch (symmetryType) { case kMirrored -> { if (feedforwards.length == 4) { - yield new DriveFeedforward[] { - feedforwards[1], feedforwards[0], feedforwards[3], feedforwards[2] - }; + yield new double[] {feedforwards[1], feedforwards[0], feedforwards[3], feedforwards[2]}; } else if (feedforwards.length == 2) { - yield new DriveFeedforward[] {feedforwards[1], feedforwards[0]}; + yield new double[] {feedforwards[1], feedforwards[0]}; } yield feedforwards; // idk } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpoint.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpoint.java index 8957a893..09153321 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpoint.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpoint.java @@ -1,6 +1,6 @@ package com.pathplanner.lib.util.swerve; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -11,10 +11,10 @@ * @param robotRelativeSpeeds Robot-relative chassis speeds * @param moduleStates Array of individual swerve module states. These will be in FL, FR, BL, BR * order. - * @param feedforwards Array of feedforwards for each module's drive motor. These will be in FL, FR, - * BL, BR order. + * @param feedforwards Feedforwards for each module's drive motor. The arrays in this record will be + * in FL, FR, BL, BR order. */ public record SwerveSetpoint( ChassisSpeeds robotRelativeSpeeds, SwerveModuleState[] moduleStates, - DriveFeedforward[] feedforwards) {} + DriveFeedforwards feedforwards) {} 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 472e4275..1381a745 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 @@ -3,7 +3,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.util.DriveFeedforward; +import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -319,8 +319,6 @@ public SwerveSetpoint generateSetpoint( prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta); retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); - DriveFeedforward[] retFF = new DriveFeedforward[config.numModules]; - double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; double chassisAccelX = (retSpeeds.vxMetersPerSecond - prevVelX) / dt; @@ -337,6 +335,11 @@ public SwerveSetpoint generateSetpoint( Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces); var retStates = config.toSwerveModuleStates(retSpeeds); + double[] accelFF = new double[config.numModules]; + double[] linearForceFF = new double[config.numModules]; + double[] torqueCurrentFF = new double[config.numModules]; + double[] forceXFF = new double[config.numModules]; + double[] forceYFF = new double[config.numModules]; for (int m = 0; m < config.numModules; m++) { double appliedForce = wheelForces[m].getNorm() * wheelForces[m].getAngle().minus(retStates[m].angle).getCos(); @@ -362,14 +365,19 @@ public SwerveSetpoint generateSetpoint( torqueCurrent *= -1.0; } - double accel = + accelFF[m] = (retStates[m].speedMetersPerSecond - prevSetpoint.moduleStates()[m].speedMetersPerSecond) / dt; - - retFF[m] = new DriveFeedforward(accel, appliedForce, torqueCurrent); + linearForceFF[m] = appliedForce; + torqueCurrentFF[m] = torqueCurrent; + forceXFF[m] = wheelForces[m].getX(); + forceYFF[m] = wheelForces[m].getY(); } - return new SwerveSetpoint(retSpeeds, retStates, retFF); + return new SwerveSetpoint( + retSpeeds, + retStates, + new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF)); } /** diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp index 7ee9d38d..fb086058 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/auto/AutoBuilder.cpp @@ -34,7 +34,7 @@ std::function< void AutoBuilder::configure(std::function poseSupplier, std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Subsystem *driveSubsystem) { diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp index c3ad22ff..717eb9bb 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp @@ -8,7 +8,7 @@ using namespace pathplanner; FollowPathCommand::FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, - std::function)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) : m_originalPath(path), m_poseSupplier( @@ -130,11 +130,8 @@ void FollowPathCommand::End(bool interrupted) { // 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 (!interrupted && m_path->getGoalEndState().getVelocity() < 0.1_mps) { - std::vector < DriveFeedforward > ff; - for (size_t m = 0; m < m_robotConfig.numModules; m++) { - ff.emplace_back(DriveFeedforward { }); - } - m_output(frc::ChassisSpeeds(), ff); + m_output(frc::ChassisSpeeds(), + DriveFeedforwards::zeros(m_robotConfig.numModules)); } PathPlannerLogging::logActivePath(nullptr); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index 3a7aecb5..7edce063 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -15,7 +15,7 @@ PathfindingCommand::PathfindingCommand( std::shared_ptr targetPath, PathConstraints constraints, std::function poseSupplier, std::function speedsSupplier, - std::function)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) : m_targetPath(targetPath), m_targetPose(), m_goalEndState( @@ -59,7 +59,7 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, PathConstraints constraints, units::meters_per_second_t goalEndVel, std::function poseSupplier, std::function speedsSupplier, - std::function)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, frc2::Requirements requirements) : m_targetPath(), m_targetPose( targetPose), m_originalTargetPose(targetPose), m_goalEndState( @@ -97,11 +97,8 @@ void PathfindingCommand::Initialize() { if (currentPose.Translation().Distance(m_targetPose.Translation()) < 0.5_m) { - std::vector < DriveFeedforward > ff; - for (size_t m = 0; m < m_robotConfig.numModules; m++) { - ff.emplace_back(DriveFeedforward { }); - } - m_output(frc::ChassisSpeeds(), ff); + m_output(frc::ChassisSpeeds(), + DriveFeedforwards::zeros(m_robotConfig.numModules)); Cancel(); } else { Pathfinding::setStartPosition(currentPose.Translation()); @@ -231,11 +228,8 @@ void PathfindingCommand::End(bool interrupted) { // 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 (!interrupted && m_goalEndState.getVelocity() < 0.1_mps) { - std::vector < DriveFeedforward > ff; - for (size_t m = 0; m < m_robotConfig.numModules; m++) { - ff.emplace_back(DriveFeedforward { }); - } - m_output(frc::ChassisSpeeds(), ff); + m_output(frc::ChassisSpeeds(), + DriveFeedforwards::zeros(m_robotConfig.numModules)); } PathPlannerLogging::logActivePath(nullptr); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 6828ce98..cad21d4c 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -186,6 +186,7 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( state.pose = frc::Pose2d(frc::Translation2d(xPos, yPos), frc::Rotation2d(rotationRad)); state.fieldSpeeds = frc::ChassisSpeeds { xVel, yVel, angularVelRps }; + state.feedforwards = DriveFeedforwards::zeros(4); fullTrajStates.emplace_back(state); } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp index 155a708a..289bb3d6 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp @@ -134,11 +134,12 @@ PathPlannerTrajectory::PathPlannerTrajectory( auto wheelForces = config.chassisForcesToWheelForceVectors( chassisForces); - + std::vector < units::meters_per_second_squared_t > accelFF; + std::vector < units::newton_t > linearForceFF; + std::vector < units::ampere_t > torqueCurrentFF; + std::vector < units::newton_t > forceXFF; + std::vector < units::newton_t > forceYFF; for (size_t m = 0; m < config.numModules; m++) { - units::meters_per_second_squared_t accel = - (state.moduleStates[m].speed - - prevState.moduleStates[m].speed) / dt; units::newton_t appliedForce { wheelForces[m].Norm()() * (wheelForces[m].Angle() @@ -148,9 +149,16 @@ PathPlannerTrajectory::PathPlannerTrajectory( units::ampere_t torqueCurrent = config.moduleConfig.driveMotor.Current(wheelTorque); - prevState.feedforwards.emplace_back(DriveFeedforward { accel, - appliedForce, torqueCurrent }); + accelFF.emplace_back( + (state.moduleStates[m].speed + - prevState.moduleStates[m].speed) / dt); + linearForceFF.emplace_back(appliedForce); + torqueCurrentFF.emplace_back(torqueCurrent); + forceXFF.emplace_back(units::newton_t { wheelForces[m].X()() }); + forceYFF.emplace_back(units::newton_t { 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 @@ -176,10 +184,8 @@ PathPlannerTrajectory::PathPlannerTrajectory( } // Create feedforwards for the end state - for (size_t m = 0; m < config.numModules; m++) { - m_states[m_states.size() - 1].feedforwards.emplace_back( - DriveFeedforward { 0_mps_sq, 0_N, 0_A }); - } + m_states[m_states.size() - 1].feedforwards = DriveFeedforwards::zeros( + config.numModules); } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp index bbbc97f9..e18c5331 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectoryState.cpp @@ -25,10 +25,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::interpolate( t)); lerpedState.linearVelocity = GeometryUtil::unitLerp(linearVelocity, endVal.linearVelocity, t); - for (size_t m = 0; m < feedforwards.size(); m++) { - lerpedState.feedforwards.emplace_back( - feedforwards[m].interpolate(endVal.feedforwards[m], t)); - } + lerpedState.feedforwards = feedforwards.interpolate(endVal.feedforwards, t); return lerpedState; } @@ -46,9 +43,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::reverse() const { reversed.pose = frc::Pose2d(pose.Translation(), pose.Rotation() + frc::Rotation2d(180_deg)); reversed.linearVelocity = -linearVelocity; - for (auto ff : feedforwards) { - reversed.feedforwards.emplace_back(ff.reverse()); - } + reversed.feedforwards = feedforwards.reverse(); return reversed; } @@ -60,7 +55,7 @@ PathPlannerTrajectoryState PathPlannerTrajectoryState::flip() const { flipped.linearVelocity = linearVelocity; flipped.pose = FlippingUtil::flipFieldPose(pose); flipped.fieldSpeeds = FlippingUtil::flipFieldSpeeds(fieldSpeeds); - flipped.feedforwards = FlippingUtil::flipFeedforwards(feedforwards); + flipped.feedforwards = feedforwards.flip(); return flipped; } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h b/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h index c2fca1c2..47b9dea9 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h @@ -17,7 +17,7 @@ #include "pathplanner/lib/path/PathPlannerPath.h" #include "pathplanner/lib/config/RobotConfig.h" #include "pathplanner/lib/controllers/PathFollowingController.h" -#include "pathplanner/lib/util/DriveFeedforward.h" +#include "pathplanner/lib/util/DriveFeedforwards.h" #include "pathplanner/lib/util/FlippingUtil.h" namespace pathplanner { @@ -46,8 +46,7 @@ class AutoBuilder { static void configure(std::function poseSupplier, std::function resetPose, std::function robotRelativeSpeedsSupplier, - std::function< - void(frc::ChassisSpeeds, std::vector)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Subsystem *driveSubsystem); diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h index 50852cf3..6585fc68 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h @@ -19,7 +19,7 @@ #include "pathplanner/lib/util/PathPlannerLogging.h" #include "pathplanner/lib/util/PPLibTelemetry.h" #include "pathplanner/lib/events/EventScheduler.h" -#include "pathplanner/lib/util/DriveFeedforward.h" +#include "pathplanner/lib/util/DriveFeedforwards.h" namespace pathplanner { class FollowPathCommand: public frc2::CommandHelper path, std::function poseSupplier, std::function speedsSupplier, - std::function< - void(frc::ChassisSpeeds, std::vector)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements); @@ -64,7 +63,7 @@ class FollowPathCommand: public frc2::CommandHelper m_originalPath; std::function m_poseSupplier; std::function m_speedsSupplier; - std::function)> m_output; + std::function m_output; std::shared_ptr m_controller; RobotConfig m_robotConfig; std::function m_shouldFlipPath; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h index ad3b8bf4..dd26ef8e 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindThenFollowPath.h @@ -29,8 +29,7 @@ class PathfindThenFollowPath: public frc2::SequentialCommandGroup { PathConstraints pathfindingConstraints, std::function poseSupplier, std::function currentRobotRelativeSpeeds, - std::function< - void(frc::ChassisSpeeds, std::vector)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements) { diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h index caf2df2b..3d90a9d4 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h @@ -17,7 +17,7 @@ #include "pathplanner/lib/config/RobotConfig.h" #include "pathplanner/lib/util/PathPlannerLogging.h" #include "pathplanner/lib/util/PPLibTelemetry.h" -#include "pathplanner/lib/util/DriveFeedforward.h" +#include "pathplanner/lib/util/DriveFeedforwards.h" namespace pathplanner { class PathfindingCommand: public frc2::CommandHelper poseSupplier, std::function speedsSupplier, - std::function< - void(frc::ChassisSpeeds, std::vector)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, std::function shouldFlipPath, frc2::Requirements requirements); @@ -73,8 +72,7 @@ class PathfindingCommand: public frc2::CommandHelper poseSupplier, std::function speedsSupplier, - std::function< - void(frc::ChassisSpeeds, std::vector)> output, + std::function output, std::shared_ptr controller, RobotConfig robotConfig, frc2::Requirements requirements); @@ -95,7 +93,7 @@ class PathfindingCommand: public frc2::CommandHelper m_poseSupplier; std::function m_speedsSupplier; - std::function)> m_output; + std::function m_output; std::shared_ptr m_controller; RobotConfig m_robotConfig; std::function m_shouldFlipPath; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h b/pathplannerlib/src/main/native/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h index 1b9e49eb..38dda8ab 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/trajectory/PathPlannerTrajectoryState.h @@ -11,7 +11,7 @@ #include "pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h" #include "pathplanner/lib/path/PathConstraints.h" #include "pathplanner/lib/util/GeometryUtil.h" -#include "pathplanner/lib/util/DriveFeedforward.h" +#include "pathplanner/lib/util/DriveFeedforwards.h" namespace pathplanner { class PathPlannerTrajectoryState { @@ -20,7 +20,7 @@ class PathPlannerTrajectoryState { frc::ChassisSpeeds fieldSpeeds; frc::Pose2d pose; units::meters_per_second_t linearVelocity = 0_mps; - std::vector feedforwards; + DriveFeedforwards feedforwards; frc::Rotation2d heading; units::meter_t deltaPos = 0_m; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforward.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforward.h deleted file mode 100644 index ac1ecd22..00000000 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforward.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include -#include -#include "pathplanner/lib/util/GeometryUtil.h" - -namespace pathplanner { -struct DriveFeedforward { - /** - * Linear acceleration at the wheel in meters per second - */ - units::meters_per_second_squared_t acceleration = 0_mps_sq; - - /** - * Linear force applied by the motor at the wheel in newtons - */ - units::newton_t force = 0_N; - - /** - * Torque-current of the motor in amps - */ - units::ampere_t torqueCurrent = 0_A; - - constexpr DriveFeedforward interpolate(const DriveFeedforward &endValue, - const double t) const { - return DriveFeedforward { GeometryUtil::unitLerp(acceleration, - endValue.acceleration, t), GeometryUtil::unitLerp(force, - endValue.force, t), GeometryUtil::unitLerp(torqueCurrent, - endValue.torqueCurrent, t) }; - } - - constexpr DriveFeedforward reverse() const { - return DriveFeedforward { -acceleration, -force, -torqueCurrent }; - } -}; -} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforwards.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforwards.h new file mode 100644 index 00000000..32d83aa4 --- /dev/null +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/DriveFeedforwards.h @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "pathplanner/lib/util/GeometryUtil.h" +#include "pathplanner/lib/util/FlippingUtil.h" + +namespace pathplanner { +struct DriveFeedforwards { +public: + std::vector accelerations; + std::vector linearForces; + std::vector torqueCurrents; + std::vector robotRelativeForcesX; + std::vector robotRelativeForcesY; + + /** + * Create drive feedforwards consisting of all zeros + * + * @param numModules Number of drive modules + * @return Zero feedforwards + */ + static inline DriveFeedforwards zeros(const size_t numModules) { + return DriveFeedforwards { std::vector + < units::meters_per_second_squared_t > (numModules, 0_mps_sq), + std::vector < units::newton_t > (numModules, 0_N), std::vector + < units::ampere_t > (numModules, 0_A), std::vector + < units::newton_t > (numModules, 0_N), std::vector + < units::newton_t > (numModules, 0_N) }; + } + + inline DriveFeedforwards interpolate(const DriveFeedforwards &endVal, + const double t) const { + return DriveFeedforwards { interpolateVector(accelerations, + endVal.accelerations, t), interpolateVector(linearForces, + endVal.linearForces, t), interpolateVector(torqueCurrents, + endVal.torqueCurrents, t), interpolateVector( + robotRelativeForcesX, endVal.robotRelativeForcesX, t), + interpolateVector(robotRelativeForcesY, + endVal.robotRelativeForcesY, t) }; + } + + /** + * Reverse the feedforwards for driving backwards. This should only be used for differential drive + * robots. + * + * @return Reversed feedforwards + */ + inline DriveFeedforwards reverse() const { + if (accelerations.size() != 2) { + throw std::runtime_error( + "Feedforwards should only be reversed for differential drive trains"); + } + + return DriveFeedforwards { std::vector< + units::meters_per_second_squared_t> { -accelerations[1], + -accelerations[0] }, std::vector { + -linearForces[1], -linearForces[0] }, std::vector< + units::ampere_t> { -torqueCurrents[1], -torqueCurrents[0] }, + std::vector { -robotRelativeForcesX[1], + -robotRelativeForcesX[0] }, + std::vector { -robotRelativeForcesY[1], + -robotRelativeForcesY[0] } }; + } + + /** + * Flip the feedforwards for the other side of the field. Only does anything if mirrored symmetry + * is used + * + * @return Flipped feedforwards + */ + inline DriveFeedforwards flip() const { + return DriveFeedforwards { FlippingUtil::flipFeedforwards( + accelerations), FlippingUtil::flipFeedforwards(linearForces), + FlippingUtil::flipFeedforwards(torqueCurrents), + FlippingUtil::flipFeedforwards(robotRelativeForcesX), + FlippingUtil::flipFeedforwards(robotRelativeForcesY) }; + } + +private: + template::value>> + static constexpr std::vector interpolateVector( + const std::vector &a, const std::vector &b, + const double t) { + std::vector ret; + for (size_t i = 0; i < a.size(); i++) { + ret.emplace_back(GeometryUtil::unitLerp(a[i], b[i], t)); + } + return ret; + } +}; +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/FlippingUtil.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/FlippingUtil.h index db818835..5c76d1fb 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/FlippingUtil.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/FlippingUtil.h @@ -5,7 +5,6 @@ #include #include #include -#include "pathplanner/lib/util/DriveFeedforward.h" namespace pathplanner { class FlippingUtil { @@ -83,23 +82,20 @@ class FlippingUtil { } } - static inline std::vector flipFeedforwards( - const std::vector &feedforwards) { + template::value>> + static inline std::vector flipFeedforwards( + const std::vector &feedforwards) { switch (symmetryType) { case kRotational: return feedforwards; case kMirrored: default: if (feedforwards.size() == 4) { - std::vector < DriveFeedforward > flipped; - flipped.emplace_back(feedforwards[1]); - flipped.emplace_back(feedforwards[0]); - flipped.emplace_back(feedforwards[3]); - flipped.emplace_back(feedforwards[2]); + return std::vector { feedforwards[1], feedforwards[0], + feedforwards[3], feedforwards[2] }; } else if (feedforwards.size() == 2) { - std::vector < DriveFeedforward > flipped; - flipped.emplace_back(feedforwards[1]); - flipped.emplace_back(feedforwards[0]); + return std::vector { feedforwards[1], feedforwards[0] }; } return feedforwards; // idk }