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

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++. --------- Signed-off-by: Zach Harel <zach@zharel.me> Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Co-authored-by: Tyler Veness <calcmogul@gmail.com>
2026-07-05 03:21:42 +00:00 · 2025-12-08 19:25:07 -05:00
parent 44cf645632
commit 936be71a7d
101 changed files with 7041 additions and 523 deletions
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp
@@ -0,0 +1,94 @@
+// 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.
+
+#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp"
+
+#include <gtest/gtest.h>
+
+#include "wpi/units/acceleration.hpp"
+
+using namespace wpi::math;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(MecanumDriveWheelAccelerationsTest, DefaultConstructor) {
+  MecanumDriveWheelAccelerations wheelAccelerations;
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.0, kEpsilon);
+}
+
+TEST(MecanumDriveWheelAccelerationsTest, ParameterizedConstructor) {
+  MecanumDriveWheelAccelerations wheelAccelerations{1.0_mps_sq, 2.0_mps_sq,
+                                                    3.0_mps_sq, 4.0_mps_sq};
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 3.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 4.0, kEpsilon);
+}
+
+TEST(MecanumDriveWheelAccelerationsTest, Plus) {
+  const MecanumDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq, 2.0_mps_sq,
+                                            1.5_mps_sq};
+  const MecanumDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.5_mps_sq,
+                                             1.0_mps_sq};
+
+  const auto wheelAccelerations = left + right;
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 3.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 2.5, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 2.5, kEpsilon);
+}
+
+TEST(MecanumDriveWheelAccelerationsTest, Minus) {
+  const MecanumDriveWheelAccelerations left{5.0_mps_sq, 4.0_mps_sq, 6.0_mps_sq,
+                                            2.5_mps_sq};
+  const MecanumDriveWheelAccelerations right{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq,
+                                             0.5_mps_sq};
+
+  const auto wheelAccelerations = left - right;
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 3.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 2.0, kEpsilon);
+}
+
+TEST(MecanumDriveWheelAccelerationsTest, UnaryMinus) {
+  const auto wheelAccelerations = -MecanumDriveWheelAccelerations{
+      1.0_mps_sq, -2.0_mps_sq, 3.0_mps_sq, -4.0_mps_sq};
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), -1.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), -3.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 4.0, kEpsilon);
+}
+
+TEST(MecanumDriveWheelAccelerationsTest, Multiplication) {
+  const auto wheelAccelerations =
+      MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 3.0_mps_sq,
+                                     3.5_mps_sq} *
+      2.0;
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 6.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 7.0, kEpsilon);
+}
+
+TEST(MecanumDriveWheelAccelerationsTest, Division) {
+  const auto wheelAccelerations =
+      MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 1.5_mps_sq,
+                                     1.0_mps_sq} /
+      2.0;
+
+  EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.frontRight.value(), 1.25, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.75, kEpsilon);
+  EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.5, kEpsilon);
+}