[wpimath] Replace Trajectory API with new one by zachwaffle4 · Pull Request #8172 · wpilibsuite/allwpilib

zachwaffle4 · 2025-08-12T16:13:08Z

The new API is based on this document.

Java

C++

Notes

This PR does not remove the old trajectory generators, even though they aren't really used. The TrajectoryGenerator class was refactored to produce Trajectory<SplineSample>, where SplineSample replaces the old Trajectory.State class. I did that because many tests rely on the old Trajectory class (such as kinematics and Kalman filters), so they have all been updated to use Trajectory<SplineSample instead; in another PR I will move SplineSample and the old TrajectoryGenerator/Config/etc classes into the test source set.

github-actions · 2025-08-12T16:13:17Z

This PR modifies commands. Please open a corresponding PR in Python Commands and include a link to this PR.

… new api Signed-off-by: Zach Harel <zach@zharel.me>

Signed-off-by: Zach Harel <zach@zharel.me>

# Conflicts: # wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java # wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java # wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java # wpimath/src/main/java/edu/wpi/first/math/trajectory/DifferentialSample.java # wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java # wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java # wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java # wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectorySample.java # wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java # wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java # wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java # wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java # wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java # wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java # wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java # wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java

Signed-off-by: Zach Harel <zach@zharel.me>

…rential drive kinematics, voltage, and region enforcement and their relevant tests Signed-off-by: Zach Harel <zach@zharel.me>

zachwaffle4 · 2025-08-16T09:32:13Z

i am very sorry that the git history is kinda cursed because of how i fixed the tests/commands

…sentation; implement interpolation methods and kinematics handling Signed-off-by: Zach Harel <zach@zharel.me>

…assisSpeeds classes Signed-off-by: Zach Harel <zach@zharel.me>

…jectory data handling Signed-off-by: Zach Harel <zach@zharel.me>

…ve fromSample method, and make transform abstract Signed-off-by: Zach Harel <zach@zharel.me>

…nt interpolation for SwerveModuleState Signed-off-by: Zach Harel <zach@zharel.me>

…r accelerations) and make both classes interpolatable Signed-off-by: Zach Harel <zach@zharel.me>

Signed-off-by: Zach Harel <zach@zharel.me>

calcmogul · 2025-08-18T19:12:41Z

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectorySample.java

+        new ChassisSpeeds(
+            vel.vx + accel.ax * dts, vel.vy + accel.ay * dts, vel.omega + accel.alpha * dts);
+
+    var newPose = pose.exp(new Twist2d(newVel.vx * dts, newVel.vy * dts, newVel.omega * dts));


The x and y velocities are typically in the global frame, so the pose exponential won't give correct results.

The vel and accel properties are (supposed to) be in the robot frame.

gradle/wrapper/gradle-wrapper.properties

calcmogul · 2025-08-18T19:15:43Z

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectorySample.java

+  public final Pose2d pose;
+
+  /** The robot velocity at this sample (in the robot's reference frame). */
+  public final ChassisSpeeds vel;


Suggested change

public final ChassisSpeeds vel;

public final ChassisSpeeds velocity;

I think the abbreviations here are fine

I disagree. We're not code golfing here. You can spare 5 more letters.

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectorySample.java

wpimath/src/main/java/org/wpilib/math/trajectory/TrajectorySample.java

wpimath/src/main/java/edu/wpi/first/math/trajectory/DifferentialSample.java

calcmogul · 2025-08-18T19:34:03Z

wpimath/src/main/java/edu/wpi/first/math/trajectory/DifferentialSample.java

+    double rightAccel = input.get(1);
+
+    double vx = (vl + vr) / 2.0;
+    double trackwidth = (vr - vl) / omega;


This is numerically ill-conditioned (divide-by-zero) if the robot isn't rotating. In this expression, trackwidth is undefined if omega = 0. If you substitute in trackwidth and simplify, alpha is undefined if vr - vl = 0.

The ill-conditioning can be avoided by getting the trackwidth directly from the trajectory generator, but I don't see a clean way to do that off the top of my head.

I can set alpha to 0 if either (vr-vl) or omega = 0 but other than that I don't know how else to resolve this

If vr - vl = 0, ω = 0, and ar - al ≠ 0, then the trackwidth will be NaN (0/0) while α should be nonzero. The indeterminate value for trackwidth can be resolved with L'Hôpital's rule, but that gives (ar - al)/α, which contains the value we're trying to compute.

