diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index f7261c87..efe02fa1 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -542,11 +542,34 @@ def _loadChoreoTrajectoryIntoCache(trajectory_name: str) -> None: yVel = float(s['vy']) angularVelRps = float(s['omega']) + fx = s['fx'] + fy = s['fy'] + forcesX = [] + forcesY = [] + for i in range(len(fx)): + forcesX.append(float(fx[i])) + forcesY.append(float(fy[i])) + state.timeSeconds = time state.linearVelocity = math.hypot(xVel, yVel) state.pose = Pose2d(xPos, yPos, rotationRad) state.fieldSpeeds = ChassisSpeeds(xVel, yVel, angularVelRps) - state.feedforwards = DriveFeedforwards.zeros(4) + + # The module forces are field relative, rotate them to be robot relative + for i in range(len(forcesX)): + rotated = Translation2d(forcesX[i], forcesY[i]).rotateBy(-state.pose.rotation()) + forcesX[i] = rotated.x + forcesY[i] = rotated.y + + # All other feedforwards besides X and Y components will be zeros because they cannot be + # calculated without RobotConfig + state.feedforwards = DriveFeedforwards( + [0.0] * len(forcesX), + [0.0] * len(forcesX), + [0.0] * len(forcesX), + forcesX, + forcesY + ) fullTrajStates.append(state) 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 c12ef96d..706848c3 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -371,11 +371,38 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName) double yVel = ((Number) sample.get("vy")).doubleValue(); double angularVelRps = ((Number) sample.get("omega")).doubleValue(); + JSONArray fx = (JSONArray) sample.get("fx"); + JSONArray fy = (JSONArray) sample.get("fy"); + double[] forcesX = new double[fx.size()]; + double[] forcesY = new double[fy.size()]; + for (int i = 0; i < fx.size(); i++) { + forcesX[i] = ((Number) fx.get(i)).doubleValue(); + forcesY[i] = ((Number) fy.get(i)).doubleValue(); + } + state.timeSeconds = time; 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 = DriveFeedforwards.zeros(4); + + // The module forces are field relative, rotate them to be robot relative + for (int i = 0; i < forcesX.length; i++) { + Translation2d rotated = + new Translation2d(forcesX[i], forcesY[i]) + .rotateBy(state.pose.getRotation().unaryMinus()); + forcesX[i] = rotated.getX(); + forcesY[i] = rotated.getY(); + } + + // All other feedforwards besides X and Y components will be zeros because they cannot be + // calculated without RobotConfig + state.feedforwards = + new DriveFeedforwards( + new double[forcesX.length], + new double[forcesX.length], + new double[forcesX.length], + forcesX, + forcesY); fullTrajStates.add(state); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java index 8f285a3a..0c6768b8 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/DriveFeedforwards.java @@ -13,6 +13,9 @@ * 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. * + *

NOTE: If using Choreo paths, all feedforwards but the X and Y component arrays will be filled + * with zeros. + * * @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 @@ -35,6 +38,9 @@ public record DriveFeedforwards( * will all be in FL, FR, BL, BR order. If using a differential drive, these will be in L, R * order. * + *

NOTE: If using Choreo paths, all feedforwards but the X and Y component arrays will be + * filled with zeros. + * * @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 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 19cc3efb..e3f739a4 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -181,12 +181,39 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( units::meters_per_second_t yVel { s.at("vy").get() }; units::radians_per_second_t angularVelRps { s.at("omega").get() }; + auto fx = s.at("fx"); + auto fy = s.at("fy"); + + std::vector < units::newton_t > forcesX; + std::vector < units::newton_t > forcesY; + for (size_t i = 0; i < fx.size(); i++) { + forcesX.emplace_back(fx[i].get()); + forcesY.emplace_back(fy[i].get()); + } + state.time = time; state.linearVelocity = units::math::hypot(xVel, yVel); state.pose = frc::Pose2d(frc::Translation2d(xPos, yPos), frc::Rotation2d(rotationRad)); state.fieldSpeeds = frc::ChassisSpeeds { xVel, yVel, angularVelRps }; - state.feedforwards = DriveFeedforwards::zeros(4); + + // The module forces are field relative, rotate them to be robot relative + for (size_t i = 0; i < forcesX.size(); i++) { + frc::Translation2d rotated = frc::Translation2d(units::meter_t { + forcesX[i]() }, units::meter_t { forcesY[i]() }).RotateBy( + -state.pose.Rotation()); + forcesX[i] = units::newton_t { rotated.X()() }; + forcesY[i] = units::newton_t { rotated.Y()() }; + } + + // All other feedforwards besides X and Y components will be zeros because they cannot be + // calculated without RobotConfig + state.feedforwards = DriveFeedforwards( + std::vector < units::meters_per_second_squared_t + > (forcesX.size(), 0_mps_sq), + std::vector < units::newton_t > (forcesX.size(), 0_N), + std::vector < units::ampere_t > (forcesX.size(), 0_A), forcesX, + forcesY); fullTrajStates.emplace_back(state); }