Skip to content

[wpimath] Fix trapezoid profile#8556

Open
cb144p wants to merge 40 commits into
wpilibsuite:mainfrom
cb144p:fix-trapezoid-profile
Open

[wpimath] Fix trapezoid profile#8556
cb144p wants to merge 40 commits into
wpilibsuite:mainfrom
cb144p:fix-trapezoid-profile

Conversation

@cb144p

@cb144p cb144p commented Jan 8, 2026

Copy link
Copy Markdown

Closes #7917.

This PR rewrites the trapezoid profile implementations in WPILib to use a phase space formulation. In doing so, the API is updated to match the exponential profile implementations. The following changes to functionality have been made:

  • Profiles with large differences in velocity and small differences in position between the initial and final states are handled correctly.
  • Profiles where the initial and final states form the minimum time profile for the initial and final velocities are now always handled correctly.
    • See TrapezoidProfile.md for more details.
  • The TimeLeftUntil method has been updated to take two states instead of a distance from the last state passed into the Calculate method. This functionality matches that of the exponential profile.

The tests have been updated to check for these bugs and to reflect the API change. The algorithm used in the new implementation has been documented in TrapezoidProfile.md.

Additionally, the LinearSystemLoopTest.java file has been updated to properly use the TrapezoidProfile class.

@cb144p cb144p requested a review from a team as a code owner January 8, 2026 23:44
@github-actions github-actions Bot added component: wpimath Math library 2027 2027 target labels Jan 8, 2026
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/algorithms.md Outdated
@PeterJohnson PeterJohnson changed the base branch from 2027 to main May 3, 2026 17:56
@cb144p cb144p force-pushed the fix-trapezoid-profile branch from 062aeae to eb8b150 Compare May 22, 2026 15:14
Comment thread wpimath/algorithms/TrapezoidProfile/TrapezoidProfile.md Outdated
Comment thread wpimath/algorithms/TrapezoidProfile/TrapezoidProfile.md
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
@cb144p cb144p force-pushed the fix-trapezoid-profile branch from 3b376d2 to 47077ca Compare July 8, 2026 20:15
Comment thread wpimath/algorithms.md Outdated
Comment thread wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java Outdated
Comment thread wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java Outdated
Comment thread wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp Outdated
Comment thread wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp Outdated
Comment thread wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
Comment thread wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp Outdated
@calcmogul

calcmogul commented Jul 10, 2026

Copy link
Copy Markdown
Member

There's merge conflicts in the Python tests.

@cb144p cb144p force-pushed the fix-trapezoid-profile branch from 4e53ffa to cc43549 Compare July 10, 2026 13:19
Comment thread wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp Outdated
Comment thread wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java Outdated
@cb144p cb144p force-pushed the fix-trapezoid-profile branch from 295bc53 to 03c87e7 Compare July 10, 2026 21:44
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

2027 2027 target build Monorepo organization, Gradle/CMake/Bazel, upstream_utils, etc component: wpimath Math library type: testing Related to testing.

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Rewrite TrapezoidProfile internals to use phase space formulation

3 participants