[wpimath] Add ChassisAccelerations and drivetrain accelerations classes and add forward and inverse kinematics for accelerations to the interface #8185

zachwaffle4 · 2025-08-22T00:17:32Z

ChassisAccelerations and the drivetrain acceleration types are added in both Java and C++. ChassisAccelerations is basically just ChassisSpeeds but for accelerations! DifferentialDriveWheelAccelerations, MecanumDriveWheelAccelerations, and SwerveModuleAccelerations are the acceleration equivalent of the drivetrain speeds types.

In Java, the Kinematics interface now has an additional generic parameter A which represents the accelerations, and toChassisAccelerations and toWheelAccelerations methods, which are implemented the same way as toChassisSpeeds and toWheelSpeeds.

Protobuf and struct classes were also added for all four classes in Java and C++.

…r accelerations) and make both classes interpolatable Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

…use it already had the protobuf) and fix toString format Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

…celerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <[email protected]>

…anumDriveWheelAccelerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

…classes to include new third generic parameter Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

…them and also make gradle happy Signed-off-by: Zach Harel <[email protected]>

KangarooKoala

Do we need to include adding interpolate() to the wheel speeds classes (and wheel accelerations classes) in this PR? It's already pretty large and doesn't seem directly related.

wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java

wpimath/src/main/proto/kinematics.proto

zachwaffle4 · 2025-08-22T01:33:11Z

I included the interpolation here solely because it was also something I wanted to add and I didn't want to make another PR

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

…celeration vs velocity Signed-off-by: Zach Harel <[email protected]>

…erations and related files Signed-off-by: Zach Harel <[email protected]>

…of angularAcceleration Signed-off-by: Zach Harel <[email protected]>

…rAcceleration and then Signed-off-by: Zach Harel <[email protected]>

…swerve (which includes angular velocity due to centripetal acceleration component) Signed-off-by: Zach Harel <[email protected]>

…y in module acceleration calculations Signed-off-by: Zach Harel <[email protected]>

wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

wpimath/src/main/proto/kinematics.proto

…rivekinematics to its own method Signed-off-by: Zach Harel <[email protected]>

…ons for clarity Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp

Signed-off-by: Zach Harel <[email protected]>

…ematic-acceleration # Conflicts: # wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp # wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp # wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp # wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp # wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp # wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp # wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp # wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp # wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp # wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp # wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp # wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp # wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp # wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp # wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp # wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp # wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp

Signed-off-by: Zach Harel <[email protected]>

…y binding configurations Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

KangarooKoala

Was #8185 (comment) ever resolved?

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

wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp

Signed-off-by: Zach Harel <[email protected]>

…pp/reality Signed-off-by: Zach Harel <[email protected]>

Signed-off-by: Zach Harel <[email protected]>

zachwaffle4 · 2025-11-26T22:15:52Z

Was #8185 (comment) ever resolved?

yes (just now)

…istency in robotpy Signed-off-by: Zach Harel <[email protected]>

…and operators Signed-off-by: Zach Harel <[email protected]>

KangarooKoala

So close...

KangarooKoala · 2025-11-27T02:07:40Z

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp

 * @tparam WheelSpeeds Wheel speeds type.
 * @tparam WheelPositions Wheel positions type.


Suggested change

* @tparam WheelSpeeds Wheel speeds type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelSpeeds Wheel speeds type.

KangarooKoala · 2025-11-27T02:08:09Z

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp

 * @tparam WheelSpeeds Wheel speeds type.
 * @tparam WheelPositions Wheel positions type.


Suggested change

* @tparam WheelSpeeds Wheel speeds type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelSpeeds Wheel speeds type.

KangarooKoala · 2025-11-27T02:08:45Z

wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp

 * @tparam WheelSpeeds Wheel speeds type.
 * @tparam WheelPositions Wheel positions type.


Suggested change

* @tparam WheelSpeeds Wheel speeds type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelSpeeds Wheel speeds type.

KangarooKoala · 2025-11-27T02:08:55Z

wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp

 * @tparam WheelSpeeds Wheel speeds type.
 * @tparam WheelPositions Wheel positions type.


Suggested change

* @tparam WheelSpeeds Wheel speeds type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelPositions Wheel positions type.

* @tparam WheelSpeeds Wheel speeds type.

KangarooKoala · 2025-11-27T02:09:28Z

wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java

+  @Override
+  public void pack(ProtobufSwerveModuleAcceleration msg, SwerveModuleAcceleration value) {
+    msg.setAcceleration(value.acceleration);
+    Rotation2d.proto.pack(msg.getAngle(), value.angle);


Suggested change

Rotation2d.proto.pack(msg.getAngle(), value.angle);

Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);