Seems like the best fix is to treat alpha as a model input, then have the trajectory generator provide that in the sample since the trajectory generator knows the trackwidth. Including the angular acceleration along with the wheel linear accelerations is analogous to including the angular velocity along with the wheel linear velocities.

I just fixed this in Choreo: SleipnirGroup/Choreo#1301.

wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java

Signed-off-by: Zach Harel <zach@zharel.me>

…for clarity Signed-off-by: Zach Harel <zach@zharel.me>

…djust dynamics methods Signed-off-by: Zach Harel <zach@zharel.me>

…use it already had the protobuf) and fix toString format Signed-off-by: Zach Harel <zach@zharel.me>

Signed-off-by: Zach Harel <zach@zharel.me>

…celerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <zach@zharel.me>

…anumDriveWheelAccelerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <zach@zharel.me>

# Conflicts: # wpimath/robotpy_pybind_build_info.bzl # wpimath/src/main/python/semiwrap/DifferentialSample.yml # wpimath/src/main/python/semiwrap/DifferentialTrajectory.yml # wpimath/src/main/python/semiwrap/HolonomicTrajectory.yml # wpimath/src/main/python/semiwrap/SplineSample.yml # wpimath/src/main/python/semiwrap/SplineTrajectory.yml # wpimath/src/main/python/semiwrap/TrajectorySample.yml

…tting

…includes

…om TrajectorySample and add new attributes

…ories

…mples from Trajectory

…related methods

…tory types

…tead of Time

# Conflicts: # wpimath/src/generated/main/java/org/wpilib/math/proto/Trajectory.java # wpimath/src/generated/main/native/cpp/wpimath/protobuf/trajectory.npb.cpp # wpimath/src/main/java/org/wpilib/math/trajectory/proto/TrajectoryProto.java # wpimath/src/main/java/org/wpilib/math/trajectory/proto/TrajectoryStateProto.java # wpimath/src/main/proto/trajectory.proto # wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java # wpimath/src/test/java/org/wpilib/math/trajectory/proto/TrajectoryProtoTest.java # wpimath/src/test/java/org/wpilib/math/trajectory/proto/TrajectoryStateProtoTest.java

calcmogul · 2026-02-17T21:45:25Z

wpimath/src/generated/main/java/org/wpilib/math/proto/ProtobufTrajectoryState.java

+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.


There's a lot of duplicate copyright headers.

yeah i think i broke something with the generated quickbuf files

calcmogul · 2026-02-17T21:47:38Z

wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp

 }

-void FieldObject2d::SetTrajectory(const wpi::math::Trajectory& trajectory) {
+template <typename SampleType>


Template function definitions need to be in the header instead.

oops (for context this was originally not a template function and so i just. updated the function definition in here)

calcmogul · 2026-02-17T21:49:45Z

wpimath/src/main/java/org/wpilib/math/controller/LTVDifferentialDriveController.java

    // v = (v_r + v_l) / 2     (1)
    // w = (v_r - v_l) / (2r)  (2)
    // k = w / v               (3)
    //
    // v_l = v - wr
    // v_l = v - (vk)r
    // v_l = v(1 - kr)
    //
    // v_r = v + wr
    // v_r = v + (vk)r
    // v_r = v(1 + kr)


This derivation is no longer used and can be removed.

calcmogul · 2026-02-17T21:51:05Z

wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java

  public boolean equals(Object o) {
    return o == this
-        || o instanceof ChassisAccelerations c && ax == c.ax && ay == c.ay && alpha == c.alpha;
+        || o instanceof ChassisAccelerations c


This seems unrelated to the PR and should be separate.

This is required such that NaN accelerations compare equal so that the tests pass with the spline trajectory generators.

calcmogul · 2026-02-17T21:51:27Z

wpimath/src/main/java/org/wpilib/math/trajectory/DifferentialSample.java

+   * @param leftSpeed The left-wheel speed at this sample.
+   * @param rightSpeed The right-wheel speed at this sample.


Suggested change

* @param leftSpeed The left-wheel speed at this sample.

* @param rightSpeed The right-wheel speed at this sample.

* @param leftSpeed The left wheel speed at this sample.

* @param rightSpeed The right wheel speed at this sample.

calcmogul · 2026-02-17T22:01:24Z

wpimath/src/main/native/include/wpi/math/trajectory/SplineSample.hpp

+   * @param pose The robot pose at this sample (in the field reference frame).
+   * @param linearVelocity The linear velocity in m/s.
+   * @param linearAcceleration The linear acceleration in m/s².
+   * @param curvature The curvature of the path at this sample, in 1/m.


Suggested change

* @param curvature The curvature of the path at this sample, in 1/m.

* @param curvature The curvature of the path at this sample, in rad/m.

calcmogul · 2026-02-17T22:02:57Z

wpimath/src/main/native/include/wpi/math/trajectory/Trajectory.hpp

-WPILIB_DLLEXPORT
-void to_json(wpi::util::json& json, const Trajectory::State& state);
+ protected:
+  // Helper: make all samples relative to a Pose2d


Need proper docs here. Helper: is redundant.

calcmogul · 2026-02-17T22:03:13Z

wpimath/src/main/native/include/wpi/math/trajectory/Trajectory.hpp

+    return out;
+  }
+
+  // Helper: concatenate with another list of samples, offsetting their


Need proper docs here. Helper: is redundant.

calcmogul · 2026-02-17T22:04:21Z

wpimath/src/main/native/include/wpi/math/trajectory/TrajectorySample.hpp

+      wpi::util::Lerp(start.acceleration.ay, end.acceleration.ay, t),
+      wpi::util::Lerp(start.acceleration.alpha, end.acceleration.alpha, t)};
+
+  // v_{k+1} = v_k + a_k * dt


