diff --git a/pathplannerlib/build.gradle b/pathplannerlib/build.gradle index 6809929c..05d811b5 100644 --- a/pathplannerlib/build.gradle +++ b/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 } 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 7f3d3f64..8803d4cf 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/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 diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java new file mode 100644 index 00000000..3f848abe --- /dev/null +++ b/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 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 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 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 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; + } + } + +}