Unpacking doesn't need to be mutable, but packing does (since that's when we modify msg).

KangarooKoala · 2025-11-27T02:10:00Z

wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java

+  @Override
+  public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAcceleration msg) {
+    return new SwerveModuleAcceleration(
+        msg.getAcceleration(), Rotation2d.proto.unpack(msg.getMutableAngle()));


Suggested change

msg.getAcceleration(), Rotation2d.proto.unpack(msg.getMutableAngle()));

msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle()));

This doesn't need to be mutable since we're only unpacking (so we don't modify anything).

zachwaffle4 added 15 commits August 17, 2025 12:26

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

38ef95f

…r accelerations) and make both classes interpolatable Signed-off-by: Zach Harel <[email protected]>

add struct for accelerations

a9d58f9

Signed-off-by: Zach Harel <[email protected]>

feat: add protobuf for acceleration

4a81fc8

Signed-off-by: Zach Harel <[email protected]>

feat: implement Protobuf serialization for ChassisAccelerations (beca…

d959a87

…use it already had the protobuf) and fix toString format Signed-off-by: Zach Harel <[email protected]>

make spotless happy

c502366

Signed-off-by: Zach Harel <[email protected]>

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

2bb10ae

Signed-off-by: Zach Harel <[email protected]>

feat: create DifferentialDriveWheelAccelerations, MecanumDriveWheelAc…

5c6329c

…celerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <[email protected]>

feat: c++ implementations of DifferentialDriveWheelAccelerations, Mec…

0f6b1e0

…anumDriveWheelAccelerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel <[email protected]>

fix: make spotless happy

84cf12d

Signed-off-by: Zach Harel <[email protected]>

add accelerations to the interface

7606950

Signed-off-by: Zach Harel <[email protected]>

fix: update Kinematics type parameters in Odometry and PoseEstimator …

998fc49

…classes to include new third generic parameter Signed-off-by: Zach Harel <[email protected]>

refactor: remove unused methods from ChassisAccelerations class

cb078b5

Signed-off-by: Zach Harel <[email protected]>

test: add unit tests for ChassisAccelerations and related classes

02da263

Signed-off-by: Zach Harel <[email protected]>

test: add unit tests for ChassisAccelerations and related classes

01683b1

Signed-off-by: Zach Harel <[email protected]>

add interpolation methods for wheelspeeds and accelerations and test …

a3a488a

…them and also make gradle happy Signed-off-by: Zach Harel <[email protected]>

zachwaffle4 requested a review from a team as a code owner August 22, 2025 00:17

github-actions bot added component: wpimath Math library 2027 2027 target labels Aug 22, 2025

KangarooKoala reviewed Aug 22, 2025

View reviewed changes

SamCarlberg reviewed Aug 22, 2025

View reviewed changes

wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java Show resolved Hide resolved

zachwaffle4 added 6 commits August 21, 2025 21:57

fix: update documentation in ChassisAccelerations class to clarify ac…

44b3f13

…celeration vs velocity Signed-off-by: Zach Harel <[email protected]>

refactor: replace angularAcceleration with angle in SwerveModuleAccel…

a4e392e

…erations and related files Signed-off-by: Zach Harel <[email protected]>

refactor: update SwerveModuleAccelerations to use Rotation2d instead …

b5ee68d

…of angularAcceleration Signed-off-by: Zach Harel <[email protected]>

refactor: update SwerveDriveKinematics to use angle instead of angula…

3156b9f

…rAcceleration and then Signed-off-by: Zach Harel <[email protected]>

refactor: include the proper second order kinematics calculation for …

2531ceb

…swerve (which includes angular velocity due to centripetal acceleration component) Signed-off-by: Zach Harel <[email protected]>

refactor: update SwerveDriveKinematicsTest to include angular velocit…

5d4d723

…y in module acceleration calculations Signed-off-by: Zach Harel <[email protected]>

KangarooKoala reviewed Aug 22, 2025

View reviewed changes

zachwaffle4 added 2 commits August 22, 2025 20:53

refactor: extract the setting of inverse kinematics matrix in swerved…

5db0a78

…rivekinematics to its own method Signed-off-by: Zach Harel <[email protected]>

refactor: update ChassisAccelerations constructor parameter descripti…

adaeffd

…ons for clarity Signed-off-by: Zach Harel <[email protected]>

zachwaffle4 added 5 commits November 13, 2025 15:44

put the proto files that got eaten back

37eec64

Signed-off-by: Zach Harel <[email protected]>

