Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Optimize 2nd derivative of quintic splines (#3292)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Prateek Machiraju
2023-11-30 21:07:52 -08:00
committed by GitHub
parent 4fcf0b25a1
commit 51eecef2bd
16 changed files with 336 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
View File

@@ -172,8 +172,8 @@ class MecanumDriveOdometryTest {
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, maxError, 0.3, "Incorrect max error");
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error");
assertEquals(0.0, maxError, 0.35, "Incorrect max error");
assertEquals(
1.0,
odometryDistanceTravelled / trajectoryDistanceTravelled,

18
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.math.trajectory;
import static edu.wpi.first.math.util.Units.feetToMeters;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.geometry.Pose2d;
@@ -80,4 +81,21 @@ class TrajectoryGeneratorTest {
assertEquals(traj.getStates().size(), 1);
assertEquals(traj.getTotalTimeSeconds(), 0);
}
@Test
void testQuinticCurvatureOptimization() {
Trajectory t =
TrajectoryGenerator.generateTrajectory(
Arrays.asList(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(270)),
new Pose2d(0, -1, Rotation2d.fromDegrees(360)),
new Pose2d(1, 0, Rotation2d.fromDegrees(90))),
new TrajectoryConfig(2, 2));
for (int i = 1; i < t.getStates().size() - 1; ++i) {
assertNotEquals(0, t.getStates().get(i).curvatureRadPerMeter);
}
}
}

14
wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
View File

@@ -41,3 +41,17 @@ TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
ASSERT_EQ(t.States().size(), 1u);
ASSERT_EQ(t.TotalTime(), 0_s);
}
TEST(TrajectoryGenerationTest, CurvatureOptimization) {
auto t = TrajectoryGenerator::GenerateTrajectory(
{{1_m, 0_m, 90_deg},
{0_m, 1_m, 180_deg},
{-1_m, 0_m, 270_deg},
{0_m, -1_m, 0_deg},
{1_m, 0_m, 90_deg}},
TrajectoryConfig{12_fps, 12_fps_sq});
for (size_t i = 1; i < t.States().size() - 1; ++i) {
EXPECT_NE(0, t.States()[i].curvature.to<double>());
}
}