mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-04 03:11:43 +00:00
[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>
This commit is contained in:
Zach Harel
@@ -0,0 +1,101 @@
|
||||
// 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/ChassisAccelerations.hpp"
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/angular_acceleration.hpp"
|
||||
|
||||
using namespace wpi::math;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(ChassisAccelerationsTest, DefaultConstructor) {
|
||||
ChassisAccelerations accelerations;
|
||||
|
||||
EXPECT_NEAR(accelerations.ax.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(accelerations.ay.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(accelerations.alpha.value(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, ParameterizedConstructor) {
|
||||
ChassisAccelerations accelerations{1.0_mps_sq, 2.0_mps_sq, 3.0_rad_per_s_sq};
|
||||
|
||||
EXPECT_NEAR(accelerations.ax.value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(accelerations.ay.value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(accelerations.alpha.value(), 3.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, ToRobotRelative) {
|
||||
const auto chassisAccelerations =
|
||||
ChassisAccelerations{1.0_mps_sq, 0.0_mps_sq, 0.5_rad_per_s_sq}
|
||||
.ToRobotRelative(Rotation2d{-90_deg});
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, ToFieldRelative) {
|
||||
const auto chassisAccelerations =
|
||||
ChassisAccelerations{1.0_mps_sq, 0.0_mps_sq, 0.5_rad_per_s_sq}
|
||||
.ToFieldRelative(Rotation2d{45_deg});
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 1.0 / std::sqrt(2.0), kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 1.0 / std::sqrt(2.0), kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, Plus) {
|
||||
const ChassisAccelerations left{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq};
|
||||
const ChassisAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.25_rad_per_s_sq};
|
||||
|
||||
const auto chassisAccelerations = left + right;
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 3.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 1.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, Minus) {
|
||||
const ChassisAccelerations left{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq};
|
||||
const ChassisAccelerations right{2.0_mps_sq, 0.5_mps_sq, 0.25_rad_per_s_sq};
|
||||
|
||||
const auto chassisAccelerations = left - right;
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, UnaryMinus) {
|
||||
const auto chassisAccelerations =
|
||||
-ChassisAccelerations{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq};
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), -0.5, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), -0.75, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, Multiplication) {
|
||||
const auto chassisAccelerations =
|
||||
ChassisAccelerations{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq} * 2.0;
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 1.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(ChassisAccelerationsTest, Division) {
|
||||
const auto chassisAccelerations =
|
||||
ChassisAccelerations{2.0_mps_sq, 1.0_mps_sq, 1.5_rad_per_s_sq} / 2.0;
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 1.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 0.5, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.75, kEpsilon);
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
|
||||
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
@@ -76,3 +77,74 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
|
||||
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.value(), -std::numbers::pi, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForZeros) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const ChassisAccelerations chassisAccelerations;
|
||||
const auto wheelAccelerations =
|
||||
kinematics.ToWheelAccelerations(chassisAccelerations);
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForZeros) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const DifferentialDriveWheelAccelerations wheelAccelerations;
|
||||
const auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForStraightLine) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const ChassisAccelerations chassisAccelerations{3.0_mps_sq, 0_mps_sq,
|
||||
0_rad_per_s_sq};
|
||||
const auto wheelAccelerations =
|
||||
kinematics.ToWheelAccelerations(chassisAccelerations);
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 3, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 3, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForStraightLine) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const DifferentialDriveWheelAccelerations wheelAccelerations{3.0_mps_sq,
|
||||
3.0_mps_sq};
|
||||
const auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 3, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), 0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForRotateInPlace) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const ChassisAccelerations chassisAccelerations{
|
||||
0.0_mps_sq, 0.0_mps_sq,
|
||||
wpi::units::radians_per_second_squared_t{std::numbers::pi}};
|
||||
const auto wheelAccelerations =
|
||||
kinematics.ToWheelAccelerations(chassisAccelerations);
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), -0.381 * std::numbers::pi,
|
||||
kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), +0.381 * std::numbers::pi,
|
||||
kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForRotateInPlace) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const DifferentialDriveWheelAccelerations wheelAccelerations{
|
||||
wpi::units::meters_per_second_squared_t{+0.381 * std::numbers::pi},
|
||||
wpi::units::meters_per_second_squared_t{-0.381 * std::numbers::pi}};
|
||||
const auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(chassisAccelerations.ax.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.ay.value(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisAccelerations.alpha.value(), -std::numbers::pi, kEpsilon);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
// 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/DifferentialDriveWheelAccelerations.hpp"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
|
||||
using namespace wpi::math;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, DefaultConstructor) {
|
||||
DifferentialDriveWheelAccelerations wheelAccelerations;
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, ParameterizedConstructor) {
|
||||
DifferentialDriveWheelAccelerations wheelAccelerations{1.5_mps_sq,
|
||||
2.5_mps_sq};
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 1.5, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 2.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, Plus) {
|
||||
const DifferentialDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq};
|
||||
const DifferentialDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq};
|
||||
|
||||
const auto wheelAccelerations = left + right;
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 3.0, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 2.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, Minus) {
|
||||
const DifferentialDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq};
|
||||
const DifferentialDriveWheelAccelerations right{2.0_mps_sq, 0.5_mps_sq};
|
||||
|
||||
const auto wheelAccelerations = left - right;
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, UnaryMinus) {
|
||||
const auto wheelAccelerations =
|
||||
-DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq};
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), -0.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, Multiplication) {
|
||||
const auto wheelAccelerations =
|
||||
DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} * 2.0;
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 2.0, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 1.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveWheelAccelerationsTest, Division) {
|
||||
const auto wheelAccelerations =
|
||||
DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} / 2.0;
|
||||
|
||||
EXPECT_NEAR(wheelAccelerations.left.value(), 0.5, kEpsilon);
|
||||
EXPECT_NEAR(wheelAccelerations.right.value(), 0.25, kEpsilon);
|
||||
}
|
||||
@@ -228,3 +228,102 @@ TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
|
||||
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
|
||||
EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{5_mps_sq, 0_mps_sq, 0_rad_per_s_sq};
|
||||
auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations);
|
||||
|
||||
EXPECT_NEAR(5.0, wheelAccelerations.frontLeft.value(), 0.1);
|
||||
EXPECT_NEAR(5.0, wheelAccelerations.frontRight.value(), 0.1);
|
||||
EXPECT_NEAR(5.0, wheelAccelerations.rearLeft.value(), 0.1);
|
||||
EXPECT_NEAR(5.0, wheelAccelerations.rearRight.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardAccelerations) {
|
||||
MecanumDriveWheelAccelerations wheelAccelerations{3.536_mps_sq, 3.536_mps_sq,
|
||||
3.536_mps_sq, 3.536_mps_sq};
|
||||
auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(3.536, chassisAccelerations.ax.value(), 0.1);
|
||||
EXPECT_NEAR(0, chassisAccelerations.ay.value(), 0.1);
|
||||
EXPECT_NEAR(0, chassisAccelerations.alpha.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StrafeInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{0_mps_sq, 4_mps_sq, 0_rad_per_s_sq};
|
||||
auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations);
|
||||
|
||||
EXPECT_NEAR(-4.0, wheelAccelerations.frontLeft.value(), 0.1);
|
||||
EXPECT_NEAR(4.0, wheelAccelerations.frontRight.value(), 0.1);
|
||||
EXPECT_NEAR(4.0, wheelAccelerations.rearLeft.value(), 0.1);
|
||||
EXPECT_NEAR(-4.0, wheelAccelerations.rearRight.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StrafeForwardAccelerations) {
|
||||
MecanumDriveWheelAccelerations wheelAccelerations{
|
||||
-2.828427_mps_sq, 2.828427_mps_sq, 2.828427_mps_sq, -2.828427_mps_sq};
|
||||
auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(0, chassisAccelerations.ax.value(), 0.1);
|
||||
EXPECT_NEAR(2.8284, chassisAccelerations.ay.value(), 0.1);
|
||||
EXPECT_NEAR(0, chassisAccelerations.alpha.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, RotationInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{
|
||||
0_mps_sq, 0_mps_sq,
|
||||
wpi::units::radians_per_second_squared_t{2 * std::numbers::pi}};
|
||||
auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations);
|
||||
|
||||
EXPECT_NEAR(-150.79645, wheelAccelerations.frontLeft.value(), 0.1);
|
||||
EXPECT_NEAR(150.79645, wheelAccelerations.frontRight.value(), 0.1);
|
||||
EXPECT_NEAR(-150.79645, wheelAccelerations.rearLeft.value(), 0.1);
|
||||
EXPECT_NEAR(150.79645, wheelAccelerations.rearRight.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, RotationForwardAccelerations) {
|
||||
MecanumDriveWheelAccelerations wheelAccelerations{
|
||||
-150.79645_mps_sq, 150.79645_mps_sq, -150.79645_mps_sq, 150.79645_mps_sq};
|
||||
auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(0, chassisAccelerations.ax.value(), 0.1);
|
||||
EXPECT_NEAR(0, chassisAccelerations.ay.value(), 0.1);
|
||||
EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest,
|
||||
MixedTranslationRotationInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{2_mps_sq, 3_mps_sq, 1_rad_per_s_sq};
|
||||
auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations);
|
||||
|
||||
EXPECT_NEAR(-25.0, wheelAccelerations.frontLeft.value(), 0.1);
|
||||
EXPECT_NEAR(29.0, wheelAccelerations.frontRight.value(), 0.1);
|
||||
EXPECT_NEAR(-19.0, wheelAccelerations.rearLeft.value(), 0.1);
|
||||
EXPECT_NEAR(23.0, wheelAccelerations.rearRight.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest,
|
||||
MixedTranslationRotationForwardAccelerations) {
|
||||
MecanumDriveWheelAccelerations wheelAccelerations{
|
||||
-17.677670_mps_sq, 20.51_mps_sq, -13.44_mps_sq, 16.26_mps_sq};
|
||||
auto chassisAccelerations =
|
||||
kinematics.ToChassisAccelerations(wheelAccelerations);
|
||||
|
||||
EXPECT_NEAR(1.413, chassisAccelerations.ax.value(), 0.1);
|
||||
EXPECT_NEAR(2.122, chassisAccelerations.ay.value(), 0.1);
|
||||
EXPECT_NEAR(0.707, chassisAccelerations.alpha.value(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq};
|
||||
auto wheelAccelerations =
|
||||
kinematics.ToWheelAccelerations(accelerations, m_fl);
|
||||
|
||||
EXPECT_NEAR(0, wheelAccelerations.frontLeft.value(), 0.1);
|
||||
EXPECT_NEAR(24.0, wheelAccelerations.frontRight.value(), 0.1);
|
||||
EXPECT_NEAR(-24.0, wheelAccelerations.rearLeft.value(), 0.1);
|
||||
EXPECT_NEAR(48.0, wheelAccelerations.rearRight.value(), 0.1);
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -310,3 +310,183 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
|
||||
EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
|
||||
EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{
|
||||
0_mps_sq, 0_mps_sq,
|
||||
wpi::units::radians_per_second_squared_t{2 * std::numbers::pi}};
|
||||
wpi::units::radians_per_second_t angularVelocity =
|
||||
2_rad_per_s * std::numbers::pi;
|
||||
auto [flAccel, frAccel, blAccel, brAccel] =
|
||||
m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity);
|
||||
|
||||
// For turn-in-place with angular acceleration of 2π rad/s² and angular
|
||||
// velocity of 2π rad/s, each module has both tangential acceleration (from
|
||||
// angular acceleration) and centripetal acceleration (from angular velocity).
|
||||
// The total acceleration magnitude is approximately 678.4.
|
||||
//
|
||||
// For each swerve module at position (x, y) relative to the robot center:
|
||||
// - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m
|
||||
// - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s
|
||||
//
|
||||
// Two acceleration components:
|
||||
// 1) Tangential acceleration (from angular acceleration α = 2π rad/s²):
|
||||
// a_tangential = α × r = 2π × 16.97 = 106.63 m/s²
|
||||
// Direction: perpendicular to radius vector
|
||||
//
|
||||
// 2) Centripetal acceleration (from angular velocity ω = 2π rad/s):
|
||||
// a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s²
|
||||
// Direction: toward center of rotation
|
||||
//
|
||||
// Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²)
|
||||
// = √(106.63² + 668.7²) = 678.4 m/s²
|
||||
//
|
||||
// For module positions:
|
||||
// FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° →
|
||||
// total angle = -144°
|
||||
// FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° →
|
||||
// total angle = 126°
|
||||
// BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° →
|
||||
// total angle = -54°
|
||||
// BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° →
|
||||
// total angle = 36°
|
||||
//
|
||||
// The acceleration angles are not simply tangential because the centripetal
|
||||
// component from the existing angular velocity dominates and affects the
|
||||
// direction.
|
||||
|
||||
double centerRadius = m_kinematics.GetModules()[0].Norm().value();
|
||||
double tangentialAccel = centerRadius * accelerations.alpha.value(); // α * r
|
||||
double centripetalAccel = centerRadius * angularVelocity.value() *
|
||||
angularVelocity.value(); // ω² * r
|
||||
double totalAccel = std::hypot(tangentialAccel, centripetalAccel);
|
||||
|
||||
std::array<Rotation2d, 4> expectedAngles;
|
||||
for (size_t i = 0; i < 4; i++) {
|
||||
Rotation2d radiusAngle = m_kinematics.GetModules()[i].Angle();
|
||||
|
||||
// Tangential acceleration: perpendicular to radius (90° CCW from radius)
|
||||
Rotation2d tangentialDirection = radiusAngle + Rotation2d{90_deg};
|
||||
double tangentialX = tangentialAccel * tangentialDirection.Cos();
|
||||
double tangentialY = tangentialAccel * tangentialDirection.Sin();
|
||||
|
||||
// Centripetal acceleration: toward center (opposite of radius)
|
||||
Rotation2d centripetalDirection = radiusAngle + Rotation2d{180_deg};
|
||||
double centripetalX = centripetalAccel * centripetalDirection.Cos();
|
||||
double centripetalY = centripetalAccel * centripetalDirection.Sin();
|
||||
|
||||
// Vector sum of tangential and centripetal accelerations
|
||||
double totalX = tangentialX + centripetalX;
|
||||
double totalY = tangentialY + centripetalY;
|
||||
|
||||
expectedAngles[i] = Rotation2d{totalX, totalY};
|
||||
}
|
||||
|
||||
EXPECT_NEAR(totalAccel, flAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(totalAccel, frAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(totalAccel, blAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(totalAccel, brAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[0].Degrees().value(),
|
||||
flAccel.angle.Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[1].Degrees().value(),
|
||||
frAccel.angle.Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[2].Degrees().value(),
|
||||
blAccel.angle.Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[3].Degrees().value(),
|
||||
brAccel.angle.Degrees().value(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) {
|
||||
SwerveModuleAcceleration flAccel{106.629_mps_sq, 135_deg};
|
||||
SwerveModuleAcceleration frAccel{106.629_mps_sq, 45_deg};
|
||||
SwerveModuleAcceleration blAccel{106.629_mps_sq, -135_deg};
|
||||
SwerveModuleAcceleration brAccel{106.629_mps_sq, -45_deg};
|
||||
|
||||
auto chassisAccelerations =
|
||||
m_kinematics.ToChassisAccelerations(flAccel, frAccel, blAccel, brAccel);
|
||||
|
||||
EXPECT_NEAR(0.0, chassisAccelerations.ax.value(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, chassisAccelerations.ay.value(), kEpsilon);
|
||||
EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(),
|
||||
kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) {
|
||||
ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq};
|
||||
// For this test, assume an angular velocity of 1 rad/s
|
||||
wpi::units::radians_per_second_t angularVelocity = 1.0_rad_per_s;
|
||||
auto [flAccel, frAccel, blAccel, brAccel] =
|
||||
m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity,
|
||||
m_fl);
|
||||
|
||||
// When rotating about the front-left module position with both angular
|
||||
// acceleration (1 rad/s²) and angular velocity (1 rad/s), each module
|
||||
// experiences both tangential and centripetal accelerations that combine
|
||||
// vectorially.
|
||||
//
|
||||
// Center of rotation: FL module at (12, 12) inches
|
||||
// Module positions relative to center of rotation:
|
||||
// - FL: (0, 0) - at center of rotation
|
||||
// - FR: (0, -24) - 24 m south of center
|
||||
// - BL: (-24, 0) - 24 m west of center
|
||||
// - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest
|
||||
//
|
||||
// For each module at distance r from center of rotation:
|
||||
// 1) Tangential acceleration: a_t = α × r = 1 × r
|
||||
// Direction: perpendicular to radius vector (90° CCW from radius)
|
||||
//
|
||||
// 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r
|
||||
// Direction: toward center of rotation
|
||||
|
||||
std::array<double, 4> expectedAccelerations;
|
||||
std::array<Rotation2d, 4> expectedAngles;
|
||||
|
||||
for (size_t i = 0; i < 4; i++) {
|
||||
Translation2d relativePos = m_kinematics.GetModules()[i] - m_fl;
|
||||
double r = relativePos.Norm().value();
|
||||
|
||||
if (r < 1e-9) {
|
||||
expectedAccelerations[i] = 0.0; // No acceleration at center of rotation
|
||||
expectedAngles[i] =
|
||||
Rotation2d{}; // Angle is undefined at center of rotation
|
||||
} else {
|
||||
double tangentialAccel =
|
||||
r * accelerations.alpha.value(); // α * r = 1 * r
|
||||
double centripetalAccel = r * angularVelocity.value() *
|
||||
angularVelocity.value(); // ω² * r = 1 * r
|
||||
expectedAccelerations[i] = std::hypot(tangentialAccel, centripetalAccel);
|
||||
|
||||
Rotation2d radiusAngle{(relativePos.X().value()),
|
||||
(relativePos.Y().value())};
|
||||
|
||||
// Tangential acceleration: perpendicular to radius (90° CCW from radius)
|
||||
Rotation2d tangentialDirection = radiusAngle + Rotation2d{90_deg};
|
||||
double tangentialX = tangentialDirection.Cos() * r; // α * r = 1 * r
|
||||
double tangentialY = tangentialDirection.Sin() * r;
|
||||
|
||||
// Centripetal acceleration: toward center (opposite of radius)
|
||||
Rotation2d centripetalDirection = radiusAngle + Rotation2d{180_deg};
|
||||
double centripetalX = centripetalDirection.Cos() * r; // ω² * r = 1 * r
|
||||
double centripetalY = centripetalDirection.Sin() * r;
|
||||
|
||||
// Vector sum of tangential and centripetal accelerations
|
||||
double totalX = tangentialX + centripetalX;
|
||||
double totalY = tangentialY + centripetalY;
|
||||
|
||||
expectedAngles[i] = Rotation2d{totalX, totalY};
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_NEAR(expectedAccelerations[0], flAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAccelerations[1], frAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAccelerations[2], blAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAccelerations[3], brAccel.acceleration.value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[0].Degrees().value(),
|
||||
flAccel.angle.Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[1].Degrees().value(),
|
||||
frAccel.angle.Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[2].Degrees().value(),
|
||||
blAccel.angle.Degrees().value(), kEpsilon);
|
||||
EXPECT_NEAR(expectedAngles[3].Degrees().value(),
|
||||
brAccel.angle.Degrees().value(), kEpsilon);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,40 @@
|
||||
// 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 <numbers>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
|
||||
using namespace wpi::math;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(SwerveModuleAccelerationsTest, DefaultConstructor) {
|
||||
SwerveModuleAcceleration moduleAccelerations;
|
||||
|
||||
EXPECT_NEAR(moduleAccelerations.acceleration.value(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) {
|
||||
SwerveModuleAcceleration moduleAccelerations{2.5_mps_sq, Rotation2d{1.5_rad}};
|
||||
|
||||
EXPECT_NEAR(moduleAccelerations.acceleration.value(), 2.5, kEpsilon);
|
||||
EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 1.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(SwerveModuleAccelerationsTest, Equals) {
|
||||
SwerveModuleAcceleration moduleAccelerations1{2.0_mps_sq,
|
||||
Rotation2d{1.5_rad}};
|
||||
SwerveModuleAcceleration moduleAccelerations2{2.0_mps_sq,
|
||||
Rotation2d{1.5_rad}};
|
||||
SwerveModuleAcceleration moduleAccelerations3{2.1_mps_sq,
|
||||
Rotation2d{1.5_rad}};
|
||||
|
||||
EXPECT_EQ(moduleAccelerations1, moduleAccelerations2);
|
||||
EXPECT_NE(moduleAccelerations1, moduleAccelerations3);
|
||||
}
|
||||
Reference in New Issue
Block a user