fixy part 2

edadb2f

Signed-off-by: Zach Harel <[email protected]>

fixy part 3: wpiformat

2deddf2

Signed-off-by: Zach Harel <[email protected]>

fuck poseestimator in particular

d9b2f1e

Signed-off-by: Zach Harel <[email protected]>

fixy part 4: javaformat

6e687b6

Signed-off-by: Zach Harel <[email protected]>

calcmogul requested changes Nov 13, 2025

View reviewed changes

wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp Outdated Show resolved Hide resolved

zachwaffle4 added 5 commits November 13, 2025 17:06

fixy part 5: includes use double quotes instead of angle brackets

43d8bb5

Signed-off-by: Zach Harel <[email protected]>

fixy part 5: includes use double quotes instead of angle brackets

1213b3d

Signed-off-by: Zach Harel <[email protected]>

fixy part 6: run wpiformat after fixy part 5

8a6f519

Signed-off-by: Zach Harel <[email protected]>

Add python bindings

a4fc349

Signed-off-by: Zach Harel <[email protected]>

zachwaffle4 mentioned this pull request Nov 14, 2025

Bazel Build Failure #8379

Open

zachwaffle4 added 4 commits November 14, 2025 23:53

Merge remote-tracking branch 'upstream/2027' into kinematic-acceleration

1a5edee

Merge remote-tracking branch 'upstream/2027' into kinematic-acceleration

9cb2bd8

Add support for wheel accelerations in kinematics and odometry robotp…

84fb2b2

…y binding configurations Signed-off-by: Zach Harel <[email protected]>

fix kinematics protobuf generation

f9ef343

Signed-off-by: Zach Harel <[email protected]>

zachwaffle4 requested a review from calcmogul November 20, 2025 05:02

calcmogul previously approved these changes Nov 20, 2025

View reviewed changes

KangarooKoala reviewed Nov 20, 2025

View reviewed changes

wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java Show resolved Hide resolved

wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp Show resolved Hide resolved

zachwaffle4 added 4 commits November 25, 2025 12:50

rename SwerveModuleAccelerations to SwerveModuleAccelerations

9e5634e

Signed-off-by: Zach Harel <[email protected]>

wpiformat/javaformat

49d95e5

Signed-off-by: Zach Harel <[email protected]>

Refactor kinematics template parameters for consistency across java/c…

d7d5960

…pp/reality Signed-off-by: Zach Harel <[email protected]>

fix unpack method to use mutable angle in SwerveModuleAcceleration

7f81a4b

Signed-off-by: Zach Harel <[email protected]>

zachwaffle4 dismissed calcmogul’s stale review via 7f81a4b November 26, 2025 22:14

zachwaffle4 added 2 commits November 26, 2025 17:19

rename SwerveModuleAccelerations to SwerveModuleAcceleration for cons…

56c89d2

…istency in robotpy Signed-off-by: Zach Harel <[email protected]>

add python SwerveModuleAcceleration class definition with attributes …

5a51d44

…and operators Signed-off-by: Zach Harel <[email protected]>

calcmogul mentioned this pull request Nov 27, 2025

Rename SwerveModuleState to SwerveModuleSpeed or SwerveModuleVelocity #8423

Open

calcmogul approved these changes Nov 27, 2025

View reviewed changes

KangarooKoala reviewed Nov 27, 2025

View reviewed changes

		* @tparam WheelSpeeds Wheel speeds type.
		* @tparam WheelPositions Wheel positions type.

	Rotation2d.proto.pack(msg.getAngle(), value.angle);
	Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);

	msg.getAcceleration(), Rotation2d.proto.unpack(msg.getMutableAngle()));
	msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle()));

[wpimath] Add ChassisAccelerations and drivetrain accelerations classes and add forward and inverse kinematics for accelerations to the interface #8185

Are you sure you want to change the base?

[wpimath] Add ChassisAccelerations and drivetrain accelerations classes and add forward and inverse kinematics for accelerations to the interface #8185

Uh oh!

Conversation

zachwaffle4 commented Aug 22, 2025

Uh oh!

KangarooKoala left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

zachwaffle4 commented Aug 22, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

KangarooKoala left a comment • 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!

Uh oh!

Uh oh!

zachwaffle4 commented Nov 26, 2025

Uh oh!

KangarooKoala left a comment

Choose a reason for hiding this comment

Uh oh!

KangarooKoala Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

KangarooKoala Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

KangarooKoala Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

KangarooKoala Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

KangarooKoala Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

KangarooKoala Nov 27, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

KangarooKoala left a comment •

edited

Loading