Load module X/Y forces from Choreo paths (#887)

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)
 

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);
       }

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.
  *
+ * <p>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.
    *
+   * <p>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

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<double>() };
 		units::radians_per_second_t angularVelRps { s.at("omega").get<double>() };
 
+		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<double>());
+			forcesY.emplace_back(fy[i].get<double>());
+		}
+
 		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);
 	}