Suggested change

// v_{k+1} = v_k + a_k * dt

// vₖ₊₁ = vₖ + aₖΔt

calcmogul · 2026-02-17T22:05:29Z

wpimath/src/main/native/include/wpi/math/trajectory/TrajectorySample.hpp

+      start.velocity.vy + start.acceleration.ay * interpT,
+      start.velocity.omega + start.acceleration.alpha * interpT};
+
+  // x_{k+1} = x_k + v_k * dt + 0.5 a (dt)^2


Suggested change

// x_{k+1} = x_k + v_k * dt + 0.5 a (dt)^2

// xₖ₊₁ = xₖ + vₖΔt + ½aₖ(Δt)²

github-actions bot added component: wpilibj WPILib Java component: command-based WPILib Command Based Library component: wpimath Math library component: examples 2027 2027 target labels Aug 12, 2025

zachwaffle4 added 5 commits August 15, 2025 14:27

update old trajectoryparameterizer and trajectorygenerator to use the…

a056288

… new api Signed-off-by: Zach Harel <zach@zharel.me>

update old tests that needed trajectories to use the new api

d1f0df7

Signed-off-by: Zach Harel <zach@zharel.me>

refactor trajectory classes for improved clarity and functionality

7d7c86b

Signed-off-by: Zach Harel <zach@zharel.me>

bring back trajectory constraints for centripetal acceleration, diffe…

6b204fe

…rential drive kinematics, voltage, and region enforcement and their relevant tests Signed-off-by: Zach Harel <zach@zharel.me>

github-actions bot removed the component: command-based WPILib Command Based Library label Aug 15, 2025

zachwaffle4 added 8 commits August 16, 2025 12:46

feat: add MecanumSample and SwerveSample classes for trajectory repre…

6c39d83

…sentation; implement interpolation methods and kinematics handling Signed-off-by: Zach Harel <zach@zharel.me>

feat: implement interpolation methods for ChassisAccelerations and Ch…

05ba329

…assisSpeeds classes Signed-off-by: Zach Harel <zach@zharel.me>

feat: add DifferentialSampleStruct and TrajectorySampleStruct for tra…

7761137

…jectory data handling Signed-off-by: Zach Harel <zach@zharel.me>

feat: enhance trajectory sample classes with JSON serialization, remo…

864e58f

…ve fromSample method, and make transform abstract Signed-off-by: Zach Harel <zach@zharel.me>

feat: add JSON serialization tests for trajectory samples and impleme…

315be49

…nt interpolation for SwerveModuleState Signed-off-by: Zach Harel <zach@zharel.me>

feat: add ChassisAccelerations class (similar to ChassisSpeeds but fo…

38ef95f

…r accelerations) and make both classes interpolatable Signed-off-by: Zach Harel <zach@zharel.me>

add struct for accelerations

a9d58f9

Signed-off-by: Zach Harel <zach@zharel.me>

