Fix oscillating velocity for differential drive

Also attempt to add unit tests to exercise this code, but they are
Windows-specific at the moment (needs fix!)

pathplannerlib/build.gradle

@@ -8,6 +8,7 @@ plugins {
     id 'edu.wpi.first.GradleVsCode' version '2.1.0'
     id 'com.diffplug.spotless' version '6.11.0'
     id 'jacoco'
+    id 'com.adarshr.test-logger' version '4.0.0'
 }
 
 // WPILib Version
@@ -27,6 +28,16 @@ if (project.hasProperty('releaseMode')) {
     wpilibRepositories.addAllDevelopmentRepositories(project)
 }
 
+testlogger {
+    showStandardStreams true
+}
+
+configurations {
+    testJNIZips {
+        canBeResolved = true
+    }
+}
+
 // Apply C++ configuration
 apply from: 'config.gradle'
 
@@ -49,6 +60,12 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter-api:5.9.0'
     testImplementation 'org.junit.jupiter:junit-jupiter-params:5.9.0'
+
+    testJNIZips "edu.wpi.first.ntcore:ntcore-cpp:$wpilibVersion:windowsx86-64@zip"
+    testJNIZips "edu.wpi.first.hal:hal-cpp:$wpilibVersion:windowsx86-64@zip"
+    testJNIZips "edu.wpi.first.wpinet:wpinet-cpp:$wpilibVersion:windowsx86-64@zip"
+    testJNIZips "edu.wpi.first.wpiutil:wpiutil-cpp:$wpilibVersion:windowsx86-64@zip"
+
     testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.9.0'
     testRuntimeOnly "us.hebi.quickbuf:quickbuf-runtime:1.3.2"
 }
@@ -177,8 +194,15 @@ jacocoTestReport {
     }
 }
 
+def extractTestJNILibs = tasks.register('extractTestJNILibs', Copy) {
+    from { configurations.testJNIZips.collect { zipTree(it) } }
+    include '**/*.dll'
+    into "${buildDir}/test-native-libs"
+}
+
 test {
+    dependsOn extractTestJNILibs
     useJUnitPlatform()
-
+    systemProperties['java.library.path'] = "${buildDir}/test-native-libs/windows/x86-64/shared"
     finalizedBy jacocoTestReport
 }

pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java

@@ -527,7 +527,7 @@ private static void reverseAccelPass(
               state.pose.getRotation());
       var accelStates = config.toSwerveModuleStates(chassisAccel);
       for (int m = 0; m < config.numModules; m++) {
-        double moduleAcceleration = accelStates[m].speedMetersPerSecond;
+        double moduleAcceleration = Math.abs(accelStates[m].speedMetersPerSecond);
 
         // Calculate the module velocity at the current state
         // vf^2 = v0^2 + 2ad

pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java

@@ -0,0 +1,94 @@
+package com.pathplanner.lib.trajectory;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.IdealStartingState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.Waypoint;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.system.plant.DCMotor;
+
+public class PathPlannerTrajectoryTest {
+  @Test
+  public void testGenerateTrajectoryHolonomic() {
+    RobotConfig holonomicConfig = new RobotConfig(74.088, 6.883,
+        new ModuleConfig(0.048, 5.45, 1.2, DCMotor.getKrakenX60(1).withReduction(5.143), 60, 4),
+        new Translation2d(0.273, 0.273),
+        new Translation2d(0.273, -0.273),
+        new Translation2d(-0.273, 0.273),
+        new Translation2d(-0.273, -0.273));
+    List<Waypoint> waypoints = Arrays.asList(
+        new Waypoint(null, new Translation2d(), new Translation2d(1.0, 0.0)),
+        new Waypoint(new Translation2d(5.0, 0.0), new Translation2d(6.0, 0.0), null));
+    PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 540.0, 720.0);
+    IdealStartingState startingState = new IdealStartingState(MetersPerSecond.zero(), new Rotation2d());
+    GoalEndState endState = new GoalEndState(MetersPerSecond.zero(), new Rotation2d());
+    PathPlannerPath path = new PathPlannerPath(waypoints, pathConstraints, startingState, endState);
+    PathPlannerTrajectory trajectory = new PathPlannerTrajectory(path, new ChassisSpeeds(), new Rotation2d(),
+        holonomicConfig);
+    List<PathPlannerTrajectoryState> states = trajectory.getStates();
+    assertTrue(states.size() > 10);
+    double lastLV = 0;
+    for (int i = 0; i < states.size(); i++) {
+      double linearVelocity = states.get(i).moduleStates[0].speedMetersPerSecond;
+      assertTrue(linearVelocity <= pathConstraints.maxVelocityMPS());
+      if (i > 0) {
+        if (i < states.size() / 2) {
+          // Verify that we are more or less monotonically increasing for the first half.
+          assertTrue(linearVelocity - lastLV >= -0.001);
+        } else {
+          // and decreasing for the second half.
+          assertTrue(linearVelocity - lastLV <= 0.001);
+        }
+      }
+      lastLV = linearVelocity;
+    }
+  }
+
+  @Test
+  public void testGenerateTrajectoryDifferential() {
+    RobotConfig differentialConfig = new RobotConfig(74.088, 6.883,
+        new ModuleConfig(0.048, 5.45, 1.2, DCMotor.getKrakenX60(1).withReduction(5.143), 60, 2),
+        0.56);
+    List<Waypoint> waypoints = Arrays.asList(
+        new Waypoint(null, new Translation2d(), new Translation2d(1.0, 0.0)),
+        new Waypoint(new Translation2d(5.0, 0.0), new Translation2d(6.0, 0.0), null));
+    PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 540.0, 720.0);
+    IdealStartingState startingState = new IdealStartingState(MetersPerSecond.zero(), new Rotation2d());
+    GoalEndState endState = new GoalEndState(MetersPerSecond.zero(), new Rotation2d());
+    PathPlannerPath path = new PathPlannerPath(waypoints, pathConstraints, startingState, endState);
+    PathPlannerTrajectory trajectory = new PathPlannerTrajectory(path, new ChassisSpeeds(), new Rotation2d(),
+        differentialConfig);
+    List<PathPlannerTrajectoryState> states = trajectory.getStates();
+    assertTrue(states.size() > 10);
+    double lastLV = 0;
+    for (int i = 0; i < states.size(); i++) {
+      double linearVelocity = states.get(i).moduleStates[0].speedMetersPerSecond;
+      assertTrue(linearVelocity <= pathConstraints.maxVelocityMPS());
+      if (i > 0) {
+        if (i < states.size() / 2) {
+          // Verify that we are more or less monotonically increasing for the first half.
+          assertTrue(linearVelocity - lastLV >= -0.001);
+        } else {
+          // and decreasing for the second half.
+          assertTrue(linearVelocity - lastLV <= 0.001);
+        }
+      }
+      lastLV = linearVelocity;
+    }
+  }
+
+}