feat: add protobuf for acceleration

4a81fc8

Signed-off-by: Zach Harel <zach@zharel.me>

calcmogul requested changes Aug 18, 2025

View reviewed changes

zachwaffle4 added 8 commits August 21, 2025 11:44

go back to gradle wrapper 8.14.3

da4111e

Signed-off-by: Zach Harel <zach@zharel.me>

feat: rename computeStateDerivatives to dynamics and update comments …

10a15b6

…for clarity Signed-off-by: Zach Harel <zach@zharel.me>

feat: update DifferentialSample to include angular acceleration and a…

de07dcf

…djust dynamics methods Signed-off-by: Zach Harel <zach@zharel.me>

feat: implement Protobuf serialization for ChassisAccelerations (beca…

d959a87

…use it already had the protobuf) and fix toString format Signed-off-by: Zach Harel <zach@zharel.me>

make spotless happy

c502366

Signed-off-by: Zach Harel <zach@zharel.me>

add c++ versions of the accelerations class/struct/proto

2bb10ae

Signed-off-by: Zach Harel <zach@zharel.me>

feat: create DifferentialDriveWheelAccelerations, MecanumDriveWheelAc…

5c6329c

…celerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <zach@zharel.me>

feat: c++ implementations of DifferentialDriveWheelAccelerations, Mec…

0f6b1e0

…anumDriveWheelAccelerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <zach@zharel.me>

zachwaffle4 added 29 commits January 2, 2026 14:23

put trajectory stuff back into semiwrap config files

689b72e

Refactor TrajectorySample and DifferentialSample for consistent forma…

fd0011a

…tting

format TrajectorySerializationTest and SampleJsonTest

1ae0638

Remove unused constructor from DifferentialTrajectory

9990ca4

Add public access modifier to HolonomicTrajectory class

eeb56eb

Update trajectory YAML files to include method definitions and extra …

e6aa9bd

…includes

remove crtp from Trajectory

4f8dc5d

Update Trajectory.yml

86cb750

Implement numerical integration for DifferentialTrajectory interpolation

48ecb14

Refactor DifferentialSample and SplineSample to remove inheritance fr…

eb20c7c

…om TrajectorySample and add new attributes

Merge remote-tracking branch 'upstream/2027' into trajectory-api

424339d

Add constructor to DifferentialSample for SplineSample conversion

a684ab7

Implement TransformBy for Differential, Holonomic, and Spline traject…

f4e6c89

…ories

Clarify parameter name in WithNewTimestamp documentation

40132a1

Add SplineSample support to DifferentialSample and remove TransformSa…

19f0859

…mples from Trajectory

remove trajectory java protobuf file

387633f

Rename TrajectoryBase to HolonomicTrajectory to match c++ and update …

65a22a1

…related methods

redo trajectory protobuf files

b5ff1fb

implement structs + protobuf for sample types and protobuf for trajec…

8493170

…tory types

implement json stuff for trajectory related items

65f84f2

Add JSON serialization tests for trajectory samples

03c5fde

Merge remote-tracking branch 'upstream/2027' into trajectory-api

e7ae469

Refactor trajectory timestamp and duration handling to use double ins…

af1362c

…tead of Time

Add unit tests for kinematic interpolation in TrajectorySample

cc6bbef

java format

72a6f41

robotpy yaml

4a8d194

rerun protobuf and javaformat after merge

45c1fb8

calcmogul requested changes Feb 17, 2026

View reviewed changes

	public final ChassisSpeeds vel;
	public final ChassisSpeeds velocity;

		* @param leftSpeed The left-wheel speed at this sample.
		* @param rightSpeed The right-wheel speed at this sample.

	* @param curvature The curvature of the path at this sample, in 1/m.
	* @param curvature The curvature of the path at this sample, in rad/m.

	// x_{k+1} = x_k + v_k * dt + 0.5 a (dt)^2
	// xₖ₊₁ = xₖ + vₖΔt + ½aₖ(Δt)²

Comments

Conversation

zachwaffle4 commented Aug 12, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Java

C++

Notes

Uh oh!

github-actions bot commented Aug 12, 2025

Uh oh!

zachwaffle4 commented Aug 16, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

zachwaffle4 Feb 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

zachwaffle4 commented Aug 12, 2025 •

edited

Loading

zachwaffle4 Feb 18, 2026 •

edited

Loading